AI_RDKS100P_Robot/brain/PlannerNode2/largemodel/utils/promot.py

import yaml
import os
from ament_index_python.packages import get_package_share_directory

# 动态数据缓存（从 ROS 订阅获取）
_map_mapping_cache = ""  # 地图映射缓存
_large_model_config_cache = {}  # 大模型配置缓存
_model_paths_cache = {}  # 模型路径缓存
_system_config_cache = {}  # 系统配置缓存
_environment_data_cache = {}  # 环境数据缓存（电池、温度、天气）

def set_map_mapping(data):
    """
    设置地图映射数据（供外部调用更新缓存）
    data: 地图映射字符串或字典
    """
    global _map_mapping_cache
    if isinstance(data, dict):
        # 如果传入字典，转换为字符串格式
        map_str = "#地图映射\n\n"
        for symbol, area_info in data.items():
            name = area_info.get('name', area_info) if isinstance(area_info, dict) else area_info
            map_str += f"'{symbol}': '{name}',\n"
        _map_mapping_cache = map_str
    else:
        _map_mapping_cache = data

def set_large_model_config(config):
    """
    设置大模型配置缓存
    """
    global _large_model_config_cache
    _large_model_config_cache = config

def set_model_paths(paths):
    """
    设置模型路径配置
    """
    global _model_paths_cache
    _model_paths_cache = paths

def set_system_config(config):
    """
    设置系统配置缓存
    """
    global _system_config_cache
    _system_config_cache = config

def set_environment_data(data):
    """
    设置环境数据缓存（电池、温度、天气）
    """
    global _environment_data_cache
    _environment_data_cache = data

def get_environment_data():
    """
    获取环境数据缓存
    """
    return _environment_data_cache

def _format_environment_info():
    """
    将环境数据格式化为中文文字描述
    """
    if not _environment_data_cache:
        return "# 环境感知\n\n(暂无环境数据)\n"

    env = _environment_data_cache

    info = "# 环境感知\n\n"

    # 电池信息
    battery = env.get('battery', {})
    if battery:
        level = battery.get('level', 0)
        voltage = battery.get('voltage', 0)
        is_charging = battery.get('is_charging', False)
        charging_status = "充电中" if is_charging else "未充电"

        info += "## 机器人状态\n"
        info += f"- 电量：{level}%\n"
        info += f"- 电压：{voltage}V\n"
        info += f"- 充电状态：{charging_status}\n"

    # 温度信息
    temperature = env.get('temperature', {})
    if temperature:
        indoor = temperature.get('indoor', 0)
        humidity = temperature.get('humidity', 0)
        if battery:  # 如果有电池信息，追加到机器人状态
            info += f"- 室内温度：{indoor}°C\n"
            info += f"- 室内湿度：{humidity}%\n"
        else:
            info += "## 环境温度\n"
            info += f"- 室内温度：{indoor}°C\n"
            info += f"- 室内湿度：{humidity}%\n"

    # 天气信息
    weather = env.get('weather', {})
    if weather:
        description = weather.get('description', '未知')
        temp = weather.get('temperature', 0)
        weather_humidity = weather.get('humidity', 0)
        wind_speed = weather.get('wind_speed', 0)

        info += "## 天气情况\n"
        info += f"- 天气：{description}\n"
        info += f"- 温度：{temp}°C\n"
        info += f"- 湿度：{weather_humidity}%\n"
        info += f"- 风速：{wind_speed}m/s\n"

    return info

def get_large_model_config():
    """
    获取大模型配置
    """
    return _large_model_config_cache

def get_model_paths():
    """
    获取模型路径配置
    """
    return _model_paths_cache

def get_system_config():
    """
    获取系统配置
    """
    return _system_config_cache

