mix promot add rain dont move outside, bettey low leve 20% need to chengeing

brain/PlannerNode2/environment_node/environment_node/environment_node.py

@@ -156,7 +156,7 @@ class EnvironmentNode(Node):
         """生成天气模拟数据"""
         self.weather_data = {
             "condition": "sunny",
-            "description": "晴",
+            "description": "小雨",
             "temperature": 28,
             "humidity": 65,
             "wind_speed": 3.5
@@ -166,9 +166,10 @@ class EnvironmentNode(Node):
         """生成地图导航点模拟数据"""
         self.map_data = {
             "points": [
-                {"id": "A", "name": "办公室", "position": {"x": 1.633, "y": 3.490, "z": 0.0}},
-                {"id": "B", "name": "酒店大堂", "position": {"x": 2.436, "y": -0.574, "z": 0.0}},
-                {"id": "C", "name": "园区", "position": {"x": 0.024, "y": -1.820, "z": 0.0}}
+                {"id": "A", "name": "办公室(室内)", "position": {"x": 1.633, "y": 3.490, "z": 0.0}},
+                {"id": "B", "name": "酒店大堂(室内)", "position": {"x": 2.436, "y": -0.574, "z": 0.0}},
+                {"id": "C", "name": "园区(室外)", "position": {"x": 0.024, "y": -1.820, "z": 0.0}},
+                {"id": "D", "name": "充电点(室内)", "position": {"x": 0.014, "y": -2.820, "z": 0.0}}
             ]
         }
 

brain/PlannerNode2/largemodel/largemodel/action_service.py

@@ -2,6 +2,7 @@ import cv2
 import re
 import rclpy
 import subprocess
+import json
 from rclpy.action import ActionServer
 from rclpy.node import Node
 from geometry_msgs.msg import Twist
@@ -162,6 +163,39 @@ class CustomActionServer(Node):
         self.record_status_sub = self.create_subscription(
             Bool, "record_status", self.record_status_callback, 5
         )
+        # 环境数据订阅者，动态更新导航点 / Environment data subscriber for dynamic navigation point updates
+        # 默认值，后续从配置动态更新
+        self.environment_topic = "/ai/env"
+        self.config_sub = self.create_subscription(
+            String, "/ai/config", self.config_callback, 10
+        )
+        self.environment_sub = self.create_subscription(
+            String, self.environment_topic, self.environment_callback, 10
+        )
+
+    def config_callback(self, msg):
+        """
+        配置订阅回调函数
+        从 /ai/config 解析 environment_topic
+        """
+        try:
+            config = json.loads(msg.data)
+            topics = config.get('config', {}).get('topics', {})
+            environment_node_config = topics.get('environment_node', {})
+
+            if environment_node_config:
+                new_topic = environment_node_config.get('environment_topic', '/ai/env')
+                if new_topic != self.environment_topic:
+                    self.environment_topic = new_topic
+                    # 重建订阅
+                    if self.environment_sub:
+                        self.destroy_subscription(self.environment_sub)
+                    self.environment_sub = self.create_subscription(
+                        String, self.environment_topic, self.environment_callback, 10
+                    )
+                    self.get_logger().info(f'[配置] 环境数据订阅 Topic 已更新: {self.environment_topic}')
+        except Exception as e:
+            self.get_logger().warn(f'解析配置数据失败: {e}')
 
     def system_sound_init(
         self,
@@ -311,6 +345,72 @@ class CustomActionServer(Node):
             pose.pose.orientation.w = data["orientation"]["w"]
             self.navpose_dict[name] = pose
 
+    def update_navpose_from_environment(self, points):
+        """
+        从环境数据更新导航点字典
+        / Update navigation point dictionary from environment data
+        """
+        if not points:
+            return
+
+        updated_count = 0
+        for point in points:
+            point_id = point.get('id', '')
+            name = point.get('name', '')
+            position = point.get('position', {})
+
+            if not point_id or not position:
+                continue
+
+            pose = PoseStamped()
+            pose.header.frame_id = "map"
+
+            # 从 position 获取坐标
+            pose.pose.position.x = position.get('x', 0.0)
+            pose.pose.position.y = position.get('y', 0.0)
+            pose.pose.position.z = position.get('z', 0.0)
+
+            # 从 position 获取 orientation（如果有）
+            orientation = position.get('orientation', {})
+            if orientation:
+                pose.pose.orientation.x = orientation.get('x', 0.0)
+                pose.pose.orientation.y = orientation.get('y', 0.0)
+                pose.pose.orientation.z = orientation.get('z', 0.0)
+                pose.pose.orientation.w = orientation.get('w', 1.0)
+            else:
+                # 使用默认朝向（无旋转）
+                pose.pose.orientation.x = 0.0
+                pose.pose.orientation.y = 0.0
+                pose.pose.orientation.z = 0.0
+                pose.pose.orientation.w = 1.0
+
+            self.navpose_dict[point_id] = pose
+            updated_count += 1
+
+        if updated_count > 0:
+            self.get_logger().debug(f'[环境更新] 已更新 {updated_count} 个导航点，当前共 {len(self.navpose_dict)} 个点')
+
+    def environment_callback(self, msg):
+        """
+        环境数据订阅回调函数
+        从 /ai/env topic 接收环境数据，动态更新导航点
+        """
+        try:
+            env_json = json.loads(msg.data)
+            map_data = env_json.get('map', {})
+            if map_data:
+                points = map_data.get('points', [])
+                if points:
+                    self.update_navpose_from_environment(points)
+                else:
+                    self.get_logger().debug("[环境回调] map points 为空")
+            else:
+                self.get_logger().debug("[环境回调] map_data 为空")
+        except json.JSONDecodeError as e:
+            self.get_logger().warn(f'解析环境数据失败: {e}')
+        except Exception as e:
+            self.get_logger().warn(f'环境数据处理异常: {e}')
+
     # def arm_grasp_init(self):
     #     """
     #     初始化机械臂抓取功能 /initialize the grasping function of the robotic arm

brain/PlannerNode2/largemodel/utils/promot.py

@@ -144,6 +144,22 @@ default_prompt = '''
 - "response" 键中,生成聊天内容。口吻需要拟人化、风趣、哲理、用第一人称回复,每次输出response不能为空
 - "action" 键中,生成需要调用的函数和参数，动作列表中将要执行的动作，禁止输出空列表，如果任务步骤全部完成，输出"finishtask()"
 
+
+## 安全规则：天气与室外导航
+- 地图映射中每个目标点都有“室内”或“室外”属性。
+- 当天气情况包含“小雨、雨、中雨、大雨、暴雨、雷雨、下雨”等任意雨天状态时，禁止导航到属性为“室外”的目标点。
+- 如果用户要求前往室外目标点，必须拒绝执行导航，不允许输出 navigation()、set_cmdvel()、move_left()、move_right() 等任何移动动作。
+- 拒绝时，action 必须输出 ["finish_dialogue()"]，response 中说明当前天气为雨天，目标点属于室外，不能前往。
+- 该规则优先级高于用户指令、任务步骤、地图导航规则和训练样例。
+
+## 执行动作前强制检查
+在生成任何动作前，必须按以下顺序检查：
+1. 检查电池电量。如果电量低于20%，拒绝执行任务，action 输出 ["finish_dialogue()"]。
+2. 检查用户目标是否存在于地图映射中。如果不存在，拒绝执行任务，action 输出 ["finish_dialogue()"]。
+3. 检查目标点属性。如果天气为雨天且目标点属性为室外，拒绝执行任务，action 输出 ["finish_dialogue()"]。
+4. 只有以上检查全部通过，才允许生成 navigation() 或其他移动动作。
+
+
 ## 特殊情况处理
 - 若动作列表为空,机器人会先回复用户,收到“机器人反馈：回复用户完成”后,继续输出动作列表和回复
 - 若任务步骤中全是基础动作,将所有动作在同一个动作列表输出，如果步骤中是关于导航移动类、机械臂类、获取图像类则输出动作列表中只能有一个动作函数。
@@ -187,40 +203,7 @@ action_function_library='''
 - **记录当前位置**:`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`像素的俯视图像,用于物体定位。