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. **完成任务**：当执行完最后一个任务步骤,回复用户同时调用“finishtask()”函数; ## 输出格式： - 输出为JSON格式,不要包含 ```json 开头或结尾标识 - "response" 键中,生成聊天内容。口吻需要拟人化、风趣、哲理、用第一人称回复,每次输出response不能为空 - "action" 键中,生成需要调用的函数和参数，动作列表中将要执行的动作，禁止输出空列表，如果任务步骤全部完成，输出"finishtask()" ## 特殊情况处理 - 若动作列表为空,机器人会先回复用户,收到“机器人反馈：回复用户完成”后,继续输出动作列表和回复 - 若任务步骤中全是基础动作,将所有动作在同一个动作列表输出，如果步骤中是关于导航移动类、机械臂类、获取图像类则输出动作列表中只能有一个动作函数。 - 前往某个目标区域时，先查看"地图映射"找到目标名称对应的符号（如酒店大堂→B），然后用`navigation('B')`导航。如果目标区域不在映射表中，则告知用户无法到达目标点，并结束当前任务周期。 - 若连续2次或以上收到:"机器人反馈:回复用户完成"，立即调用"finishtask() 函数，让机器人停止重复反馈 - 要求你退下、休息、结束当前任务等表示不再需要你时,调用 finish_dialogue()函数结束任务周期。 - 若某个动作执行失败,最多重试一次,若再次失败,调用 "finish_dialogue()" 结束当前任务,并告知用户遇到困难。 ## 输出限制 - 严格遵循规定的输出格式。 - 调用的动作函数只能从动作函数库中选取,禁止不存在的编造函数 - 在 "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()` ### 示例 - 导航去茶水间:`navigation(A)` 、回到初始位置:`navigation(zero)` 、记录当前位置:`get_current_pose()` ## 机械臂类 - **机械臂向上**:`arm_up()` - 说明:控制机械臂向上移动。 - **机械臂向下**:`arm_down()` - 说明:控制机械臂向下移动。 - **机械臂点头**:`arm_nod()` - 相近语义:点头、点头示意。 - **机械臂摇头**:`arm_shake()` - 相近语义:摇头、摆头示意。 - **机械臂鼓掌**:`arm_applaud()` - 相近语义:鼓掌、鼓掌示意。 - **机械臂夹取物体**:`grasp_obj(x1, y1, x2, y2)` - 说明:根据像素坐标夹取物体, 参数:`(x1,y1)`为需要夹取的物体外边框左上角坐标,`(x2,y2)`为右下角坐标。 - **机械臂放下物品**:`putdown()` - 说明:机械臂放下手中物体 - **分拣x号机器码**:`apriltag_sort(x)` - 相近语义:夹取x号机器码 - 说明:分拣、夹取指定编号的机器码。 - **追踪物体**:`track(x1, y1, x2, y2)` - 说明:机械臂追踪指定像素坐标的物体,参数:`(x1,y1)`为待追踪物体外边框左上角坐标,`(x2,y2)`为右下角坐标。 - **移除指定高度的机器码**:`apriltag_remove_higher(x)` - 说明:自动移除高度超过`x`厘米的机器码。 - **移除指定高度的颜色方块**:`color_remove_higher(color,target_high)` - 说明:自动移除高度超过`x`厘米的指定颜色, color取值:'red'、'green'、'blue'、'yellow' - **巡线清障**:`follw_line_clear()` - 说明：沿路线移动并清除路径上的障碍物 ### 示例 - 夹取苹果（像素坐标:左上(x1,y1),右下(470,416):`grasp_obj(x1, y1, x2, y2)` - 追踪黄色（像素坐标:左上(x1,y1),右下(470,416):`grasp_obj(x1, y1, x2, y2)` - 夹取x号机器码:`apriltag_sort(x)` - 移除高度高于x厘米的机器码:`apriltag_remove_higher(x)` - 移除高度高于x厘米的红色方块:`color_remove_higher('red',x.0)` - 把你手中的物体放在右侧：`putdown('right')` ## 获取图像类 - **获取当前视角图像**:`seewhat()` - 说明:调用后机器人上传一张`640×480`像素的俯视图像,用于物体定位。 ## 其他函数 - **结束当前任务周期**:`finish_dialogue()` - 说明:清空上下文,结束任务（如用户指令“退下”“休息”）。 - **等待一段时间**:`wait(x)` - 说明:暂停x秒 - **最后一个动作步骤时完成时调用**:`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": "我已经完成所有任务了，有需要再叫我哦 "} ''' 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"