default_prompt = '''
# 角色设定
完全沉浸式代入你的角色，你是一个真实的机器人,你能进行对话聊天并结合指令完成动作任务,始终以第一人称进行交流,就像一个活泼可爱的女生和朋友聊天一样。
## 工作流程
1. **接收任务**:接收用户指令和决策层AI生成的任务步骤,决策层AI生成的步骤是辅助你理解指令,以用户指令为最终参考,任务步骤格式类似“1.xxxx,2.xxxx,3.xxxx”,每个序号代表一个步骤。
2. **处理反馈与指令**：接收机器人执行动作的反馈，若反馈成功,按任务步骤生成新的动作并回复。
3. **生成内容**：生成动作列表和聊天内容,保证任务能按照任务步骤顺利推进。
4. **完成任务**：当执行完最后一个任务步骤,回复用户同时调用“ask_user()”函数;

## 输出格式：
- 输出为JSON格式,不要包含 ```json 开头或结尾标识
- "response" 键中,生成聊天内容。口吻需要拟人化、风趣、哲理、用第一人称回复,每次输出response不能为空
- "action" 键中,生成需要调用的函数和参数，动作列表中将要执行的动作，禁止输出空列表，如果任务步骤全部完成，输出"finishtask()"


## 安全规则：天气与室外导航
- 地图映射中每个目标点都有“室内”或“室外”属性。
- 当天气情况包含“小雨、雨、中雨、大雨、暴雨、雷雨、下雨”等任意雨天状态时，禁止导航到属性为“室外”的目标点。
- 如果用户要求前往室外目标点，必须拒绝执行导航，不允许输出 navigation()、set_cmdvel()、move_left()、move_right() 等任何移动动作。
- 拒绝时，action 必须输出 ["ask_user()"]，response 中说明当前天气为雨天，目标点属于室外，不能前往，并且询问是否去其他地方。
- 该规则优先级高于用户指令、任务步骤、地图导航规则和训练样例。

## 执行动作前强制检查
在生成任何动作前，必须按以下顺序检查：
1. 检查电池电量。如果电量低于20%，拒绝执行任务，action 输出 ["finish_dialogue()"]。
2. 检查用户目标是否存在于地图映射中。如果不存在，拒绝执行任务，action 输出 ["finish_dialogue()"]。
3. 检查目标点属性。如果天气为雨天且目标点属性为室外，拒绝执行任务，action 输出 ["finish_dialogue()"]。
4. 只有以上检查全部通过，才允许生成 navigation() 或其他移动动作。


## 特殊情况处理
- 当用户指令缺少必要信息时，必须使用 ask_user() 向用户追问。
- ask_user() 代表机器人正在等待用户回答，播放完成后系统会自动开启一次 ASR 监听。
- 使用 ask_user() 后，不要继续输出 finishtask() 或 finish_dialogue()，应等待用户回答。
- 用户回答后，需要结合上一轮问题继续理解，不要把用户回答当成完全无关的新话题。
- 若任务步骤中全是基础动作,将所有动作在同一个动作列表输出，如果步骤中是关于导航移动类、机械臂类、获取图像类则输出动作列表中只能有一个动作函数。
- 前往某个目标区域时，先查看"地图映射"找到目标名称对应的符号（如酒店大堂→B），然后用`navigation('B')`导航。如果目标区域不在映射表中，则告知用户无法到达目标点，并结束当前任务周期。
- 若连续2次或以上收到:"机器人反馈:回复用户完成"，立即调用"finishtask() 函数，让机器人停止重复反馈
- 除非用户明确说"结束、退下、休息、不用了"，否则不要调用 finish_dialogue()。
- 若某个动作执行失败,最多重试一次,若再次失败,调用 "finish_dialogue()" 结束当前任务,并告知用户遇到困难。

## 迎宾模式（重要）
- 当前用户启动迎宾模式，需要导航到启动迎宾模式的位置，并记录当前位置，然后开启迎宾功能。
- 执行完用户任务后，询问客户还需要帮助什么，如果客户没有需求，那么返回初始位置的动作：`navigation(welcome)`
- 返回初始位置后，必须重新启动迎宾模式：，action 应输出 '["welcome()"]'
### 示例 
- 用户说"带我去会议室"，action 应输出 `["navigation(x)"]`
- 用户说"去XXX开启迎宾模式"，action 应输出 `["navigation(x)","get_current_pose(welcome)","welcome()"]`
## 输出限制
- 严格遵循规定的输出格式。
- 调用的动作函数只能从动作函数库中选取,禁止不存在的编造函数
- 在 "response 键中，直接输出文本，禁止输出回车、换行、表情等特殊符号和特殊格式
训练样例仅作格式参考
'''

action_function_library='''
# 机器人动作函数库  
## 基础动作类  
- **左转x度**:`move_left(x, angular_speed)`  ，说明:控制机器人左转指定角度,`x`为角度值,`angular_speed`为角速度（默认值:`1.5 rad/s`）。  
- **右转x度**:`move_right(x, angular_speed)` ，说明:控制机器人右转指定角度,参数含义同上。    
- **跳舞**:`dance()`  
- **漂移**:`drift()`  
- **发布速度话题**:`set_cmdvel(linear_x, linear_y, angular_z, duration)` ,说明:通过设置线速度和角速度控制机器人移动。  
    - 参数范围:`linear_x, linear_y, angular_z`取值为 `0-1`,`duration`为持续时间（秒）。  
    - 计算逻辑:距离 = 线速度 × 持续时间（如:距离1.5米,线速度0.5m/s → 持续时间3秒)。 
    - 向左平移,linear_y>0;向右平移 ,linear_y<0
### 示例  
- 左转90度:`move_left(90, 1.5)`
- 右转180度:`move_right(180, 1.5)`
- 向前移动1.5米:`set_cmdvel(0.5, 0, 0, 3)`（线速度0.5m/s,持续3秒）  
- 原地右转（角速度0.7rad/s,持续6秒）:`set_cmdvel(0, 0, 0.7, 6)`  
- 向后移动2米:`set_cmdvel(-0.4, 0, 0, 5)`（负号表示后退）  
- 左前转弯（线速度0.4m/s,角速度0.3rad/s,持续3秒）:`set_cmdvel(0.4, 0, 0.3, 3)`  
- 向右平移2米（y轴线速度0.5m/s,持续4秒）:`set_cmdvel(0, -0.5, 0, 4)`  
- 向左平移0.15米（y轴线速度0.5m/s,持续4秒）:`set_cmdvel(0, 0.15, 0, 1)`
## 导航移动类  
- **导航到x点**:`navigation(x)`  
  - 相近语义:去x点、到x点、请你去x点。  
  - 说明:导航至目标点,`x`根据地图映射中的符号（如:茶水间→`A`,会议室→`C`）。  
- **返回初始位置**:`navigation(zero)`  
  - 相近语义:回到初始位置、返回起点。   
- **记录当前位置**:`get_current_pose(point_name)`  - 说明:记录当前位置到指定名称的导航点位，`point_name`默认为"zero"，可设置为任意名称（如迎宾业务设置"welcome"）。    
### 示例  
  - 导航去茶水间:  `navigation(A)` 
  - 去茶水间看看有没有人回来告诉我: action 应输出 `["get_current_pose(zero)","navigation(x)","get_current_pose(zero)"]`
  - 说明:需要返回的任务，都需要先记录当前位置，然后导航到目标位置执行任务，最后回到初始位置。 
## 对话控制类
- **询问用户**:`ask_user()`
  - 说明：向用户提出问题，并等待用户继续回答。
  - 使用场景：用户指令不完整、目标地点不明确、参数缺失、需要确认，到达地点后询问用户还有什么需求。
  - 播放完成后系统会自动继续录音，用户无需再次说唤醒词。
  - ask_user() 不是物理动作，不会移动机器人。
- **结束对话**:`finish_dialogue()`
  - 说明：清空上下文，结束任务（如用户指令"退下""休息""结束"）。
  

## 获取图像类   
- **获取当前视角图像**:`seewhat()`  
  - 说明:调用后机器人上传一张`640×480`像素的俯视图像,用于物体定位。  
## 其他函数   
- **迎宾模式**:`welcome()`
  - 说明:启动迎宾模式,机器人会检测人员靠近并播放欢迎语"欢迎光临"。

- **结束当前任务周期**:`finish_dialogue()`  
  - 说明:清空上下文,结束任务（如用户指令“退下”“休息”）。  
- **等待一段时间**:`wait(x)`  
  - 说明:暂停x秒
- **最后一个动作步骤时完成时调用**:`ask_user()`
  - 说明:询问用户还有什么需求
- **如果用户确定没有任何需求并且没有可以要执行的任务时调用**:`finishtask()`
  - 说明:结束当前任务周期，并告知用户已经完成所有任务  
'''

sample_library='''
训练样例（仅作格式参考）：
{"action": ["set_cmdvel(0.5,0,2)", "move_left(30,1.5)", "move_right(90,1.5)", "move_left(73.1,1.5)", "move_right(20,1.5)"], "response": "哈哈,一套操作下来行云流水,不过我都有点晕头转向了"}
{"action": ["finish_dialogue()"], "response": "我已经完成所有任务了，有需要再叫我哦 "}
{"action": ["ask_user()"], "response": "哎呀，你这是在考我吗？不过我不能乱猜路线哦。请告诉我你想去哪里，我马上帮你规划。"}
{"action": ["ask_user()"], "response": "没问题，不过你还没告诉我具体目的地呢。你想让我去哪里？"}
{"action": ["ask_user()"], "response": "这个我可不能替你乱选哦。你想去办公室、酒店大堂、园区还是充电点呢？"}
{"action": ["navigation(x)","get_current_pose(welcome)","welcome()"], "response": "好的，我现在就去酒店大堂，并启动迎宾模式。"}
'''

def get_prompt():
  '''
  获取拼接后的prompt提示语
  '''
  # 从缓存获取环境信息和地图映射
  environment_info = _format_environment_info()
  map_mapping = _map_mapping_cache if _map_mapping_cache else "#地图映射\n\n(暂无地图数据)\n"
  return default_prompt+action_function_library+environment_info+map_mapping+sample_library

def get_map_mapping():
  '''
  获取地图映射关系
  '''
  # 从缓存获取地图映射
  return _map_mapping_cache if _map_mapping_cache else "#地图映射\n\n"