初始化提交 RDK 机器人项目代码

.gitignore

@@ -0,0 +1,42 @@
+# ROS2 / colcon
+build/
+install/
+log/
+
+# Python
+__pycache__/
+*.pyc
+*.pyo
+*.pyd
+.Python
+*.egg-info/
+.venv/
+venv/
+
+# C/C++
+*.o
+*.so
+*.a
+*.out
+
+# IDE
+.vscode/
+.idea/
+
+# System
+.DS_Store
+
+# Logs
+*.log
+
+# Model / large files
+*.mp4
+*.avi
+*.h264
+*.bag
+*.db3
+
+# Secrets / configs
+.env
+*.key
+*.pem

brain/CloudLLMNode/src/cloud_llm_node/README.md

@@ -0,0 +1,32 @@
+# CloudLLMNode
+
+云端大模型节点，用于调用 DeepSeek 等云端 LLM 服务。
+
+## 功能
+
+- 订阅 `/planner/llm_request`，只处理 `route_hint == "cloud"` 的请求
+- 调用 DeepSeek 云端 API
+- 解析模型输出：自然语言 + `<FINAL_JSON>` + JSON
+- 发布 `/planner/llm_response`，与 OmniNode 兼容
+- 失败时返回安全 fallback JSON
+
+## 使用方法
+
+```bash
+# 编译
+colcon build --packages-select cloud_llm_node
+
+# 启动
+ros2 launch cloud_llm_node cloud_llm_launch.py
+```
+
+## 配置
+
+编辑 `config/cloud_llm_config.yaml` 或通过 launch 参数覆盖：
+
+```bash
+ros2 launch cloud_llm_node cloud_llm_launch.py \
+    cloud_llm_api_key:=sk-your-key \
+    cloud_llm_model:=deepseek-chat
+```
+

brain/CloudLLMNode/src/cloud_llm_node/__init__.py

@@ -0,0 +1,2 @@
+from cloud_llm_node import main
+

brain/CloudLLMNode/src/cloud_llm_node/cloud_llm_node.py

@@ -0,0 +1,605 @@
+#!/usr/bin/env python3
+"""
+CloudLLMNode - 云端大模型节点
+
+职责：
+1. 订阅 /planner/llm_request，只处理 route_hint == "cloud" 的请求
+2. 调用 DeepSeek 云端 API
+3. 解析模型输出：自然语言 + <FINAL_JSON> + JSON
+4. 发布 /planner/llm_response，与 OmniNode 兼容
+5. 失败时返回安全 fallback JSON
+
+设计原则：
+- CloudLLMNode 是"模型调用适配层"，不是"业务提示词生成器"
+- 严格依赖 /planner/llm_request 中已有内容
+- 不在节点内部定义任何机器人业务提示词
+"""
+
+import json
+import re
+import time
+import uuid
+from typing import Optional, Dict, Any, Tuple, List
+
+import rclpy
+from rclpy.node import Node
+from std_msgs.msg import String
+
+
+class CloudLLMNode(Node):
+    """云端大模型节点"""
+
+    # 极薄协议约束 - 不包含任何业务内容
+    DEFAULT_SYSTEM_PROMPT = """你是一个通用模型助手。
+请严格基于输入内容回答。
+
+==输出协议（必须严格遵守）==
+1. 第一段：自然语言回复（可直接播报给用户）
+2. 单独一行：<FINAL_JSON>
+3. 第三段：严格 JSON 对象
+
+==禁止事项==
+1. 禁止使用 markdown 代码块（如 ```json）
+2. 禁止在 JSON 后追加任何解释文字
+3. 禁止输出纯 JSON（必须包含自然语言前缀）
+4. JSON 必须是合法可解析的
+"""
+
+    def __init__(self, node_name: str = "cloud_llm_node"):
+        super().__init__(node_name)
+
+        self._default_api_key = ""
+        self._default_base_url = "https://api.deepseek.com"
+        self._default_model = "deepseek-chat"
+        self._default_timeout = 30.0
+        self._default_enabled = True
+        self._default_separator = "<FINAL_JSON>"
+        self._default_system_prompt_path = ""
+        self._default_debug = False
+
+        self.declare_parameter("cloud_llm_api_key", self._default_api_key)
+        self.declare_parameter("cloud_llm_base_url", self._default_base_url)
+        self.declare_parameter("cloud_llm_model", self._default_model)
+        self.declare_parameter("cloud_llm_timeout_sec", self._default_timeout)
+        self.declare_parameter("cloud_llm_enabled", self._default_enabled)
+        self.declare_parameter("model_response_separator", self._default_separator)
+        self.declare_parameter("cloud_system_prompt_path", self._default_system_prompt_path)
+        self.declare_parameter("cloud_debug", self._default_debug)
+
+        self.declare_parameter("llm_request_topic", "/planner/llm_request")
+        self.declare_parameter("llm_response_topic", "/planner/llm_response")
+
+        def _get_param(name: str, default: Any) -> Any:
+            value = self.get_parameter(name).value
+            if value is None or value == "":
+                return default
+            if isinstance(default, float) and isinstance(value, str):
+                try:
+                    return float(value)
+                except (ValueError, TypeError):
+                    return default
+            if isinstance(default, bool) and isinstance(value, str):
+                return value.lower() in ('true', '1', 'yes')
+            return value
+
+        self.api_key: str = _get_param("cloud_llm_api_key", self._default_api_key)
+        self.base_url: str = _get_param("cloud_llm_base_url", self._default_base_url)
+        self.model: str = _get_param("cloud_llm_model", self._default_model)
+        self.timeout_sec: float = _get_param("cloud_llm_timeout_sec", self._default_timeout)
+        self.enabled: bool = _get_param("cloud_llm_enabled", self._default_enabled)
+        self.separator: str = _get_param("model_response_separator", self._default_separator)
+        self.system_prompt_path: str = _get_param("cloud_system_prompt_path", self._default_system_prompt_path)
+        self.debug: bool = _get_param("cloud_debug", self._default_debug)
+
+        self.request_topic: str = self.get_parameter("llm_request_topic").value
+        self.response_topic: str = self.get_parameter("llm_response_topic").value
+
+        if not self.enabled:
+            self.get_logger().warn("[CloudLLMNode] cloud_llm_enabled=False，节点已禁用")
+        elif not self.api_key:
+            self.get_logger().warn("[CloudLLMNode] cloud_llm_api_key 未配置，请检查配置文件")
+            self.enabled = False
+        else:
+            self.get_logger().info(f"[CloudLLMNode] 配置加载完成:")
+            self.get_logger().info(f"  - Base URL: {self.base_url}")
+            self.get_logger().info(f"  - Model: {self.model}")
+            self.get_logger().info(f"  - Timeout: {self.timeout_sec}s")
+            self.get_logger().info(f"  - Separator: {self.separator}")
+
+        self.system_prompt = self._load_system_prompt()
+
+        qos = rclpy.qos.QoSProfile(depth=10)
+
+        self.sub_request = self.create_subscription(
+            String, self.request_topic, self._on_llm_request, qos
+        )
+        self.pub_response = self.create_publisher(String, self.response_topic, qos)
+        self.pub_natural_response = self.create_publisher(String, "/llm_response", qos)
+
+        self.get_logger().info(f"[CloudLLMNode] 订阅 topic: {self.request_topic}")
+        self.get_logger().info(f"[CloudLLMNode] 发布 topic: {self.response_topic}")
+        self.get_logger().info(f"[CloudLLMNode] 发布自然语言 topic: /llm_response")
+
+        self.waiting_for_response = False
+        self.current_request_id = None
+        self.current_session_id = None
+
+        self.get_logger().info("[CloudLLMNode] 节点初始化完成")
+
+    def _load_system_prompt(self) -> str:
+        if self.system_prompt_path:
+            try:
+                with open(self.system_prompt_path, 'r', encoding='utf-8') as f:
+                    content = f.read().strip()
+                    if content:
+                        self.get_logger().info(f"[CloudLLMNode] 从文件加载 system prompt: {self.system_prompt_path}")
+                        return content
+            except Exception as e:
+                self.get_logger().warn(f"[CloudLLMNode] 加载 system prompt 文件失败: {e}，使用内置默认")
+        return self.DEFAULT_SYSTEM_PROMPT
+
+    def _on_llm_request(self, msg: String) -> None:
+        try:
+            request_data = json.loads(msg.data)
+        except json.JSONDecodeError as e:
+            self.get_logger().error(f"[CloudLLMNode] JSON 解析失败: {e}")
+            return
+
+        route_hint = request_data.get("route_hint", "")
+        request_id = request_data.get("request_id", "unknown")
+        session_id = request_data.get("session_id", "default")
+
+        if self.debug:
+            self.get_logger().info(f"[CloudLLMNode] 收到请求: request_id={request_id}, route_hint={route_hint}")
+
+        if route_hint != "cloud":
+            return
+
+        if not self.enabled:
+            self.get_logger().warn(f"[CloudLLMNode] 云端 LLM 未启用，忽略 cloud 请求")
+            return
+
+        if self.waiting_for_response:
+            self.get_logger().warn(f"[CloudLLMNode] 正在等待上一个响应，忽略重复请求")
+            return
+
+        self.waiting_for_response = True
+        self.current_request_id = request_id
+        self.current_session_id = session_id
+
+        try:
+            self._process_cloud_request(request_data)
+        except Exception as e:
+            self.get_logger().error(f"[CloudLLMNode] 处理请求异常: {e}")
+            self._publish_fallback_response(request_id, session_id, f"process_error: {e}")
+        finally:
+            self.waiting_for_response = False
+            self.current_request_id = None
+            self.current_session_id = None
+
+    def _process_cloud_request(self, request_data: dict) -> None:
+        request_id = request_data.get("request_id", "unknown")
+        session_id = request_data.get("session_id", "default")
+        user_text = request_data.get("user_text", "")
+
+        if self.debug:
+            self.get_logger().info(f"[CloudLLMNode] 处理 cloud 请求: user_text={user_text}")
+
+        # 构建消息列表
+        messages = self.build_cloud_messages(request_data)
+        
+        # 打印完整构建的消息（方便调试）
+        self.get_logger().info(f"[CloudLLMNode] ===== 构建的完整消息 =====")
+        for i, msg in enumerate(messages):
+            role = msg.get("role", "unknown")
+            content = msg.get("content", "")
+            # 打印前2000字符以确保完整
+            self.get_logger().info(f"[CloudLLMNode] [{i}] Role: {role}")
+            if len(content) > 2000:
+                self.get_logger().info(f"[CloudLLMNode] [{i}] Content (前2000字符):\n{content[:2000]}")
+                self.get_logger().info(f"[CloudLLMNode] [{i}] Content (后500字符):\n...{content[-500:]}")
+            else:
+                self.get_logger().info(f"[CloudLLMNode] [{i}] Content:\n{content}")
+        self.get_logger().info(f"[CloudLLMNode] ===== 消息构建完成 =====")
+        
+        success, full_response = self._call_deepseek_api(messages)
+
+        if not success:
+            self.get_logger().error(f"[CloudLLMNode] API 调用失败: {full_response}")
+            self._publish_fallback_response(request_id, session_id, f"api_error: {full_response}")
+            return
+
+        # 打印完整的 LLM 原始响应
+        self.get_logger().info(f"[CloudLLMNode] ===== LLM 完整原始响应 (len={len(full_response)}) =====")
+        self.get_logger().info(f"[CloudLLMNode] {full_response}")
+        self.get_logger().info(f"[CloudLLMNode] ===== 响应打印完毕 =====")
+
+        success, parsed_json, natural_response = self._parse_llm_response(full_response)
+
+        if not success:
+            self.get_logger().warn(f"[CloudLLMNode] JSON 解析失败，使用 fallback")
+            self._publish_fallback_response(request_id, session_id, "json_parse_failed")
+            return
+
+        self._publish_response(request_id, session_id, parsed_json, natural_response)
+
+    def build_cloud_messages(self, request_data: dict) -> List[dict]:
+        """
+        构建发给云端 LLM 的消息列表。
+        
+        优先级策略：
+        1. 最高优先：request_data 中已有 'messages' 字段 → 直接使用
+        2. 其次：request_data 中已有 'system_prompt' 字段 → 作为 system message
+        3. 再次：request_data 中已有 'prompt' / 'prompt_text' 字段 → 直接作为 user message
+        4. 最低：都不存在 → 使用极薄协议 + 最小字段转发
+        
+        设计原则：CloudLLMNode 是"模型调用适配层"，不是"业务提示词生成器"
+        """
+        
+        # 优先级 1: 如果 request_data 中已有完整的 messages，直接使用
+        if "messages" in request_data and request_data["messages"]:
+            messages = request_data["messages"]
+            if isinstance(messages, list) and len(messages) > 0:
+                self.get_logger().info(f"[CloudLLMNode] 使用 request_data 中已有的 messages (共 {len(messages)} 条)")
+                return messages
+        
+        # 优先级 2: 如果有完整的 prompt 文本
+        prompt_text = None
+        if "prompt" in request_data and request_data["prompt"]:
+            prompt_text = request_data["prompt"]
+        elif "prompt_text" in request_data and request_data["prompt_text"]:
+            prompt_text = request_data["prompt_text"]
+        
+        if prompt_text:
+            self.get_logger().info(f"[CloudLLMNode] 使用 request_data 中已有的 prompt (len={len(prompt_text)})")
+            return [
+                {"role": "system", "content": self.system_prompt},
+                {"role": "user", "content": prompt_text}
+            ]
+        
+        # 优先级 3: 如果有 system_prompt 字段
+        custom_system_prompt = request_data.get("system_prompt", "")
+        if custom_system_prompt:
+            self.get_logger().info(f"[CloudLLMNode] 使用 request_data 中的 system_prompt")
+            system_content = custom_system_prompt
+        else:
+            system_content = self.system_prompt
+        
+        # 优先级 4: 最小字段转发模式（不做业务加工）
+        user_prompt = self._build_minimal_user_prompt(request_data)
+        
+        return [
+            {"role": "system", "content": system_content},
+            {"role": "user", "content": user_prompt}
+        ]
+
+    def _build_minimal_user_prompt(self, request_data: dict) -> str:
+        """
+        最小字段转发 - 只做原样转发，不做业务加工。
+        
+        格式简单清晰，不包含任何机器人业务提示词。
+        """
+        parts = []
+        
+        # 用户输入
+        user_text = request_data.get("user_text", "")
+        if user_text:
+            parts.append(f"用户输入: {user_text}")
+        
+        # 记忆上下文
+        memory_text = request_data.get("memory_text", "")
+        if memory_text:
+            parts.append(f"\n记忆上下文: {memory_text}")
+        
+        # 世界状态
+        world_snapshot = request_data.get("world_snapshot", {})
+        if world_snapshot:
+            if isinstance(world_snapshot, str):
+                parts.append(f"\n世界状态: {world_snapshot}")
+            else:
+                parts.append(f"\n世界状态: {json.dumps(world_snapshot, ensure_ascii=False)}")
+        
+        # 意图信息
+        intent_info = request_data.get("intent_info", {})
+        if intent_info:
+            intent_str = json.dumps(intent_info, ensure_ascii=False)
+            parts.append(f"\n意图信息: {intent_str}")
+        
+        # 老师记忆
+        teacher_memory_hint = request_data.get("teacher_memory_hint", "")
+        if teacher_memory_hint:
+            if isinstance(teacher_memory_hint, dict):
+                parts.append(f"\n老师记忆: {json.dumps(teacher_memory_hint, ensure_ascii=False)}")
+            else:
+                parts.append(f"\n老师记忆: {teacher_memory_hint}")
+        
+        # 可用工具
+        available_tools = request_data.get("available_tools", [])
+        if available_tools:
+            if isinstance(available_tools, list):
+                tools_str = ", ".join(str(t) for t in available_tools)
+            else:
+                tools_str = str(available_tools)
+            parts.append(f"\n可用工具: {tools_str}")
+        
+        # 工具描述
+        tool_descriptions = request_data.get("tool_descriptions", [])
+        if tool_descriptions:
+            if isinstance(tool_descriptions, list):
+                desc_parts = []
+                for t in tool_descriptions:
+                    if isinstance(t, dict):
+                        name = t.get("name", "")
+                        desc = t.get("description", "")
+                        desc_parts.append(f"- {name}: {desc}")
+                    else:
+                        desc_parts.append(f"- {t}")
+                parts.append(f"\n工具描述:\n" + "\n".join(desc_parts))
+            else:
+                parts.append(f"\n工具描述: {tool_descriptions}")
+        
+        # 领域规则
+        domain_rules = request_data.get("domain_rules", {})
+        if domain_rules:
+            if isinstance(domain_rules, dict):
+                parts.append(f"\n领域规则: {json.dumps(domain_rules, ensure_ascii=False)}")
+            else:
+                parts.append(f"\n领域规则: {domain_rules}")
+        
+        # 规划模式
+        planner_mode = request_data.get("planner_mode", "")
+        if planner_mode:
+            parts.append(f"\n规划模式: {planner_mode}")
+        
+        # 当前状态
+        current_state = request_data.get("current_state", {})
+        if current_state:
+            if isinstance(current_state, dict):
+                parts.append(f"\n当前状态: {json.dumps(current_state, ensure_ascii=False)}")
+            else:
+                parts.append(f"\n当前状态: {current_state}")
+        
+        # 输出要求
+        parts.append("\n\n请严格基于以上内容回答，并严格按协议输出。")
+        
+        return "\n".join(parts)
+
+    def _call_deepseek_api(self, messages: list) -> Tuple[bool, str]:
+        try:
+            import openai
+            
+            # 打印完整的请求信息
+            self.get_logger().info(f"[CloudLLMNode] ===== 发送请求到云端 LLM =====")
+            self.get_logger().info(f"[CloudLLMNode] API: {self.base_url}/chat/completions")
+            self.get_logger().info(f"[CloudLLMNode] Model: {self.model}")
+            self.get_logger().info(f"[CloudLLMNode] Temperature: 0.7, Max Tokens: 2048")
+            self.get_logger().info(f"[CloudLLMNode] ===== System Prompt =====")
+            for msg in messages:
+                if msg["role"] == "system":
+                    self.get_logger().info(f"[CloudLLMNode] [SYSTEM] {msg['content'][:500]}...")
+                elif msg["role"] == "user":
+                    self.get_logger().info(f"[CloudLLMNode] [USER] {msg['content'][:1000]}...")
+            self.get_logger().info(f"[CloudLLMNode] ===== 请求消息总数: {len(messages)} =====")
+            
+            client = openai.OpenAI(api_key=self.api_key, base_url=self.base_url, timeout=self.timeout_sec)
+            response = client.chat.completions.create(
+                model=self.model, messages=messages, temperature=0.7, max_tokens=2048,
+            )
+            if not response.choices:
+                return False, "empty_choices"
+            content = response.choices[0].message.content
+            if not content:
+                return False, "empty_content"
+            return True, content
+        except ImportError:
+            self.get_logger().error("[CloudLLMNode] 未安装 openai 库，请运行: pip install openai")
+            return False, "openai_not_installed"
+        except Exception as e:
+            self.get_logger().error(f"[CloudLLMNode] API 调用异常: {e}")
+            return False, str(e)
+
+    def _parse_llm_response(self, full_text: str) -> Tuple[bool, dict, str]:
+        if not full_text:
+            return False, {}, ""
+
+        natural_response = ""
+        json_part = ""
+        separator = self.separator
+
+        if separator in full_text:
+            parts = full_text.split(separator, 1)
+            natural_response = parts[0].strip()
+            if len(parts) > 1:
+                json_part = parts[1].strip()
+        else:
+            self.get_logger().warn(f"[CloudLLMNode] 未找到分隔符 {separator}，尝试兜底解析")
+            natural_response = full_text.strip()
+            json_part = full_text.strip()
+
+        success, json_data = self.extract_json_from_response(json_part)
+        if success:
+            return True, json_data, natural_response
+        else:
+            success, json_data = self.extract_json_from_response(full_text)
+            if success:
+                return True, json_data, natural_response
+            return False, {}, natural_response
+
+    def extract_json_from_response(self, text: str) -> Tuple[bool, dict]:
+        if not text:
+            return False, {}
+
+        text = text.strip()
+
+        # 方法1: 直接尝试解析整个文本
+        try:
+            json_data = json.loads(text)
+            return True, json_data
+        except json.JSONDecodeError:
+            pass
+
+        # 方法2: 查找最外层 JSON 对象
+        json_pattern = r'\{[\s\S]*\}'
+        matches = list(re.finditer(json_pattern, text))
+        if matches:
+            for match in reversed(matches):
+                candidate = match.group(0)
+                try:
+                    json_data = json.loads(candidate)
+                    if isinstance(json_data, dict) and "plan" in json_data:
+                        return True, json_data
+                except json.JSONDecodeError:
+                    continue
+
+        return False, {}
+
+    def _publish_response(self, request_id: str, session_id: str, json_data: dict, natural_response: str = "") -> None:
+        try:
+            if "request_id" not in json_data or not json_data.get("request_id"):
+                json_data["request_id"] = request_id
+            if "session_id" not in json_data or not json_data.get("session_id"):
+                json_data["session_id"] = session_id
+
+            if "plan" not in json_data or not isinstance(json_data.get("plan"), dict):
+                json_data["plan"] = {}
+
+            plan = json_data["plan"]
+
+            if "plan_id" not in plan or not plan.get("plan_id"):
+                plan["plan_id"] = f"cloud_plan_{uuid.uuid4().hex[:8]}"
+            if "goal" not in plan or not plan.get("goal"):
+                plan["goal"] = "云端 LLM 生成的任务"
+            if "reasoning" not in plan or not plan.get("reasoning"):
+                plan["reasoning"] = "由 CloudLLMNode 调用 DeepSeek 生成"
+            if "risk_level" not in plan or not plan.get("risk_level"):
+                plan["risk_level"] = "low"
+            if "requires_confirmation" not in plan:
+                plan["requires_confirmation"] = False
+            if "confirmation_message" not in plan:
+                plan["confirmation_message"] = None
+            if "steps" not in plan:
+                plan["steps"] = []
+            # 将 action_list 转换为 steps（兼容不同格式）
+            if "action_list" in plan and plan["action_list"]:
+                if not plan["steps"]:
+                    for action in plan["action_list"]:
+                        step = {
+                            "action": action.get("action", ""),
+                            "description": action.get("description", ""),
+                            "parameters": action.get("parameters", {}),
+                            "preconditions": action.get("preconditions", {}),
+                            "fallback": action.get("fallback"),
+                            "status": "pending"
+                        }
+                        plan["steps"].append(step)
+            if "status" not in plan or not plan.get("status"):
+                plan["status"] = "created"
+            if "source" not in plan or not plan.get("source"):
+                plan["source"] = "cloud_llm"
+            if "metadata" not in plan or not isinstance(plan.get("metadata"), dict):
+                plan["metadata"] = {}
+
+            for step in plan.get("steps", []):
+                if "step_id" not in step:
+                    step["step_id"] = 1
+                if "action" not in step:
+                    step["action"] = "unknown"
+                if "tool_call_type" not in step:
+                    step["tool_call_type"] = "action"
+                if "parameters" not in step:
+                    step["parameters"] = {}
+                if "preconditions" not in step:
+                    step["preconditions"] = {}
+                if "fallback" not in step:
+                    step["fallback"] = None
+                if "status" not in step:
+                    step["status"] = "pending"
+                if "description" not in step:
+                    step["description"] = step.get("action", "")
+                if "requires_confirmation" not in step:
+                    step["requires_confirmation"] = False
+                if "confirmation_message" not in step:
+                    step["confirmation_message"] = None
+                if "metadata" not in step:
+                    step["metadata"] = {}
+
+            response_json = json.dumps(json_data, ensure_ascii=False)
+            msg = String()
+            msg.data = response_json
+            self.pub_response.publish(msg)
+
+            # 同时发布自然语言到 /llm_response (供 TTS 使用)
+            if natural_response:
+                natural_msg = String()
+                natural_msg.data = natural_response
+                self.pub_natural_response.publish(natural_msg)
+
+            self.get_logger().info(
+                f"[CloudLLMNode] 发布响应: request_id={request_id}, source={plan.get('source')}, steps={len(plan.get('steps', []))}"
+            )
+        except Exception as e:
+            self.get_logger().error(f"[CloudLLMNode] 发布响应异常: {e}")
+            self._publish_fallback_response(request_id, session_id, f"publish_error: {e}")
+
+    def _publish_fallback_response(self, request_id: str, session_id: str, reason: str = "fallback") -> None:
+        fallback_json = self.build_fallback_json(request_id, session_id, reason)
+        fallback_text = "抱歉，我没有完全理解您的请求，请再说一次。"
+        try:
+            response_json = json.dumps(fallback_json, ensure_ascii=False)
+            msg = String()
+            msg.data = response_json
+            self.pub_response.publish(msg)
+
+            # 同时发布自然语言到 /llm_response (供 TTS 使用)
+            natural_msg = String()
+            natural_msg.data = fallback_text
+            self.pub_natural_response.publish(natural_msg)
+
+            self.get_logger().warn(f"[CloudLLMNode] 发布 fallback 响应: request_id={request_id}, reason={reason}")
+        except Exception as e:
+            self.get_logger().error(f"[CloudLLMNode] 发布 fallback 失败: {e}")
+
+    def build_fallback_json(self, request_id: str, session_id: str, reason: str = "fallback") -> dict:
+        return {
+            "request_id": request_id,
+            "session_id": session_id,
+            "plan": {
+                "plan_id": f"cloud_fallback_{uuid.uuid4().hex[:8]}",
+                "goal": "无法可靠解析云端模型输出",
+                "reasoning": f"Cloud LLM 输出解析失败，返回安全 fallback。原因: {reason}",
+                "risk_level": "low",
+                "requires_confirmation": False,
+                "confirmation_message": None,
+                "steps": [
+                    {
+                        "step_id": 1,
+                        "action": "ask_user",
+                        "tool_call_type": "ask_user",
+                        "parameters": {"question": "抱歉，我没有完全理解您的请求，请再说一次。"},
+                        "preconditions": {},
+                        "fallback": None,
+                        "status": "pending",
+                        "description": "fallback: 请求用户重新描述",
+                        "requires_confirmation": False,
+                        "confirmation_message": None,
+                        "metadata": {"fallback_reason": reason, "fallback_source": "cloud_llm"}
+                    }
+                ],
+                "status": "created",
+                "source": "cloud_fallback",
+                "metadata": {"response_type": "fallback", "fallback": True, "fallback_reason": reason}
+            }
+        }
+
+
+def main(args=None):
+    rclpy.init(args=args)
+    node = CloudLLMNode()
+    try:
+        rclpy.spin(node)
+    except KeyboardInterrupt:
+        node.get_logger().info("[CloudLLMNode] 收到 Ctrl+C，正在关闭...")
+    finally:
+        node.destroy_node()
+        rclpy.shutdown()
+

brain/CloudLLMNode/src/cloud_llm_node/config/cloud_llm_config.yaml

@@ -0,0 +1,13 @@
+/**:
+  ros__parameters:
+    cloud_llm_api_key: "sk-d120dec9f3224aa6861d28bb82d155dd"
+    cloud_llm_base_url: "https://api.deepseek.com"
+    cloud_llm_model: "deepseek-chat"
+    cloud_llm_timeout_sec: 30.0
+    cloud_llm_enabled: true
+    model_response_separator: "<FINAL_JSON>"
+    cloud_system_prompt_path: ""
+    cloud_debug: false
+    llm_request_topic: "/planner/llm_request"
+    llm_response_topic: "/planner/llm_response"
+

brain/CloudLLMNode/src/cloud_llm_node/launch/cloud_llm_launch.py

@@ -0,0 +1,40 @@
+"""CloudLLMNode 启动文件
+
+用法：
+    ros2 launch cloud_llm_node cloud_llm_launch.py
+"""
+
+from launch import LaunchDescription
+from launch_ros.actions import Node
+from launch.actions import DeclareLaunchArgument
+import os
+
+
+def generate_launch_description():
+    config_file = os.path.join(
+        os.path.dirname(os.path.abspath(__file__)),
+        '../config/cloud_llm_config.yaml'
+    )
+
+    return LaunchDescription([
+        DeclareLaunchArgument('cloud_llm_api_key', default_value='', description='DeepSeek API Key'),
+        DeclareLaunchArgument('cloud_llm_base_url', default_value='', description='API Base URL'),
+        DeclareLaunchArgument('cloud_llm_model', default_value='', description='模型名称'),
+        DeclareLaunchArgument('cloud_llm_timeout_sec', default_value='0', description='请求超时时间（秒）'),
+        DeclareLaunchArgument('cloud_llm_enabled', default_value='', description='是否启用云端 LLM'),
+        DeclareLaunchArgument('cloud_debug', default_value='', description='调试日志开关'),
+        DeclareLaunchArgument('cloud_system_prompt_path', default_value='', description='自定义 System Prompt 文件路径'),
+
+        Node(
+            package='cloud_llm_node',
+            executable='cloud_llm_node',
+            name='cloud_llm_node',
+            output='screen',
+            parameters=[config_file],
+            remappings=[
+                ('/planner/llm_request', 'planner/llm_request'),
+                ('/planner/llm_response', 'planner/llm_response'),
+            ],
+        ),
+    ])
+

brain/CloudLLMNode/src/cloud_llm_node/package.xml

@@ -0,0 +1,27 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>cloud_llm_node</name>
+  <version>1.0.0</version>
+  <description>云端大模型节点，用于调用 DeepSeek 等云端 LLM 服务</description>
+
+  <maintainer email="your@email.com">Your Name</maintainer>
+  <license>MIT</license>
+
+  <buildtool_depend>ament_python</buildtool_depend>
+
+  <depend>rclpy</depend>
+  <depend>std_msgs</depend>
+
+  <exec_depend>openai</exec_depend>
+
+  <test_depend>ament_copyright</test_depend>
+  <test_depend>ament_flake8</test_depend>
+  <test_depend>ament_pep257</test_depend>
+  <test_depend>python3-pytest</test_depend>
+
+  <export>
+    <build_type>ament_python</build_type>
+  </export>
+</package>
+

brain/CloudLLMNode/src/cloud_llm_node/setup.cfg

@@ -0,0 +1,6 @@
+[develop]
+script_dir=$base/lib/cloud_llm_node
+
+[install]
+install_scripts=$base/lib/cloud_llm_node
+

brain/CloudLLMNode/src/cloud_llm_node/setup.py

@@ -0,0 +1,30 @@
+from setuptools import setup, find_packages
+from glob import glob
+import os
+
+package_name = 'cloud_llm_node'
+
+setup(
+    name=package_name,
+    version='1.0.0',
+    py_modules=['cloud_llm_node'],
+    data_files=[
+        ('share/ament_index/resource_index/packages', ['resource/' + package_name]),
+        ('share/' + package_name, ['package.xml']),
+        (os.path.join('share', package_name, 'launch'), glob('launch/*.py')),
+        (os.path.join('share', package_name, 'config'), glob('config/*.yaml')),
+    ],
+    install_requires=['setuptools'],
+    zip_safe=True,
+    maintainer='Your Name',
+    maintainer_email='your@email.com',
+    description='云端大模型节点，用于调用 DeepSeek 等云端 LLM 服务',
+    license='MIT',
+    tests_require=['pytest'],
+    entry_points={
+        'console_scripts': [
+            'cloud_llm_node = cloud_llm_node:main',
+        ],
+    },
+)
+

brain/OmniNode/src/OmniNode/CMakeLists.txt

@@ -0,0 +1,85 @@
+cmake_minimum_required(VERSION 3.8)
+project(omni_node)
+
+if(CMAKE_CXX_STANDARD GREATER_EQUAL 20)
+  set(CMAKE_CXX_STANDARD 17)
+endif()
+if(NOT CMAKE_CXX_STANDARD)
+  set(CMAKE_CXX_STANDARD 17)
+endif()
+set(CMAKE_CXX_STANDARD_REQUIRED ON)
+
+find_package(ament_cmake REQUIRED)
+find_package(rclcpp REQUIRED)
+find_package(std_msgs REQUIRED)
+find_package(rcl_interfaces REQUIRED)
+find_package(nlohmann_json REQUIRED)
+
+# Read XLM_SDK_ROOT: from cmake cache (-DXLM_SDK_ROOT=... via colcon --cmake-args)
+# or from environment variable XLM_SDK_ROOT
+if(DEFINED ENV{XLM_SDK_ROOT})
+  set(XLM_SDK_ROOT "$ENV{XLM_SDK_ROOT}")
+endif()
+if(NOT XLM_SDK_ROOT)
+  message(FATAL_ERROR "XLM_SDK_ROOT is not set. Pass -DXLM_SDK_ROOT=/path/to/sdk via colcon --cmake-args")
+endif()
+message(STATUS "XLM_SDK_ROOT = ${XLM_SDK_ROOT}")
+
+add_library(omni_node_lib STATIC
+  src/omni_engine.cpp
+  src/omni_node.cpp
+)
+
+ament_target_dependencies(omni_node_lib
+  rclcpp
+  std_msgs
+  rcl_interfaces
+  nlohmann_json
+)
+
+target_include_directories(omni_node_lib
+  PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}/include
+  # 系统路径优先于 SDK，避免 SDK 中损坏的 nlohmann 版本
+  SYSTEM PUBLIC /usr/include/aarch64-linux-gnu
+  PRIVATE ${XLM_SDK_ROOT}/include
+)
+
+ament_export_libraries(omni_node_lib)
+
+add_executable(omni_node
+  src/main.cpp
+)
+
+target_include_directories(omni_node
+  SYSTEM PUBLIC /usr/include/aarch64-linux-gnu
+  PRIVATE ${XLM_SDK_ROOT}/include
+)
+
+ament_target_dependencies(omni_node
+  rclcpp
+  std_msgs
+  rcl_interfaces
+  rmw_fastrtps_cpp
+  rosidl_typesupport_cpp
+  nlohmann_json
+)
+
+target_link_libraries(omni_node
+  omni_node_lib
+  ${XLM_SDK_ROOT}/lib/libxlm.so
+)
+
+install(TARGETS omni_node omni_node_lib
+  DESTINATION lib/${PROJECT_NAME}
+)
+
+install(DIRECTORY
+  launch/
+  config/
+  DESTINATION share/${PROJECT_NAME}/
+)
+
+# Register shell environment hook so xlm SDK lib is prepended to LD_LIBRARY_PATH
+ament_environment_hooks("${CMAKE_CURRENT_SOURCE_DIR}/env/omni_node.sh.in")
+
+ament_package()

brain/OmniNode/src/OmniNode/README.md

@@ -0,0 +1,147 @@
+# OmniNode
+
+ROS2 node for Horizon Robotics S100P to run Qwen2.5-3B Omni multimodal LLM inference via xlm SDK.
+
+## Directory Structure
+
+```
+OmniNode/
+├── CMakeLists.txt
+├── package.xml
+├── config/
+│   └── omni_config.json
+├── include/
+│   └── omni_node/
+│       ├── omni_engine.hpp
+│       └── omni_node.hpp
+├── launch/
+│   └── omni_launch.py
+├── src/
+│   ├── main.cpp
+│   ├── omni_engine.cpp
+│   └── omni_node.cpp
+└── README.md
+```
+
+## Dependencies
+
+- ROS2 Humble
+- nlohmann/json (header-only, included via `single_include/nlohmann/json.hpp`)
+- xlm SDK (pre-installed on S100P)
+
+On S100P, download the single-header JSON library into the project:
+
+```bash
+cd ~/colcon_ws/src/OmniNode
+mkdir -p include/nlohmann
+wget https://raw.githubusercontent.com/nlohmann/json/develop/single_include/nlohmann/json.hpp -O include/nlohmann/json.hpp
+```
+
+## Configuration
+
+Edit `config/omni_config.json` and fill in the paths to your HBM model files:
+
+```json
+{
+  "visual_hbm_path": "/opt/models/visual.hbm",
+  "audio_hbm_path": "/opt/models/audio.hbm",
+  "text_hbm_path": "/opt/models/text.hbm",
+  "embed_tokens": "/opt/models/embed_tokens.bin",
+  "tokenizer_dir": "/opt/models/tokenizer/",
+  "model_type": 5,
+  "online_mode": true,
+  "bpu_core": -1,
+  "system_prompt": "You are Qwen, a virtual human..."
+}
+```
+
+| Field | Description |
+|---|---|
+| `visual_hbm_path` | Vision model HBM file |
+| `audio_hbm_path` | Audio model HBM file |
+| `text_hbm_path` | Text model HBM file |
+| `embed_tokens` | Embed tokens file |
+| `tokenizer_dir` | Tokenizer directory |
+| `model_type` | Model type (5 = Omni) |
+| `online_mode` | Online inference mode |
+| `bpu_core` | BPU core (-1 = any) |
+| `system_prompt` | System prompt for the model |
+
+## Build
+
+On your development PC or directly on S100P:
+
+```bash
+cd ~/colcon_ws/src/OmniNode
+colcon build --packages-select omni_node
+source install/setup.bash
+```
+
+Or with ROS2 workspace:
+
+```bash
+cd ~/ros2_ws/src
+# copy OmniNode folder here
+cd ~/ros2_ws
+colcon build --packages-select omni_node
+source install/setup.bash
+```
+
+## Run
+
+### Option 1: Direct launch
+
+```bash
+ros2 run omni_node omni_node --ros-args -p config_path:=config/omni_config.json
+```
+
+### Option 2: Launch file
+
+```bash
+ros2 launch omni_node omni_launch.py
+```
+
+### Option 3: With custom config path
+
+```bash
+ros2 launch omni_node omni_launch.py config_path:=/full/path/to/your/omni_config.json
+```
+
+## Topics
+
+| Topic | Type | Direction | Description |
+|---|---|---|---|
+| `/asr` | `std_msgs/msg/String` | Subscribe | Input ASR text |
+| `/llm_response` | `std_msgs/msg/String` | Publish | LLM response text |
+
+## Usage
+
+After the node is running, publish a text message to `/asr`:
+
+```bash
+ros2 topic pub /asr std_msgs/msg/String '{data: "请描述我在做什么"}' --once
+```
+
+The LLM response will be published to `/llm_response`:
+
+```bash
+ros2 topic echo /llm_response
+```
+
+## Architecture
+
+```
+omni_node (ROS2 Node)
+  └── OmniEngine (xlm SDK Wrapper)
+        └── xlm_init / xlm_omni_feed_text_online / xlm_omni
+```
+
+- **omni_node.hpp/cpp**: ROS2 interface — subscribes to `/asr`, publishes `/llm_response`
+- **omni_engine.hpp/cpp**: xlm SDK wrapper — loads config, initializes model, handles callback
+- **main.cpp**: entry point, creates rclcpp node and spins
+
+## Notes
+
+- BPU core `-1` lets the SDK select any available BPU core automatically
+- Online mode must be `true` for streaming token output
+- The node is single-threaded (ROS2 default executor), no extra threads are spawned

brain/OmniNode/src/OmniNode/config/omni_config.json

@@ -0,0 +1,42 @@
+{
+  "visual_hbm_path": "/home/sunrise/opt/dev/bigmodel/LLM_S100/D-Robotics_LLM_S100_1.0.0_SDK/oellm_runtime/model/Qwen2.5_Omni_3B_Visual.hbm",
+  "audio_hbm_path": "/home/sunrise/opt/dev/bigmodel/LLM_S100/D-Robotics_LLM_S100_1.0.0_SDK/oellm_runtime/model/Qwen2.5_Omni_3B_Audio.hbm",
+  "text_hbm_path": "/home/sunrise/opt/dev/bigmodel/LLM_S100/D-Robotics_LLM_S100_1.0.0_SDK/oellm_runtime/model/Qwen2.5_Omni_3B_Text.hbm",
+  "embed_tokens": "/home/sunrise/opt/dev/bigmodel/LLM_S100/D-Robotics_LLM_S100_1.0.0_SDK/oellm_runtime/model/embed_tokens.bin",
+  "tokenizer_dir": "/home/sunrise/opt/dev/bigmodel/LLM_S100/D-Robotics_LLM_S100_1.0.0_SDK/oellm_runtime/config/Qwen2.5_Omni_3B_config/",
+  "model_type": 5,
+  "online_mode": false,
+  "bpu_core": -1,
+  "system_prompt": "You are Qwen, a virtual human developed by the Qwen Team, Alibaba Group, capable of perceiving auditory and visual inputs, as well as generating text and speech.",
+
+  "enable_legacy_asr_mode": false,
+  "enable_planner_mode": true,
+  "dialog_system_prompt": "你是一个机器人助手，负责与用户自然对话。",
+  "planner_system_prompt": "你是机器人智能体。你只负责意图识别和槽位提取，不做容错纠错，不做世界状态校验，不做执行决策。必须输出三段：第一行自然语言简短回复；第二行单独一行 <FINAL_JSON>；第三行一个严格 JSON 对象。只能输出一个 JSON，禁止 markdown，禁止解释。",
+
+  "model_response_separator": "<FINAL_JSON>",
+  "fallback_response_message": "抱歉，我没有完全理解您的请求，请再说一次。",
+
+  "planner_rules": {
+    "output_protocol": "输出顺序固定：自然语言回复\n<FINAL_JSON>\nJSON。只能输出一个 JSON 对象，不要 markdown，不要解释文字。",
+    "input_classification": "你只做意图识别和槽位提取。查询类：query_position、query_count、query_status。任务类：move、feed、turn_on、turn_off、adjust、inspect。不要做世界状态判断，不要判断地点是否可访问，不要做业务决策。",
+    "slot_extraction_rules": "从用户原话中提取 target、location、amount。保留原词，不做容错、不做纠错、不做标准化、不做别名映射。例如‘去牛鹏1号喂牛’里的 location 保持为‘牛鹏1号’；后续由 PlannerNode 统一归一化。若缺少执行任务所需关键槽位，则 need_clarify=true，并在 missing 中列出缺失项。",
+    "ask_user_rules": "只有当任务类输入缺少关键槽位时才 ask_user。query 类、能力说明类、闲聊类不要 ask_user。追问内容只针对缺失槽位本身，例如缺 location 就问‘请问去哪里执行？’。",
+    "capability_question_rules": "如果用户问你会什么、你能做什么、有哪些能力，使用 speak 回答可用能力，不执行任务。",
+    "json_schema": "JSON 固定为：{\"intent\":\"query|task|chat\",\"action\":\"feed|move|query_position|query_count|query_status|turn_on|turn_off|adjust|inspect|speak\",\"target\":\"字符串或空\",\"location\":\"字符串或空\",\"amount\":\"字符串或空\",\"need_clarify\":true或false,\"missing\":[]}。",
+    "example1": {
+      "description": "用户：去牛鹏1号喂牛",
+      "user_input": "去牛鹏1号喂牛",
+      "natural_response": "好的，我已理解您的任务。",
+      "json": "{\"intent\":\"task\",\"action\":\"feed\",\"target\":\"牛\",\"location\":\"牛鹏1号\",\"amount\":\"default\",\"need_clarify\":false,\"missing\":[]}"
+    },
+    "example2": {
+      "description": "用户：去喂牛",
+      "user_input": "去喂牛",
+      "natural_response": "请补充任务信息。",
+      "json": "{\"intent\":\"task\",\"action\":\"feed\",\"target\":\"牛\",\"location\":\"\",\"amount\":\"default\",\"need_clarify\":true,\"missing\":[\"location\"]}"
+    },
+    "prohibited": ""
+  }
+}
+

brain/OmniNode/src/OmniNode/env/omni_node.sh.in

@@ -0,0 +1,13 @@
+# prepend xlm SDK lib to LD_LIBRARY_PATH when the package is sourced
+_prepend_if_not_empty() {
+    local var="$1"
+    local value="$2"
+    if [ -n "$value" ]; then
+        if [ -z "${!var}" ]; then
+            export "$var"="$value"
+        else
+            export "$var"="$value:${!var}"
+        fi
+    fi
+}
+_prepend_if_not_empty "LD_LIBRARY_PATH" "@XLM_SDK_ROOT@/lib"

brain/OmniNode/src/OmniNode/include/omni_node/omni_engine.hpp

@@ -0,0 +1,137 @@
+#ifndef OMNI_ENGINE_HPP_
+#define OMNI_ENGINE_HPP_
+
+#include <string>
+#include <vector>
+#include <functional>
+#include <memory>
+#include <iostream>
+
+#include "xlm.h"
+
+#if __has_include(<nlohmann/json.hpp>)
+#include <nlohmann/json.hpp>
+using json = nlohmann::json;
+#else
+namespace nlohmann {
+  template<typename T> class basic_json;
+}
+using json = nlohmann::basic_json<int>;
+#endif
+
+class OmniEngine {
+public:
+  enum class Mode {
+    kLegacyDialog = 0,
+    kPlannerDialog = 1,
+  };
+
+  using StreamCallback = std::function<void(const std::string&, bool)>;
+  using PlannerResponseCallback = std::function<void(const std::string&, bool)>;
+
+  struct Config {
+    std::string visual_hbm_path;
+    std::string audio_hbm_path;
+    std::string text_hbm_path;
+    std::string embed_tokens;
+    std::string tokenizer_dir;
+    int32_t model_type;
+    bool online_mode;
+    int32_t bpu_core;
+    std::string system_prompt;
+
+    bool enable_legacy_asr_mode;
+    bool enable_planner_mode;
+    std::string dialog_system_prompt;
+    std::string planner_system_prompt;
+    std::string model_response_separator;
+    std::string fallback_response_message;
+
+    // planner 规则配置
+    std::string planner_output_protocol;
+    std::string planner_input_classification;
+    std::string planner_ask_user_rules;
+    std::string planner_capability_rules;
+    std::string planner_example1_user;
+    std::string planner_example1_response;
+    std::string planner_example1_json;
+    std::string planner_example2_user;
+    std::string planner_example2_response;
+    std::string planner_example2_json;
+    std::string planner_prohibited;
+  };
+
+  explicit OmniEngine(const std::string& config_path);
+  ~OmniEngine();
+
+  OmniEngine(const OmniEngine&) = delete;
+  OmniEngine& operator=(const OmniEngine&) = delete;
+
+  bool Init();
+  void SetStreamCallback(StreamCallback callback);
+  void SetPlannerResponseCallback(PlannerResponseCallback callback);
+
+  void FeedText(const std::string& user_text);
+  void FeedPlannerRequest(const std::string& json_request);
+
+  Mode GetCurrentMode() const { return current_mode_; }
+  std::string GetCurrentRequestId() const { return current_request_id_; }
+  std::string GetCurrentSessionId() const { return current_session_id_; }
+
+  // 用于 Node 层同步配置，避免两层配置不一致
+  void SetEnableMode(bool enable_legacy_asr_mode, bool enable_planner_mode) {
+    config_.enable_legacy_asr_mode = enable_legacy_asr_mode;
+    config_.enable_planner_mode = enable_planner_mode;
+    std::cerr << "[OmniEngine] Mode sync from Node: legacy=" << enable_legacy_asr_mode
+              << ", planner=" << enable_planner_mode << std::endl;
+  }
+
+private:
+  static OmniEngine* current_instance_;
+  static void StaticCallback(xlm_result_s* result, xlm_state_e state, void* userdata);
+  void HandleCallback(xlm_result_s* result, xlm_state_e state);
+
+  std::string BuildLegacyPrompt(const std::string& user_text);
+  std::string BuildPlannerSystemPrompt(const json& request_data);
+  std::string BuildPlannerUserContext(const json& request_data);
+  std::string BuildToolDescriptions(const json& available_tools);
+
+  std::string ExtractJsonFromResponse(const std::string& full_text);
+  std::string BuildFallbackJson();
+
+  bool ParsePlannerRequest(const std::string& json_request,
+                            std::string& request_id,
+                            std::string& session_id,
+                            std::string& user_text,
+                            std::string& memory_text,
+                            std::string& mode,
+                            std::string& planner_mode,
+                            json& world_snapshot,
+                            json& available_tools,
+                            json& tool_descriptions,
+                            json& domain_rules,
+                            json& current_state);
+
+  void ExecuteFeed(const std::string& prompt, Mode mode);
+  void ExecutePlannerFeed(const std::string& system_prompt, const std::string& user_context, Mode mode);
+
+  std::string config_path_;
+  Config config_;
+  xlm_handle_t handle_;
+  StreamCallback stream_callback_;
+  PlannerResponseCallback planner_response_callback_;
+
+  Mode current_mode_;
+  std::string current_request_id_;
+  std::string current_session_id_;
+  std::string accumulated_text_;
+  std::string pending_json_;
+  std::string pending_json_file_;
+  std::vector<xlm_lm_request_t> pending_requests_;
+
+  // Planner 流式状态管理
+  bool planner_separator_seen_;     // 是否已检测到 <FINAL_JSON> 分隔符
+  size_t planner_stream_emitted_len_; // 已发送给 /llm_response 的文本长度
+};
+
+#endif  // OMNI_ENGINE_HPP_

brain/OmniNode/src/OmniNode/include/omni_node/omni_node.hpp

@@ -0,0 +1,37 @@
+#ifndef OMNI_NODE_HPP_
+#define OMNI_NODE_HPP_
+
+#include <memory>
+#include <string>
+
+#include <rclcpp/rclcpp.hpp>
+#include <std_msgs/msg/string.hpp>
+
+#include "omni_node/omni_engine.hpp"
+
+namespace omni_node {
+
+class OmniNode : public rclcpp::Node {
+public:
+  explicit OmniNode(rclcpp::NodeOptions options);
+  ~OmniNode() override;
+
+private:
+  void OnAsrReceived(const std_msgs::msg::String::SharedPtr msg);
+  void OnPlannerLlmRequestReceived(const std_msgs::msg::String::SharedPtr msg);
+
+  std::unique_ptr<OmniEngine> engine_;
+  rclcpp::Subscription<std_msgs::msg::String>::SharedPtr sub_asr_;
+  rclcpp::Subscription<std_msgs::msg::String>::SharedPtr sub_planner_request_;
+  rclcpp::Publisher<std_msgs::msg::String>::SharedPtr pub_response_;
+  rclcpp::Publisher<std_msgs::msg::String>::SharedPtr pub_planner_response_;
+  bool engine_initialized_;
+
+  bool enable_legacy_asr_mode_;
+  bool enable_planner_mode_;
+};
+
+}  // namespace omni_node
+
+#endif  // OMNI_NODE_HPP_
+

brain/OmniNode/src/OmniNode/launch/omni_launch.py

@@ -0,0 +1,45 @@
+from launch import LaunchDescription
+from launch_ros.actions import Node
+from launch.actions import DeclareLaunchArgument
+from launch.substitutions import LaunchConfiguration
+
+
+def generate_launch_description():
+    xlm_sdk_root = '/home/sunrise/opt/dev/bigmodel/LLM_S100/D-Robotics_LLM_S100_1.0.0_SDK/oellm_runtime'
+    ld_library_path = f'{xlm_sdk_root}/lib:$LD_LIBRARY_PATH'
+
+    return LaunchDescription([
+        DeclareLaunchArgument('config_path', default_value='config/omni_config.json',
+                              description='Path to omni_config.json'),
+        DeclareLaunchArgument('node_name', default_value='omni_node',
+                              description='Name of the node'),
+        DeclareLaunchArgument('xlm_sdk_root', default_value=xlm_sdk_root,
+                              description='Path to xlm SDK'),
+        DeclareLaunchArgument('enable_legacy_asr_mode', default_value='false',
+                              description='Enable legacy /asr -> /llm_response mode'),
+        DeclareLaunchArgument('enable_planner_mode', default_value='true',
+                              description='Enable planner orchestrated mode'),
+
+        Node(
+            package='omni_node',
+            executable='omni_node',
+            name=LaunchConfiguration('node_name'),
+            output='screen',
+            parameters=[
+                {'config_path': LaunchConfiguration('config_path')},
+                {'enable_legacy_asr_mode': LaunchConfiguration('enable_legacy_asr_mode')},
+                {'enable_planner_mode': LaunchConfiguration('enable_planner_mode')},
+            ],
+            remappings=[
+                ('/llm_response', 'llm_response'),
+                ('/planner/llm_request', 'planner/llm_request'),
+                ('/planner/llm_response', 'planner/llm_response'),
+            ],
+            env={
+                'LD_LIBRARY_PATH': f'{xlm_sdk_root}/lib:/opt/ros/humble/lib:$LD_LIBRARY_PATH',
+                'ROS_HOME': '/home/sunrise/.ros',
+                'ROS_LOG_DIR': '/home/sunrise/.ros/log',
+            },
+        ),
+    ])
+

brain/OmniNode/src/OmniNode/package.xml

@@ -0,0 +1,25 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>omni_node</name>
+  <version>1.0.0</version>
+  <description>ROS2 node for Horizon xlm Omni multimodal LLM inference</description>
+  <maintainer email="user@example.com">user</maintainer>
+  <license>Apache-2.0</license>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+  <buildtool_depend>ament_cmake_auto</buildtool_depend>
+
+  <depend>rclcpp</depend>
+  <depend>std_msgs</depend>
+  <depend>rcl_interfaces</depend>
+  <depend>nlohmann_json</depend>
+
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>ament_lint_common</test_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>
+

brain/OmniNode/src/OmniNode/src/main.cpp

@@ -0,0 +1,11 @@
+#include <rclcpp/rclcpp.hpp>
+#include "omni_node/omni_node.hpp"
+
+int main(int argc, char** argv) {
+  rclcpp::init(argc, argv);
+  auto node = std::make_shared<omni_node::OmniNode>(rclcpp::NodeOptions());
+  rclcpp::spin(node);
+  rclcpp::shutdown();
+  return 0;
+}
+

brain/OmniNode/src/OmniNode/src/omni_engine.cpp

@@ -0,0 +1,970 @@
+#include "omni_node/omni_engine.hpp"
+
+#include <algorithm>
+#include <cstring>
+#include <ctime>
+#include <cstdlib>
+#include <fstream>
+#include <iostream>
+#include <iterator>
+#include <stdexcept>
+#include <unordered_map>
+#include <sstream>
+#include <iomanip>
+#include <regex>
+
+#include <nlohmann/json.hpp>
+
+using json = nlohmann::json;
+
+namespace {
+
+const std::vector<std::string> kRequiredFields = {
+    "visual_hbm_path",
+    "audio_hbm_path",
+    "text_hbm_path",
+    "embed_tokens",
+    "tokenizer_dir",
+    "model_type",
+    "online_mode",
+};
+
+std::string WritePromptJsonToFile(const std::string& json_str) {
+  std::ostringstream ss;
+  ss << "/tmp/omni_prompt_" << std::hex << std::time(nullptr) << "_" << rand() << ".json";
+  std::string path = ss.str();
+  std::ofstream ofs(path);
+  if (!ofs.is_open()) {
+    std::cerr << "[OmniEngine] Failed to write temp prompt file: " << path << std::endl;
+    return "";
+  }
+  ofs << json_str;
+  ofs.close();
+  return path;
+}
+
+std::string Trim(const std::string& s) {
+  size_t start = s.find_first_not_of(" \t\n\r");
+  if (start == std::string::npos) return "";
+  size_t end = s.find_last_not_of(" \t\n\r");
+  return s.substr(start, end - start + 1);
+}
+
+std::string GetPlanId() {
+  std::ostringstream ss;
+  ss << "plan_" << std::hex << std::time(nullptr) << "_" << rand();
+  return ss.str();
+}
+
+bool JsonArrayEmpty(const json& arr) {
+  return !arr.is_array() || arr.empty();
+}
+
+bool JsonObjectEmpty(const json& obj) {
+  return !obj.is_object() || obj.empty();
+}
+
+}  // namespace
+
+OmniEngine* OmniEngine::current_instance_ = nullptr;
+
+OmniEngine::OmniEngine(const std::string& config_path)
+    : config_path_(config_path),
+      handle_(nullptr),
+      current_mode_(Mode::kLegacyDialog),
+      planner_separator_seen_(false),
+      planner_stream_emitted_len_(0) {
+  current_instance_ = this;
+}
+
+OmniEngine::~OmniEngine() {
+  if (handle_ != nullptr) {
+    xlm_destroy(&handle_);
+    handle_ = nullptr;
+  }
+  if (current_instance_ == this) {
+    current_instance_ = nullptr;
+  }
+}
+
+bool OmniEngine::Init() {
+  std::ifstream config_file(config_path_);
+  if (!config_file.is_open()) {
+    std::cerr << "[OmniEngine] Failed to open config file: " << config_path_ << std::endl;
+    return false;
+  }
+
+  json config;
+  try {
+    config_file >> config;
+  } catch (const std::exception& e) {
+    std::cerr << "[OmniEngine] Failed to parse JSON: " << e.what() << std::endl;
+    return false;
+  }
+
+  for (const auto& field : kRequiredFields) {
+    if (!config.contains(field)) {
+      std::cerr << "[OmniEngine] Missing required field: " << field << std::endl;
+      return false;
+    }
+  }
+
+  config_.visual_hbm_path = config["visual_hbm_path"];
+  config_.audio_hbm_path = config["audio_hbm_path"];
+  config_.text_hbm_path = config["text_hbm_path"];
+  config_.embed_tokens = config["embed_tokens"];
+  config_.tokenizer_dir = config["tokenizer_dir"];
+  config_.model_type = config["model_type"];
+  config_.online_mode = config["online_mode"].get<bool>();
+  config_.bpu_core = config.value("bpu_core", -1);
+  config_.system_prompt = config.value("system_prompt",
+      "You are Qwen, a virtual human developed by the Qwen Team, Alibaba "
+      "Group, capable of perceiving auditory and visual inputs, as well as "
+      "generating text and speech.");
+
+  config_.enable_legacy_asr_mode = config.value("enable_legacy_asr_mode", true);
+  config_.enable_planner_mode = config.value("enable_planner_mode", true);
+  config_.dialog_system_prompt = config.value("dialog_system_prompt",
+      "你是一个机器人助手，负责与用户自然对话。");
+  config_.planner_system_prompt = config.value("planner_system_prompt",
+      "你是机器人智能体。任务：1. 先给用户输出一段自然语言回复（可直接播报）2. 然后输出 <FINAL_JSON> 分隔符 3. 最后输出严格 JSON 对象（包含 plan）");
+  config_.model_response_separator = config.value("model_response_separator", "<FINAL_JSON>");
+  config_.fallback_response_message = config.value("fallback_response_message",
+      "抱歉，我没有完全理解您的请求，请再说一次。");
+
+  // 加载 planner_rules 配置
+  if (config.contains("planner_rules") && config["planner_rules"].is_object()) {
+    auto& rules = config["planner_rules"];
+    config_.planner_output_protocol = rules.value("output_protocol", "=== 输出协议 ===\n1. 自然语言回复\n2. <FINAL_JSON>\n3. JSON 对象\n");
+    config_.planner_input_classification = rules.value("input_classification", "=== 输入分类 ===\nA. 身份/闲聊 → speak\nB. 状态查询 → speak/query_world\nC. 明确任务 → execute\nD. 缺信息 → ask_user\n");
+    config_.planner_ask_user_rules = rules.value("ask_user_rules", "=== ask_user 规则 ===\n- 直接问，不说\"我将执行\"\n");
+    config_.planner_capability_rules = rules.value("capability_question_rules", "=== 能力说明类问题 ===\n- 基于可用能力说明回答\n- 用 speak 回答\n");
+
+    if (rules.contains("example1") && rules["example1"].is_object()) {
+      config_.planner_example1_user = rules["example1"].value("user_input", "你是谁");
+      config_.planner_example1_response = rules["example1"].value("natural_response", "我是机器人助手。");
+      config_.planner_example1_json = rules["example1"].value("json", "");
+    }
+
+    if (rules.contains("example2") && rules["example2"].is_object()) {
+      config_.planner_example2_user = rules["example2"].value("user_input", "打开风扇");
+      config_.planner_example2_response = rules["example2"].value("natural_response", "请问您想打开哪个房间的风扇？");
+      config_.planner_example2_json = rules["example2"].value("json", "");
+    }
+
+    config_.planner_prohibited = rules.value("prohibited", "禁止：markdown、解释文字、省略 JSON。");
+    std::cout << "[OmniEngine] planner_rules loaded from config" << std::endl;
+  }
+
+  std::cout << "[OmniEngine] Config loaded:" << std::endl;
+  std::cout << "  enable_legacy_asr_mode: " << (config_.enable_legacy_asr_mode ? "true" : "false") << std::endl;
+  std::cout << "  enable_planner_mode: " << (config_.enable_planner_mode ? "true" : "false") << std::endl;
+
+  if (!config_.enable_legacy_asr_mode && !config_.enable_planner_mode) {
+    std::cerr << "[OmniEngine] FATAL: Both legacy_asr_mode and planner_mode are disabled!" << std::endl;
+    std::cerr << "[OmniEngine] Please enable at least one mode in config file." << std::endl;
+    return false;
+  }
+
+  xlm_common_params_t param = xlm_create_default_param();
+  param.omni_visual_model_path = config_.visual_hbm_path.c_str();
+  param.omni_audio_model_path = config_.audio_hbm_path.c_str();
+  param.omni_text_model_path = config_.text_hbm_path.c_str();
+  param.embed_tokens = config_.embed_tokens.c_str();
+  param.token_config_path = config_.tokenizer_dir.c_str();
+  param.model_type = static_cast<xlm_model_type>(config_.model_type);
+  param.omni_online_mode = config_.online_mode;
+
+  int32_t ret = xlm_init(&param, StaticCallback, &handle_);
+  if (ret != 0) {
+    std::cerr << "[OmniEngine] xlm_init failed with code: " << ret << std::endl;
+    return false;
+  }
+
+  std::cout << "[OmniEngine] xlm init success" << std::endl;
+  return true;
+}
+
+void OmniEngine::SetStreamCallback(StreamCallback callback) {
+  stream_callback_ = std::move(callback);
+}
+
+void OmniEngine::SetPlannerResponseCallback(PlannerResponseCallback callback) {
+  planner_response_callback_ = std::move(callback);
+}
+
+std::string OmniEngine::BuildLegacyPrompt(const std::string& user_text) {
+  std::ostringstream prompt;
+  prompt << config_.dialog_system_prompt << "\n\n";
+  prompt << "当前用户输入：\n" << user_text << "\n\n";
+  prompt << "请直接回复自然语言。";
+  return prompt.str();
+}
+
+bool OmniEngine::ParsePlannerRequest(const std::string& json_request,
+                                      std::string& request_id,
+                                      std::string& session_id,
+                                      std::string& user_text,
+                                      std::string& memory_text,
+                                      std::string& mode,
+                                      std::string& planner_mode,
+                                      json& world_snapshot,
+                                      json& available_tools,
+                                      json& tool_descriptions,
+                                      json& domain_rules,
+                                      json& current_state) {
+  try {
+    auto j = json::parse(json_request);
+
+    request_id = j.value("request_id", "unknown");
+    session_id = j.value("session_id", "default");
+    user_text = j.value("user_text", "");
+    memory_text = j.value("memory_text", "");  // 新增：解析记忆上下文
+    mode = j.value("mode", "dialog_and_decision");
+    planner_mode = j.value("planner_mode", "hybrid");
+
+    if (j.contains("world_snapshot")) {
+      // 支持 object 或 string 类型
+      if (j["world_snapshot"].is_object()) {
+        world_snapshot = j["world_snapshot"];
+      } else if (j["world_snapshot"].is_string()) {
+        // string 类型直接存储为字符串形式的 JSON
+        world_snapshot = json::object({{"_text", j["world_snapshot"].get<std::string>()}});
+      } else {
+        world_snapshot = json::object();
+        std::cerr << "[OmniEngine] WARNING: world_snapshot exists but is not object/string, fallback to {}" << std::endl;
+      }
+    } else {
+      world_snapshot = json::object();
+    }
+
+    if (j.contains("available_tools")) {
+      if (j["available_tools"].is_array()) {
+        available_tools = j["available_tools"];
+      } else {
+        available_tools = json::array();
+        std::cerr << "[OmniEngine] WARNING: available_tools exists but is not array, fallback to []" << std::endl;
+      }
+    } else {
+      available_tools = json::array();
+    }
+
+    // 新增：解析 tool_descriptions
+    if (j.contains("tool_descriptions")) {
+      if (j["tool_descriptions"].is_array()) {
+        tool_descriptions = j["tool_descriptions"];
+        std::cerr << "[OmniEngine] ParsePlannerRequest: tool_descriptions count=" << tool_descriptions.size() << std::endl;
+      } else {
+        tool_descriptions = json::array();
+        std::cerr << "[OmniEngine] WARNING: tool_descriptions exists but is not array, fallback to []" << std::endl;
+      }
+    } else {
+      tool_descriptions = json::array();
+      std::cerr << "[OmniEngine] WARNING: tool_descriptions not found, fallback to []" << std::endl;
+    }
+
+    if (j.contains("domain_rules")) {
+      if (j["domain_rules"].is_object()) {
+        domain_rules = j["domain_rules"];
+      } else {
+        domain_rules = json::object();
+        std::cerr << "[OmniEngine] WARNING: domain_rules exists but is not object, fallback to {}" << std::endl;
+      }
+    } else {
+      domain_rules = json::object();
+    }
+
+    if (j.contains("current_state")) {
+      if (j["current_state"].is_object()) {
+        current_state = j["current_state"];
+      } else {
+        current_state = json::object();
+        std::cerr << "[OmniEngine] WARNING: current_state exists but is not object, fallback to {}" << std::endl;
+      }
+    } else {
+      current_state = json::object();
+    }
+
+    return true;
+  } catch (const json::parse_error& e) {
+    std::cerr << "[OmniEngine] JSON parse error: " << e.what() << std::endl;
+    return false;
+  } catch (const std::exception& e) {
+    std::cerr << "[OmniEngine] Failed to parse planner request: " << e.what() << std::endl;
+    return false;
+  }
+}
+
+// BuildToolDescriptions - 优先使用 tool_descriptions，生成可用能力说明
+std::string OmniEngine::BuildToolDescriptions(const json& tool_descriptions) {
+  std::ostringstream desc;
+
+  // 如果 tool_descriptions 非空，使用 plannerNode 提供的语义
+  if (tool_descriptions.is_array() && !tool_descriptions.empty()) {
+    desc << "=== 可用能力说明 ===\n";
+    for (const auto& item : tool_descriptions) {
+      if (!item.is_object()) continue;
+
+      std::string name = item.value("name", "");
+      if (name.empty()) continue;
+
+      std::string description = item.value("description", "(未定义)");
+      std::string tool_call_type = item.value("tool_call_type", "execute");
+      std::string category = item.value("category", "action");
+
+      desc << "- " << name << ": " << description
+           << " (tool_call_type=" << tool_call_type
+           << ", category=" << category << ")\n";
+    }
+    std::cerr << "[OmniEngine] BuildToolDescriptions: using plannerNode provided descriptions, count="
+              << tool_descriptions.size() << std::endl;
+    return desc.str();
+  }
+
+  // tool_descriptions 为空时，返回空描述
+  std::cerr << "[OmniEngine] BuildToolDescriptions: tool_descriptions empty" << std::endl;
+  return "";
+}
+
+// BuildPlannerSystemPrompt - 以 config_.planner_system_prompt 为主体，使用配置中的规则
+std::string OmniEngine::BuildPlannerSystemPrompt(const json& request_data) {
+  std::ostringstream system_prompt;
+
+  // 主体：使用 config 中的 planner_system_prompt
+  system_prompt << config_.planner_system_prompt << "\n\n";
+
+  // 补充规则：使用配置中的 planner_rules
+  if (!config_.planner_output_protocol.empty()) {
+    system_prompt << config_.planner_output_protocol << "\n";
+  }
+
+  if (!config_.planner_input_classification.empty()) {
+    system_prompt << config_.planner_input_classification << "\n";
+  }
+
+  if (!config_.planner_ask_user_rules.empty()) {
+    system_prompt << config_.planner_ask_user_rules << "\n";
+  }
+
+  if (!config_.planner_capability_rules.empty()) {
+    system_prompt << config_.planner_capability_rules << "\n";
+  }
+
+  // 示例1
+  if (!config_.planner_example1_user.empty()) {
+    system_prompt << "=== 示例1 ===\n";
+    system_prompt << "用户：" << config_.planner_example1_user << "\n";
+    system_prompt << config_.planner_example1_response << "\n";
+    system_prompt << config_.model_response_separator << "\n";
+    system_prompt << config_.planner_example1_json << "\n\n";
+  }
+
+  // 示例2
+  if (!config_.planner_example2_user.empty()) {
+    system_prompt << "=== 示例2 ===\n";
+    system_prompt << "用户：" << config_.planner_example2_user << "\n";
+    system_prompt << config_.planner_example2_response << "\n";
+    system_prompt << config_.model_response_separator << "\n";
+    system_prompt << config_.planner_example2_json << "\n\n";
+  }
+
+  if (!config_.planner_prohibited.empty()) {
+    system_prompt << config_.planner_prohibited << "\n";
+  }
+
+  std::cerr << "[OmniEngine] BuildPlannerSystemPrompt: base prompt len=" << config_.planner_system_prompt.length()
+            << ", total len=" << system_prompt.str().length() << std::endl;
+
+  return system_prompt.str();
+}
+
+// BuildPlannerUserContext - 使用 tool_descriptions 优先，支持 memory_text
+std::string OmniEngine::BuildPlannerUserContext(const json& request_data) {
+  std::string request_id = request_data.value("request_id", "unknown");
+  std::string session_id = request_data.value("session_id", "default");
+  std::string user_text = request_data.value("user_text", "");
+  std::string memory_text = request_data.value("memory_text", "");  // 新增：记忆上下文
+  std::string mode = request_data.value("mode", "dialog_and_decision");
+  std::string planner_mode = request_data.value("planner_mode", "hybrid");
+
+  json world_snapshot = request_data.value("world_snapshot", json::object());
+  json available_tools = request_data.value("available_tools", json::array());
+  json tool_descriptions = request_data.value("tool_descriptions", json::array());
+  json domain_rules = request_data.value("domain_rules", json::object());
+  json current_state = request_data.value("current_state", json::object());
+
+  // 支持字符串或 object 类型的 world_snapshot
+  std::string world_snapshot_text;
+  if (world_snapshot.contains("_text")) {
+    world_snapshot_text = world_snapshot["_text"].get<std::string>();
+  } else if (world_snapshot.is_object()) {
+    world_snapshot_text = world_snapshot.dump(2);
+  } else {
+    world_snapshot_text = "{}";
+  }
+  std::string domain_rules_text = domain_rules.is_object() ? domain_rules.dump(2) : "{}";
+  std::string current_state_text = current_state.is_object() ? current_state.dump(2) : "{}";
+
+  // 使用 tool_descriptions 生成能力说明
+  std::string tools_desc = BuildToolDescriptions(tool_descriptions);
+
+  std::ostringstream user_context;
+
+  user_context << "=== 当前请求 ===\n";
+  user_context << "request_id: " << request_id << "\n";
+  user_context << "session_id: " << session_id << "\n";
+  user_context << "mode: " << mode << ", planner_mode: " << planner_mode << "\n";
+  user_context << "用户输入: " << user_text << "\n\n";
+
+  // 记忆上下文（来自 MemoryNode）
+  if (!memory_text.empty()) {
+    user_context << "=== 记忆上下文 ===\n";
+    user_context << memory_text << "\n\n";
+    std::cerr << "[OmniEngine] BuildPlannerUserContext: memory_text length=" << memory_text.length() << std::endl;
+  }
+
+  user_context << "=== 世界状态 ===\n";
+  user_context << world_snapshot_text << "\n\n";
+
+  user_context << "=== 规则约束 ===\n";
+  user_context << domain_rules_text << "\n\n";
+
+  user_context << "=== 系统状态 ===\n";
+  user_context << current_state_text << "\n\n";
+
+  user_context << "=== 可用能力说明 ===\n";
+  user_context << tools_desc << "\n";
+
+  std::cerr << "[OmniEngine] BuildPlannerUserContext: tool_descriptions count="
+            << tool_descriptions.size() << ", available_tools count=" << available_tools.size()
+            << ", memory_text length=" << memory_text.length() << std::endl;
+
+  return user_context.str();
+}
+
+std::string OmniEngine::ExtractJsonFromResponse(const std::string& full_text) {
+  std::string json_str;
+
+  // 第一优先级：严格按 <FINAL_JSON> 后面的内容解析
+  size_t sep_pos = full_text.rfind(config_.model_response_separator);
+  if (sep_pos != std::string::npos) {
+    json_str = full_text.substr(sep_pos + config_.model_response_separator.length());
+    json_str = Trim(json_str);
+
+    if (!json_str.empty()) {
+      try {
+        auto j = json::parse(json_str);
+        std::cerr << "[OmniEngine] ExtractJson: separator extraction success, json_len=" << json_str.length() << std::endl;
+        return json_str;
+      } catch (...) {
+        std::cerr << "[OmniEngine] ExtractJson: separator found but parse failed" << std::endl;
+      }
+    }
+  }
+
+  // 第二优先级：从最后一个完整 JSON 对象兜底提取
+  // 从后往前找最后一段合理 JSON
+  size_t last_brace = full_text.rfind('{');
+  if (last_brace != std::string::npos) {
+    // 从 last_brace 开始向前扩展，尝试找到完整的 JSON 对象
+    size_t search_start = last_brace;
+    // 向前查找可能更早的 {，但只取最后一段完整 JSON
+    size_t first_brace_candidate = search_start;
+
+    // 尝试从 last_brace 开始匹配完整的 JSON
+    size_t brace_count = 0;
+    size_t json_start = last_brace;
+    size_t json_end = std::string::npos;
+
+    for (size_t i = last_brace; i < full_text.length(); ++i) {
+      if (full_text[i] == '{') brace_count++;
+      else if (full_text[i] == '}') brace_count--;
+      if (brace_count == 0) {
+        json_end = i + 1;
+        break;
+      }
+    }
+
+    if (json_end != std::string::npos) {
+      std::string potential_json = full_text.substr(json_start, json_end - json_start);
+      try {
+        auto j = json::parse(potential_json);
+        if (j.contains("request_id") || j.contains("plan")) {
+          json_str = potential_json;
+          std::cerr << "[OmniEngine] ExtractJson: fallback brace extraction success, json_len=" << json_str.length() << std::endl;
+          return json_str;
+        }
+      } catch (...) {
+        std::cerr << "[OmniEngine] ExtractJson: fallback extraction failed" << std::endl;
+      }
+    }
+  }
+
+  std::cerr << "[OmniEngine] ExtractJson: extraction failed, will use fallback" << std::endl;
+  return "";
+}
+
+std::string OmniEngine::BuildFallbackJson() {
+  json fallback;
+  fallback["request_id"] = current_request_id_;
+  fallback["session_id"] = current_session_id_;
+  fallback["plan"] = json::object();
+  fallback["plan"]["plan_id"] = GetPlanId();
+  fallback["plan"]["goal"] = "无法可靠解析模型输出";
+  fallback["plan"]["reasoning"] = "LLM 输出 JSON 解析失败，返回安全 fallback";
+  fallback["plan"]["risk_level"] = "low";
+  fallback["plan"]["requires_confirmation"] = false;
+  fallback["plan"]["confirmation_message"] = nullptr;
+  fallback["plan"]["steps"] = json::array();
+
+  json step;
+  step["step_id"] = 1;
+  step["action"] = "ask_user";
+  step["tool_call_type"] = "ask_user";
+  step["parameters"] = json::object();
+  step["parameters"]["question"] = config_.fallback_response_message;
+  step["preconditions"] = json::object();
+  step["fallback"] = nullptr;
+  step["status"] = "pending";
+  step["description"] = "fallback: 请求用户重新描述";
+  step["requires_confirmation"] = false;
+  step["confirmation_message"] = nullptr;
+  step["metadata"] = json::object();
+  step["metadata"]["fallback_reason"] = "json_parse_failed";
+
+  fallback["plan"]["steps"].push_back(step);
+  fallback["plan"]["status"] = "created";
+  fallback["plan"]["source"] = "omni_fallback";
+  fallback["plan"]["metadata"] = json::object();
+  fallback["plan"]["metadata"]["response_type"] = "fallback";
+  fallback["plan"]["metadata"]["fallback"] = true;
+
+  std::cerr << "[OmniEngine] Fallback triggered: request_id=" << current_request_id_
+            << ", session_id=" << current_session_id_
+            << ", reason=json_parse_failed" << std::endl;
+
+  return fallback.dump(2);
+}
+
+void OmniEngine::ExecuteFeed(const std::string& prompt, Mode mode) {
+  accumulated_text_.clear();
+
+  // 重置 planner 流式状态
+  planner_separator_seen_ = false;
+  planner_stream_emitted_len_ = 0;
+
+  if (config_.online_mode) {
+    omni_online_text_t text_input;
+    text_input.system_text = prompt.c_str();
+    text_input.user_text = "";
+
+    int ret = xlm_omni_feed_text_online(handle_, text_input);
+    if (ret != 0) {
+      std::cerr << "[OmniEngine] xlm_omni_feed_text_online failed: " << ret << std::endl;
+      fflush(stderr);
+      return;
+    }
+  } else {
+    json conversation = json::array();
+
+    json system_msg;
+    system_msg["role"] = "system";
+    system_msg["content"] = json::array();
+    system_msg["content"].push_back({
+      {"type", "text"},
+      {"text", prompt}
+    });
+    conversation.push_back(system_msg);
+
+    json user_msg;
+    user_msg["role"] = "user";
+    user_msg["content"] = json::array();
+    user_msg["content"].push_back({
+      {"type", "text"},
+      {"text", ""}
+    });
+    conversation.push_back(user_msg);
+
+    json root;
+    root["conversation"] = conversation;
+    pending_json_ = root.dump();
+    pending_json_file_ = WritePromptJsonToFile(pending_json_);
+    if (pending_json_file_.empty()) {
+      std::cerr << "[OmniEngine] Failed to create prompt json file" << std::endl;
+      return;
+    }
+  }
+
+  pending_requests_.resize(1);
+  auto& request = pending_requests_[0];
+  memset(&request, 0, sizeof(xlm_lm_request_t));
+  request.type = XLM_INPUT_PROMPT;
+  request.system_prompt = nullptr;
+  request.prompt_json = config_.online_mode ? nullptr : pending_json_file_.c_str();
+  if (config_.bpu_core == -1) {
+    request.infer_backend = XLM_INFER_BACKEND_BPU_ANY;
+  } else {
+    // 直接使用 bpu_core 作为 backend 值
+    request.infer_backend = static_cast<xlm_infer_backend>(config_.bpu_core);
+  }
+
+  xlm_input_s input;
+  memset(&input, 0, sizeof(xlm_input_s));
+  input.request_num = 1;
+  input.requests = pending_requests_.data();
+
+  int ret = xlm_omni(handle_, &input, nullptr);
+  std::cerr << "[OmniEngine] xlm_omni returned: " << ret << std::endl;
+  fflush(stderr);
+  if (ret != 0) {
+    std::cerr << "[OmniEngine] xlm_omni failed: " << ret << std::endl;
+    fflush(stderr);
+  }
+}
+
+void OmniEngine::ExecutePlannerFeed(const std::string& system_prompt, const std::string& user_context, Mode mode) {
+  accumulated_text_.clear();
+
+  // 重置 planner 流式状态
+  planner_separator_seen_ = false;
+  planner_stream_emitted_len_ = 0;
+
+  std::cerr << "[OmniEngine] ExecutePlannerFeed: system_prompt_len=" << system_prompt.length()
+            << ", user_context_len=" << user_context.length() << std::endl;
+
+  if (config_.online_mode) {
+    // online 模式：合并 system + user 作为完整 prompt
+    std::string combined = system_prompt + "\n" + user_context;
+    omni_online_text_t text_input;
+    text_input.system_text = combined.c_str();
+    text_input.user_text = "";
+
+    int ret = xlm_omni_feed_text_online(handle_, text_input);
+    if (ret != 0) {
+      std::cerr << "[OmniEngine] ExecutePlannerFeed: xlm_omni_feed_text_online failed: " << ret << std::endl;
+      fflush(stderr);
+      return;
+    }
+  } else {
+    // offline 模式：分 system/user message
+    json conversation = json::array();
+
+    // system message：规则与协议
+    json system_msg;
+    system_msg["role"] = "system";
+    system_msg["content"] = json::array();
+    system_msg["content"].push_back({
+      {"type", "text"},
+      {"text", system_prompt}
+    });
+    conversation.push_back(system_msg);
+
+    // user message：当前请求上下文
+    json user_msg;
+    user_msg["role"] = "user";
+    user_msg["content"] = json::array();
+    user_msg["content"].push_back({
+      {"type", "text"},
+      {"text", user_context}
+    });
+    conversation.push_back(user_msg);
+
+    json root;
+    root["conversation"] = conversation;
+    pending_json_ = root.dump();
+    pending_json_file_ = WritePromptJsonToFile(pending_json_);
+    if (pending_json_file_.empty()) {
+      std::cerr << "[OmniEngine] ExecutePlannerFeed: Failed to create prompt json file" << std::endl;
+      return;
+    }
+  }
+
+  pending_requests_.resize(1);
+  auto& request = pending_requests_[0];
+  memset(&request, 0, sizeof(xlm_lm_request_t));
+  request.type = XLM_INPUT_PROMPT;
+  request.system_prompt = nullptr;
+  request.prompt_json = config_.online_mode ? nullptr : pending_json_file_.c_str();
+  if (config_.bpu_core == -1) {
+    request.infer_backend = XLM_INFER_BACKEND_BPU_ANY;
+  } else {
+    // 直接使用 bpu_core 作为 backend 值
+    request.infer_backend = static_cast<xlm_infer_backend>(config_.bpu_core);
+  }
+
+  xlm_input_s input;
+  memset(&input, 0, sizeof(xlm_input_s));
+  input.request_num = 1;
+  input.requests = pending_requests_.data();
+
+  int ret = xlm_omni(handle_, &input, nullptr);
+  std::cerr << "[OmniEngine] ExecutePlannerFeed: xlm_omni returned: " << ret << std::endl;
+  fflush(stderr);
+  if (ret != 0) {
+    std::cerr << "[OmniEngine] ExecutePlannerFeed: xlm_omni failed: " << ret << std::endl;
+    fflush(stderr);
+  }
+}
+
+void OmniEngine::FeedText(const std::string& user_text) {
+  if (!config_.enable_legacy_asr_mode) {
+    std::cerr << "[OmniEngine] Legacy ASR mode is disabled, ignoring FeedText" << std::endl;
+    return;
+  }
+
+  current_mode_ = Mode::kLegacyDialog;
+  current_request_id_.clear();
+  current_session_id_.clear();
+
+  std::cerr << "[OmniEngine] [Legacy Mode] Processing user text: " << user_text << std::endl;
+
+  std::string prompt = BuildLegacyPrompt(user_text);
+  ExecuteFeed(prompt, Mode::kLegacyDialog);
+}
+
+void OmniEngine::FeedPlannerRequest(const std::string& json_request) {
+  if (!config_.enable_planner_mode) {
+    std::cerr << "[OmniEngine] Planner mode is disabled, ignoring FeedPlannerRequest" << std::endl;
+    return;
+  }
+
+  std::string request_id, session_id, user_text, memory_text, mode, planner_mode;
+  json world_snapshot, available_tools, tool_descriptions, domain_rules, current_state;
+
+  bool parse_success = ParsePlannerRequest(json_request, request_id, session_id,
+                                           user_text, memory_text, mode, planner_mode,
+                                           world_snapshot, available_tools,
+                                           tool_descriptions,
+                                           domain_rules, current_state);
+
+  if (!parse_success) {
+    std::cerr << "[OmniEngine] [Planner Mode] Parse failed, using fallback for request parsing" << std::endl;
+  }
+
+  current_mode_ = Mode::kPlannerDialog;
+  current_request_id_ = request_id;
+  current_session_id_ = session_id;
+
+  std::cerr << "[OmniEngine] [Planner Mode] Processing request_id: " << request_id
+            << ", session_id: " << session_id << ", user_text: " << user_text << std::endl;
+  std::cerr << "[OmniEngine] [Planner Mode] tool_descriptions count: " << tool_descriptions.size()
+            << ", memory_text length: " << memory_text.length() << std::endl;
+
+  json request_data;
+  request_data["request_id"] = request_id;
+  request_data["session_id"] = session_id;
+  request_data["user_text"] = user_text;
+  request_data["memory_text"] = memory_text;  // 新增：记忆上下文
+  request_data["mode"] = mode;
+  request_data["planner_mode"] = planner_mode;
+  request_data["world_snapshot"] = world_snapshot;
+  request_data["available_tools"] = available_tools;
+  request_data["tool_descriptions"] = tool_descriptions;
+  request_data["domain_rules"] = domain_rules;
+  request_data["current_state"] = current_state;
+
+  std::string system_prompt = BuildPlannerSystemPrompt(request_data);
+  std::string user_context = BuildPlannerUserContext(request_data);
+  ExecutePlannerFeed(system_prompt, user_context, Mode::kPlannerDialog);
+}
+
+void OmniEngine::StaticCallback(xlm_result_s* result, xlm_state_e state, void* /* userdata */) {
+  if (current_instance_ == nullptr) {
+    return;
+  }
+  current_instance_->HandleCallback(result, state);
+}
+
+void OmniEngine::HandleCallback(xlm_result_s* result, xlm_state_e state) {
+  std::string text;
+
+  if (result != nullptr && result->text != nullptr) {
+    std::string raw = result->text;
+
+    if (!raw.empty() && raw.front() == '{') {
+      try {
+        auto j = json::parse(raw);
+        if (j.is_object() && j.contains("text") && !j["text"].is_null()) {
+          text = j["text"].get<std::string>();
+        } else {
+          text = raw;
+        }
+      } catch (...) {
+        text = raw;
+      }
+    } else {
+      text = raw;
+    }
+  }
+
+  if (state == XLM_STATE_END) {
+    std::cerr << "[OmniEngine] Inference END, mode=" << (current_mode_ == Mode::kPlannerDialog ? "planner" : "legacy")
+              << ", accumulated_text length: " << accumulated_text_.length() << std::endl;
+
+    // Planner mode 下打印完整 accumulated_text_ 便于调试
+    if (current_mode_ == Mode::kPlannerDialog) {
+      std::cerr << "[OmniEngine] Full accumulated_text (planner mode):\n" << accumulated_text_ << std::endl;
+    }
+
+    if (current_mode_ == Mode::kPlannerDialog) {
+      std::cerr << "[OmniEngine] XLM_STATE_END in Planner mode, NOT calling stream_callback_" << std::endl;
+      // 注意：新的 prefix guard 机制会在遇到 <FINAL 时停止发送
+      // 推理结束时，accumulated_text_ 包含完整响应（包括分隔符和 JSON）
+      // 无需再发送剩余内容
+
+      std::string json_response = ExtractJsonFromResponse(accumulated_text_);
+
+      if (json_response.empty()) {
+        std::cerr << "[OmniEngine] JSON extraction failed, using fallback" << std::endl;
+        json_response = BuildFallbackJson();
+      } else {
+        try {
+          auto j = json::parse(json_response);
+
+          // 补齐顶层字段
+          if (!j.contains("request_id") || j["request_id"].is_null() ||
+              j["request_id"].get<std::string>().empty()) {
+            j["request_id"] = current_request_id_;
+          }
+          if (!j.contains("session_id") || j["session_id"].is_null() ||
+              j["session_id"].get<std::string>().empty()) {
+            j["session_id"] = current_session_id_;
+          }
+
+          // 补齐 plan 层字段
+          if (!j.contains("plan") || !j["plan"].is_object()) {
+            j["plan"] = json::object();
+          }
+          auto& plan = j["plan"];
+          if (!plan.contains("status") || plan["status"].is_null()) {
+            plan["status"] = "created";
+          }
+          if (!plan.contains("source") || plan["source"].is_null()) {
+            plan["source"] = "llm";
+          }
+          if (!plan.contains("metadata") || !plan["metadata"].is_object()) {
+            plan["metadata"] = json::object();
+          }
+
+          // 补齐每个 step 的字段
+          if (plan.contains("steps") && plan["steps"].is_array()) {
+            for (auto& step : plan["steps"]) {
+              if (!step.contains("preconditions") || !step["preconditions"].is_object()) {
+                step["preconditions"] = json::object();
+              }
+              if (!step.contains("fallback") || step["fallback"].is_null()) {
+                step["fallback"] = nullptr;
+              }
+              if (!step.contains("status") || step["status"].is_null()) {
+                step["status"] = "pending";
+              }
+              if (!step.contains("requires_confirmation")) {
+                step["requires_confirmation"] = false;
+              }
+              if (!step.contains("confirmation_message") || step["confirmation_message"].is_null()) {
+                step["confirmation_message"] = nullptr;
+              }
+              if (!step.contains("metadata") || !step["metadata"].is_object()) {
+                step["metadata"] = json::object();
+              }
+            }
+          }
+
+          json_response = j.dump(2);
+        } catch (...) {
+          std::cerr << "[OmniEngine] JSON normalization failed, using fallback" << std::endl;
+          json_response = BuildFallbackJson();
+        }
+      }
+
+      std::cerr << "[OmniEngine] Publishing planner response: request_id="
+                << current_request_id_ << ", is_fallback="
+                << (json_response.find("omni_fallback") != std::string::npos ? "true" : "false")
+                << ", json_length=" << json_response.length() << std::endl;
+
+      if (planner_response_callback_) {
+        planner_response_callback_(json_response, true);
+      }
+    } else if (current_mode_ == Mode::kLegacyDialog) {
+      // Legacy Dialog 模式结束时，发送空消息作为流结束标记
+      if (stream_callback_) {
+        stream_callback_("", true);
+      }
+    }
+
+    accumulated_text_.clear();
+    // 重置 planner 流式状态
+    planner_separator_seen_ = false;
+    planner_stream_emitted_len_ = 0;
+
+  } else if (state == XLM_STATE_ERROR) {
+    std::cerr << "[OmniEngine] Inference error" << std::endl;
+
+    if (current_mode_ == Mode::kPlannerDialog) {
+      std::string json_response = BuildFallbackJson();
+      if (planner_response_callback_) {
+        planner_response_callback_(json_response, true);
+      }
+    }
+
+    if (stream_callback_) {
+      stream_callback_("[ERROR] Inference failed", true);
+    }
+
+    // 重置 planner 流式状态
+    planner_separator_seen_ = false;
+    planner_stream_emitted_len_ = 0;
+
+  } else {
+    if (!text.empty()) {
+      accumulated_text_ += text;
+
+      // Planner mode 流式处理：检测 "<" 分隔符
+      if (current_mode_ == Mode::kPlannerDialog && stream_callback_) {
+        // 如果已检测到分隔符，不再发送任何内容
+        if (planner_separator_seen_) {
+          // 停止发送
+        } else {
+          // 查找分隔符 "<"
+          size_t sep_pos = accumulated_text_.find('<');
+
+          if (sep_pos != std::string::npos) {
+            // 发现 "<"，发送之前未发送过的自然语言内容
+            size_t emit_start = planner_stream_emitted_len_;
+            size_t emit_end = sep_pos;
+
+            if (emit_end > emit_start) {
+              std::string to_emit = accumulated_text_.substr(emit_start, emit_end - emit_start);
+              if (!to_emit.empty()) {
+                // 去除末尾空白字符
+                size_t last_valid = to_emit.find_last_not_of(" \t\n\r");
+                if (last_valid != std::string::npos) {
+                  to_emit = to_emit.substr(0, last_valid + 1);
+                }
+                if (!to_emit.empty()) {
+                  stream_callback_(to_emit, false);
+                  std::cerr << "[OmniEngine] Planner emit len=" << to_emit.length()
+                            << " (stopped at < separator)" << std::endl;
+                }
+              }
+            }
+            // 更新已发送位置
+            planner_stream_emitted_len_ = emit_end;
+            // 设置标志：已检测到分隔符，停止发送后续内容
+            planner_separator_seen_ = true;
+          } else {
+            // 没有 "<"，发送 text
+            stream_callback_(text, false);
+            // 更新已发送位置为当前 accumulated_text_ 的长度
+            planner_stream_emitted_len_ = accumulated_text_.length();
+          }
+        }
+      }
+
+      // Legacy Dialog 模式流式输出
+      if (current_mode_ == Mode::kLegacyDialog && stream_callback_) {
+        stream_callback_(text, false);
+      }
+    }
+  }
+}

brain/OmniNode/src/OmniNode/src/omni_node.cpp

@@ -0,0 +1,158 @@
+#include "omni_node/omni_node.hpp"
+
+#include <iostream>
+
+namespace omni_node {
+
+OmniNode::OmniNode(rclcpp::NodeOptions options)
+    : Node("omni_node", options), engine_initialized_(false) {
+  // 步骤1：读取参数
+  this->declare_parameter<std::string>("config_path", "config/omni_config.json");
+  this->declare_parameter<bool>("enable_legacy_asr_mode", false);
+  this->declare_parameter<bool>("enable_planner_mode", true);
+
+  std::string config_path = this->get_parameter("config_path").as_string();
+  enable_legacy_asr_mode_ = this->get_parameter("enable_legacy_asr_mode").as_bool();
+  enable_planner_mode_ = this->get_parameter("enable_planner_mode").as_bool();
+
+  // 步骤2：创建 publisher（早于 engine 创建，避免 callback 时 publisher 未就绪）
+  pub_response_ = this->create_publisher<std_msgs::msg::String>("llm_response", 10);
+  if (enable_planner_mode_) {
+    pub_planner_response_ = this->create_publisher<std_msgs::msg::String>("planner/llm_response", 10);
+  }
+
+  RCLCPP_INFO(this->get_logger(), "Initializing OmniEngine with config: %s", config_path.c_str());
+
+  // 步骤3：创建 engine
+  engine_ = std::make_unique<OmniEngine>(config_path);
+
+  // 步骤4：注册 callback（此时 publisher 已就绪）
+  engine_->SetStreamCallback(
+      [this](const std::string& text, bool is_final) {
+        std_msgs::msg::String msg;
+        msg.data = text;
+        RCLCPP_INFO(this->get_logger(), "STREAM_CALLBACK: publishing %zu bytes, is_final=%d",
+                    text.length(), is_final);
+        this->pub_response_->publish(msg);
+        if (is_final) {
+          RCLCPP_INFO(this->get_logger(), "LLM stream completed");
+        }
+      });
+
+  engine_->SetPlannerResponseCallback(
+      [this](const std::string& json_response, bool is_final) {
+        if (is_final) {
+          std_msgs::msg::String msg;
+          msg.data = json_response;
+          this->pub_planner_response_->publish(msg);
+          RCLCPP_INFO(this->get_logger(), "Planner response published to /planner/llm_response");
+        }
+      });
+
+  // 步骤5：初始化 engine
+  bool init_ok = engine_->Init();
+  if (!init_ok) {
+    RCLCPP_ERROR(this->get_logger(), "OmniEngine init failed");
+    return;
+  }
+  engine_initialized_ = true;
+
+  // 同步 Node 层和 Engine 层的配置
+  engine_->SetEnableMode(enable_legacy_asr_mode_, enable_planner_mode_);
+
+  RCLCPP_INFO(this->get_logger(), "OmniNode mode configuration:");
+  RCLCPP_INFO(this->get_logger(), "  enable_legacy_asr_mode: %s", enable_legacy_asr_mode_ ? "true" : "false");
+  RCLCPP_INFO(this->get_logger(), "  enable_planner_mode: %s", enable_planner_mode_ ? "true" : "false");
+
+  if (!enable_legacy_asr_mode_ && !enable_planner_mode_) {
+    RCLCPP_ERROR(this->get_logger(), "FATAL: Both modes are disabled! Please enable at least one mode.");
+    RCLCPP_ERROR(this->get_logger(), "  Set enable_legacy_asr_mode:=true and/or enable_planner_mode:=true");
+    engine_initialized_ = false;
+    return;
+  }
+
+  // 步骤6：创建 subscriber（在 engine 初始化后）
+  if (enable_legacy_asr_mode_) {
+    sub_asr_ = this->create_subscription<std_msgs::msg::String>(
+        "asr", 10,
+        [this](const std_msgs::msg::String::SharedPtr msg) {
+          this->OnAsrReceived(msg);
+        });
+    RCLCPP_INFO(this->get_logger(), "Legacy ASR mode enabled - subscribed to /asr");
+  } else {
+    RCLCPP_INFO(this->get_logger(), "Legacy ASR mode disabled - /asr not subscribed");
+  }
+
+  if (enable_planner_mode_) {
+    sub_planner_request_ = this->create_subscription<std_msgs::msg::String>(
+        "planner/llm_request", 10,
+        [this](const std_msgs::msg::String::SharedPtr msg) {
+          this->OnPlannerLlmRequestReceived(msg);
+        });
+    RCLCPP_INFO(this->get_logger(), "Planner mode enabled - subscribed to /planner/llm_request");
+  } else {
+    RCLCPP_INFO(this->get_logger(), "Planner mode disabled - /planner/llm_request not subscribed");
+  }
+
+  RCLCPP_INFO(this->get_logger(), "omni_node started successfully.");
+  RCLCPP_INFO(this->get_logger(), "Topics:");
+  RCLCPP_INFO(this->get_logger(), "  Publishing: /llm_response");
+  if (enable_planner_mode_) {
+    RCLCPP_INFO(this->get_logger(), "  Publishing: /planner/llm_response");
+  }
+}
+
+OmniNode::~OmniNode() = default;
+
+void OmniNode::OnAsrReceived(const std_msgs::msg::String::SharedPtr msg) {
+  if (!engine_initialized_) {
+    RCLCPP_WARN(this->get_logger(), "Engine not initialized, ignoring message");
+    return;
+  }
+
+  if (!enable_legacy_asr_mode_) {
+    RCLCPP_WARN(this->get_logger(), "Legacy ASR mode is disabled, ignoring message");
+    return;
+  }
+
+  RCLCPP_INFO(this->get_logger(), "Received ASR text: %s", msg->data.c_str());
+
+  engine_->FeedText(msg->data);
+}
+
+void OmniNode::OnPlannerLlmRequestReceived(const std_msgs::msg::String::SharedPtr msg) {
+  if (!engine_initialized_) {
+    RCLCPP_WARN(this->get_logger(), "Engine not initialized, ignoring message");
+    return;
+  }
+
+  if (!enable_planner_mode_) {
+    RCLCPP_WARN(this->get_logger(), "Planner mode is disabled, ignoring message");
+    return;
+  }
+
+  // 检查 route_hint，如果是 "cloud" 则忽略（由 CloudLLMNode 处理）
+  std::string data = msg->data;
+  size_t route_hint_pos = data.find("\"route_hint\"");
+  if (route_hint_pos != std::string::npos) {
+    size_t colon_pos = data.find(":", route_hint_pos);
+    if (colon_pos != std::string::npos) {
+      size_t value_start = data.find("\"", colon_pos + 1);
+      size_t value_end = data.find("\"", value_start + 1);
+      if (value_start != std::string::npos && value_end != std::string::npos) {
+        std::string route_value = data.substr(value_start + 1, value_end - value_start - 1);
+        if (route_value == "cloud") {
+          RCLCPP_INFO(this->get_logger(), "Ignored cloud route_hint request (handled by CloudLLMNode)");
+          return;
+        }
+      }
+    }
+  }
+
+  RCLCPP_INFO(this->get_logger(), "Received planner LLM request, length: %zu bytes", msg->data.length());
+
+  engine_->FeedPlannerRequest(msg->data);
+}
+
+}  // namespace omni_node
+

brain/PlannerNode/src/agint_brain/README.md

@@ -0,0 +1,525 @@
+# agint_brain - AI Agent Brain Package
+
+ROS2 Humble Python 包，Planner 节点的运行时接入层。
+
+---
+
+## 目录结构
+
+```
+agint_brain/
+├── package.xml                 # ROS2 包描述
+├── setup.py                    # Python 包配置
+├── setup.cfg                   # setuptools 配置
+├── resource/
+│   └── agint_brain            # ament_index 标记文件
+├── agint_brain/                # Python 包主目录
+│   ├── __init__.py
+│   ├── planner_node.py        # ROS2 Planner 节点（主入口）
+│   ├── launch/
+│   │   ├── __init__.py
+│   │   └── planner.launch.py  # 启动文件
+│   └── config/
+│       └── planner_config.yaml # Planner 配置文件
+└── README.md
+```
+
+---
+
+## 功能说明
+
+### 节点职责
+
+`planner_node` 是 **Planner 的 ROS2 封装层**，负责：
+
+1. **订阅 Topic**
+   - `/world/snapshot` (std_msgs/String) - 世界状态快照 JSON
+   - `/planner/user_intent` (std_msgs/String) - 用户意图文本
+   - `/planner/llm_response` (std_msgs/String) - OmniNode LLM 响应
+
+2. **发布 Topic**
+   - `/plan` (std_msgs/String) - 生成的标准化 Plan JSON
+   - `/planner/llm_request` (std_msgs/String) - LLM 请求（需要 OmniNode 时）
+
+3. **核心逻辑**
+   - 调用 `planner.py` 生成 Plan
+   - 规则优先，复杂意图走 LLM
+   - LLM 超时自动 fallback
+   - 安全性保证：fallback 只生成 ASK_USER 步骤
+
+### 当前版本限制
+
+**单 pending request 串行版本**
+
+当前 planner_node 仅维护一组状态：
+- `pending_user_intent`
+- `pending_request_id`
+- `waiting_for_llm`
+
+因此当前实现是：
+- **单 pending request 的串行版本**
+- 不支持并发多用户同时规划
+- 不支持多个 LLM 请求同时挂起
+- 不支持多任务并行等待
+
+> 后续若要支持多轮并发/多用户，需要引入 request queue 或 session manager。
+
+### Fallback 安全策略
+
+当发生以下情况时，planner_node 会生成安全的 fallback Plan：
+- Planner 生成 Plan 失败
+- LLM 超时（超过 `llm_timeout_sec`）
+- LLM 返回非法 JSON
+- Plan 解析失败
+
+Fallback Plan 的特征：
+- `source = "planner_node_fallback"`
+- `tool_call_type = "ask_user"`
+- `risk_level = "low"`
+- 用于请求用户重新澄清意图
+
+**这是一个安全保护机制**：不会直接执行任何 capability，而是让用户确认后再继续。
+
+### Topic 数据格式
+
+#### /world/snapshot (订阅)
+```json
+{
+  "environment": {
+    "temperature": 32.5,
+    "humidity": 70,
+    "light_level": 500
+  },
+  "system": {
+    "actuator_status": {
+      "fan": "running",
+      "light": "on"
+    },
+    "device_status": {},
+    "mode": "auto"
+  },
+  "recent_actions": [
+    {"action": "feed", "timestamp": 1713000000}
+  ],
+  "warnings": [],
+  "errors": []
+}
+```
+
+#### /planner/user_intent (订阅)
+```
+打开风扇降温
+```
+
+#### /plan (发布)
+```json
+{
+  "plan_id": "plan_abc12345",
+  "goal": "打开风扇降温",
+  "reasoning": "用户意图: 打开风扇降温, 匹配动作: adjust_fan, 风险等级: low",
+  "risk_level": "low",
+  "requires_confirmation": false,
+  "confirmation_message": null,
+  "steps": [
+    {
+      "step_id": 1,
+      "action": "adjust_fan",
+      "tool_call_type": "execute",
+      "parameters": {"user_intent": "打开风扇降温", "context": {}},
+      "description": "执行动作: adjust_fan"
+    }
+  ],
+  "status": "created",
+  "source": "hybrid"
+}
+```
+
+#### /planner/llm_request (发布)
+```json
+{
+  "request_id": "uuid-xxxx",
+  "user_intent": "再喂一次",
+  "world_snapshot": {...},
+  "available_tools": ["feed", "adjust_fan", "speak"],
+  "domain_rules": {...},
+  "planner_mode": "hybrid",
+  "timestamp": 1713000000.0
+}
+```
+
+#### /planner/llm_response (订阅)
+```json
+{
+  "request_id": "uuid-xxxx",
+  "plan": {
+    "plan_id": "plan_llm_001",
+    "goal": "再喂一次",
+    "reasoning": "用户意图包含重复喂食请求，结合最近喂食时间判断需要用户确认",
+    "risk_level": "medium",
+    "requires_confirmation": true,
+    "confirmation_message": "距离上次喂食不到10分钟，确定要再次喂食吗？",
+    "steps": [
+      {
+        "step_id": 1,
+        "action": "ask_user",
+        "tool_call_type": "ask_user",
+        "parameters": {
+          "question": "距离上次喂食不到10分钟，确定要再次喂食吗？",
+          "original_intent": "再喂一次"
+        },
+        "preconditions": {},
+        "fallback": null,
+        "status": "pending",
+        "description": "询问用户确认重复喂食",
+        "requires_confirmation": true,
+        "confirmation_message": "距离上次喂食不到10分钟，确定要再次喂食吗？",
+        "metadata": {"source": "llm"}
+      }
+    ],
+    "status": "created",
+    "created_at": 1713000000.0,
+    "source": "llm",
+    "metadata": {}
+  }
+}
+```
+
+---
+
+## 依赖
+
+### 系统依赖
+- Ubuntu 22.04
+- ROS2 Humble
+- Python 3.10+
+
+### Python 依赖
+- `rclpy` (ROS2)
+- `std_msgs` (ROS2)
+- `pyyaml`
+
+### 核心模块（在同一 workspace）
+- `planner.py` - 规划核心逻辑
+- `tool_protocol.py` - Plan 数据结构
+- `planner_config_loader.py` - 配置加载器
+- `llm_client.py` - LLM 客户端
+
+---
+
+## 编译
+
+### 1. 进入工作空间
+
+```bash
+cd ~/ros2_ws
+```
+
+### 2. 克隆或放置包
+
+将 `agint_brain` 包放置到 `src/` 目录下：
+
+```bash
+cp -r /path/to/agint_brain ~/ros2_ws/src/
+```
+
+### 3. 确保核心模块在 PYTHONPATH
+
+如果 `planner.py` 等核心模块在其他位置，需要设置环境变量：
+
+```bash
+export PYTHONPATH="${PYTHONPATH}:/path/to/agint/brain"
+```
+
+或者在 `~/.bashrc` 中添加：
+
+```bash
+echo 'export PYTHONPATH="${PYTHONPATH}:/path/to/agint/brain"' >> ~/.bashrc
+source ~/.bashrc
+```
+
+### 4. 安装 Python 依赖
+
+```bash
+pip3 install pyyaml
+```
+
+### 5. 编译包
+
+```bash
+cd ~/ros2_ws
+colcon build --packages-select agint_brain
+source install/setup.bash
+```
+
+### 6. 安装配置文件（可选）
+
+```bash
+mkdir -p ~/.ros2_ws/install/agint_brain/share/agint_brain/config/
+cp ~/ros2_ws/src/agint_brain/agint_brain/config/planner_config.yaml \
+   ~/.ros2_ws/install/agint_brain/share/agint_brain/config/
+```
+
+---
+
+## 启动
+
+### 基本启动
+
+```bash
+ros2 launch agint_brain planner.launch.py
+```
+
+### 带参数启动
+
+```bash
+ros2 launch agint_brain planner.launch.py \
+    planner_config_path:=/path/to/planner_config.yaml \
+    input_world_topic:=/world/snapshot \
+    input_user_intent_topic:=/planner/user_intent \
+    output_plan_topic:=/plan \
+    use_llm:=true \
+    llm_timeout_sec:=10.0 \
+    debug_log:=true
+```
+
+### 参数说明
+
+| 参数 | 默认值 | 说明 |
+|------|--------|------|
+| `planner_config_path` | `planner_config.yaml` | 配置文件路径 |
+| `input_world_topic` | `/world/snapshot` | 世界状态输入 topic |
+| `input_user_intent_topic` | `/planner/user_intent` | 用户意图输入 topic |
+| `output_plan_topic` | `/plan` | Plan 输出 topic |
+| `llm_request_topic` | `/planner/llm_request` | LLM 请求 topic |
+| `llm_response_topic` | `/planner/llm_response` | LLM 响应 topic |
+| `use_llm` | `true` | 是否启用 LLM |
+| `llm_timeout_sec` | `10.0` | LLM 超时时间（秒） |
+| `debug_log` | `true` | 是否输出调试日志 |
+
+---
+
+## 测试
+
+### 1. 查看节点是否运行
+
+```bash
+ros2 node list
+# 应显示 /planner_node
+```
+
+### 2. 查看 topic 列表
+
+```bash
+ros2 topic list
+# 应显示:
+#   /plan
+#   /planner/llm_request
+#   /planner/llm_response
+#   /planner/user_intent
+#   /world/snapshot
+```
+
+### 3. 发送测试数据
+
+#### 发送 world snapshot
+
+```bash
+ros2 topic pub /world/snapshot std_msgs/String \
+    '{data: "{\"environment\": {\"temperature\": 32}, \"system\": {\"actuator_status\": {\"fan\": \"running\"}}}"}' -1
+```
+
+#### 发送用户意图
+
+```bash
+ros2 topic pub /planner/user_intent std_msgs/String \
+    '{data: "打开风扇降温"}' -1
+```
+
+### 4. 监听 Plan 输出
+
+```bash
+ros2 topic echo /plan
+```
+
+---
+
+## 测试场景
+
+### 场景 1：规则可直接处理
+
+**输入：**
+```bash
+ros2 topic pub /world/snapshot std_msgs/String \
+    '{data: "{\"environment\": {\"temperature\": 35}, \"system\": {\"actuator_status\": {\"fan\": \"off\"}}}"}' -1
+
+ros2 topic pub /planner/user_intent std_msgs/String \
+    '{data: "打开风扇降温"}' -1
+```
+
+**预期输出：**
+```
+[plan] plan_id=plan_xxxx, source=rule_engine, goal=打开风扇降温
+```
+
+---
+
+### 场景 2：需要 LLM
+
+**输入：**
+```bash
+ros2 topic pub /world/snapshot std_msgs/String \
+    '{data: "{\"environment\": {\"temperature\": 32}, \"system\": {\"actuator_status\": {}}}"}' -1
+
+ros2 topic pub /planner/user_intent std_msgs/String \
+    '{data: "再喂一次"}' -1
+```
+
+**预期行为：**
+1. 发布 `/planner/llm_request`（request_id=xxx）
+2. 等待 OmniNode 响应
+3. 收到响应后发布 `/plan`
+
+**模拟 OmniNode 响应（完整 Plan JSON）：**
+```bash
+ros2 topic pub /planner/llm_response std_msgs/String \
+    '{data: "{\"request_id\": \"xxx\", \"plan\": {\"plan_id\": \"plan_llm_001\", \"goal\": \"再喂一次\", \"reasoning\": \"LLM生成分步规划\", \"risk_level\": \"low\", \"requires_confirmation\": false, \"confirmation_message\": null, \"steps\": [{\"step_id\": 1, \"action\": \"feed\", \"tool_call_type\": \"execute\", \"parameters\": {\"amount\": \"normal\"}, \"preconditions\": {}, \"fallback\": null, \"status\": \"pending\", \"description\": \"执行喂食\", \"requires_confirmation\": false, \"confirmation_message\": null, \"metadata\": {}}], \"status\": \"created\", \"source\": \"llm\", \"metadata\": {}}}"}' -1
+```
+
+---
+
+### 场景 3：LLM 超时
+
+**步骤：**
+1. 发送触发 LLM 的用户意图
+2. 不发送 LLM 响应
+3. 等待超过 `llm_timeout_sec`（默认 10 秒）
+
+**预期输出：**
+```
+[plan] plan_id=plan_fallback_xxx, source=planner_node_fallback
+```
+
+---
+
+### 场景 4：LLM 非法响应
+
+**步骤：**
+1. 发送触发 LLM 的用户意图
+2. 发送非法 JSON 响应
+
+```bash
+ros2 topic pub /planner/llm_response std_msgs/String \
+    '{data: "not valid json"}' -1
+```
+
+**预期输出：**
+```
+[ERROR] LLM 响应 JSON 解析失败
+[plan] plan_id=plan_fallback_xxx, source=planner_node_fallback
+```
+
+---
+
+## 架构设计
+
+> **版本限制**：planner_node 当前为单 pending request 串行版本，**不支持**并发多用户/多任务并行规划。
+
+```
+┌─────────────────────────────────────────────────────────────────┐
+│                         world_node                              │
+│                     (/world/snapshot)                            │
+└─────────────────────────────┬───────────────────────────────────┘
+                              │
+                              ▼
+┌─────────────────────────────────────────────────────────────────┐
+│                       planner_node                               │
+│                                                                   │
+│  ┌──────────────┐  ┌──────────────┐  ┌────────────────────────┐ │
+│  │ on_world_    │  │ on_user_     │  │ on_llm_response        │ │
+│  │ snapshot     │  │ intent       │  │                        │ │
+│  └──────┬───────┘  └──────┬───────┘  └───────────┬────────────┘ │
+│         │                 │                      │               │
+│         ▼                 ▼                      ▼               │
+│  ┌─────────────────────────────────────────────────────────────┐│
+│  │                    Planner 核心逻辑                          ││
+│  │              (planner.py - generate_plan)                   ││
+│  └──────┬───────────────────────────────────────┬───────────────┘│
+│         │                                       │                 │
+│         ▼                                       ▼                 │
+│  ┌──────────────┐                    ┌──────────────────────┐    │
+│  │ 直接发布 /plan│                    │ 发布 /planner/        │    │
+│  │ (规则可处理)  │                    │ llm_request          │    │
+│  └──────────────┘                    └──────────┬───────────┘    │
+│                                                  │                 │
+│                                                  ▼                 │
+│                                    ┌─────────────────────────┐     │
+│                                    │     OmniNode            │     │
+│                                    │   (LLM 服务)            │     │
+│                                    └──────────┬──────────────┘    │
+│                                               │                    │
+│                                               ▼                    │
+│                                    ┌─────────────────────────┐     │
+│                                    │ /planner/llm_response   │     │
+│                                    └─────────────────────────┘     │
+│                                                                   │
+└─────────────────────────────────────────────────────────────────┘
+                              │
+                              ▼
+┌─────────────────────────────────────────────────────────────────┐
+│                       executor_node                               │
+│                         (/plan)                                  │
+└─────────────────────────────────────────────────────────────────┘
+```
+
+---
+
+## 故障排除
+
+### 1. 核心模块导入失败
+
+```
+ImportError: cannot import name 'Planner' from 'planner'
+```
+
+**解决方案：** 确保 `planner.py` 等核心模块在 PYTHONPATH 中：
+
+```bash
+export PYTHONPATH="${PYTHONPATH}:/path/to/agint/brain"
+```
+
+### 2. 配置文件找不到
+
+```
+[ERROR] 加载 planner 配置失败: 配置文件不存在
+```
+
+**解决方案：** 使用绝对路径或确保配置文件在正确位置：
+
+```bash
+ros2 launch agint_brain planner.launch.py \
+    planner_config_path:=/full/path/to/planner_config.yaml
+```
+
+### 3. 编译时缺少依赖
+
+```
+Could not find a package configuration file provided by "ament_python"
+```
+
+**解决方案：** 安装 ROS2 构建工具：
+
+```bash
+sudo apt install ros-humble-ament-python
+```
+
+---
+
+## License
+
+MIT License
+
+---
+
+## Author
+
+Jonathan

brain/PlannerNode/src/agint_brain/__init__.py

@@ -0,0 +1,17 @@
+"""agint_brain - AI Agent Brain Package
+
+ROS2 Python 包，包含：
+    - planner_node: Planner 的 ROS2 运行时接入层
+    - launch: 启动文件
+    - config: 配置文件
+
+核心模块依赖：
+    - planner.py
+    - tool_protocol.py
+    - planner_config_loader.py
+    - llm_client.py
+
+确保这些文件在 PYTHONPATH 中，或通过 ROS2 workspace 配置。
+"""
+
+__version__ = "0.1.0"

brain/PlannerNode/src/agint_brain/config/planner_config.yaml

@@ -0,0 +1,270 @@
+# Planner 配置文件
+# ============================================================================
+# 该文件包含 planner.py 的规则引擎配置和 planner_node.py 的运行参数。
+# 启动时由 planner_node 通过 planner_config_loader 加载。
+#
+# 目录位置：
+#   ROS2 包内：share/agint_brain/config/planner_config.yaml
+#   或通过参数 planner_config_path 指定绝对路径
+
+
+# ============================================================================
+# Planner 模式
+# ============================================================================
+# 可选值：rule / llm / hybrid
+#   - rule: 仅使用规则引擎（不调用 LLM）
+#   - llm: 仅使用 LLM（调用 OmniNode）
+#   - hybrid: 优先规则，复杂意图走 LLM
+mode: "hybrid"
+
+
+# ============================================================================
+# Planner 核心参数
+# ============================================================================
+planner:
+  # 默认风险等级：low / medium / high
+  default_risk_level: "low"
+
+  # 中高风险操作是否需要用户确认
+  require_confirmation_on_medium_risk: true
+  require_confirmation_on_high_risk: true
+
+  # 单个 Plan 最大步骤数
+  max_plan_steps: 10
+
+  # 计划来源标识（用于 Plan.source 字段）
+  default_source: "hybrid"
+
+
+# ============================================================================
+# 可用工具（Capability 列表）
+# ============================================================================
+  available_tools:
+    - "feed"
+    - "adjust_fan"
+    - "control_light"
+    - "speak"
+    - "query"
+    - "move"
+    - "inspect"
+    - "turn_on"
+    - "turn_off"
+    - "adjust"
+
+  # 工具语义描述（供 OmniNode 使用）
+  # 格式：name, description, tool_call_type, category
+  tool_descriptions:
+    - name: "feed"
+      description: "用于执行喂食动作（如宠物或牲畜喂食）"
+      tool_call_type: "execute"
+      category: "action"
+
+    - name: "adjust_fan"
+      description: "用于调整风扇的风力档位或开关状态"
+      tool_call_type: "execute"
+      category: "action"
+
+    - name: "control_light"
+      description: "用于控制灯光的开关或亮度"
+      tool_call_type: "execute"
+      category: "action"
+
+    - name: "speak"
+      description: "用于播报自然语言给用户，仅用于对话播报"
+      tool_call_type: "speak"
+      category: "dialog"
+
+    - name: "query"
+      description: "用于查询机器人系统内部状态，不代表联网查询外部实时信息"
+      tool_call_type: "query_world"
+      category: "query"
+
+    - name: "move"
+      description: "用于控制机器人移动到指定位置"
+      tool_call_type: "execute"
+      category: "action"
+
+    - name: "inspect"
+      description: "用于巡检、查看环境或检查状态"
+      tool_call_type: "execute"
+      category: "action"
+
+    - name: "turn_on"
+      description: "用于打开设备"
+      tool_call_type: "execute"
+      category: "action"
+
+    - name: "turn_off"
+      description: "用于关闭设备"
+      tool_call_type: "execute"
+      category: "action"
+
+    - name: "adjust"
+      description: "用于通用调节操作"
+      tool_call_type: "execute"
+      category: "action"
+
+
+# ============================================================================
+# 意图映射（关键词 → action）
+# ============================================================================
+  intent_to_action:
+    # 喂食相关
+    "喂": "feed"
+    "喂食": "feed"
+    "喂养": "feed"
+
+    # 风扇控制
+    "打开风扇": "adjust_fan"
+    "关闭风扇": "adjust_fan"
+    "调整风扇": "adjust_fan"
+    "降温": "adjust_fan"
+    "升温": "adjust_fan"
+
+    # 灯光控制
+    "开灯": "control_light"
+    "关灯": "control_light"
+    "调整灯光": "control_light"
+
+    # 通用开关
+    "打开": "turn_on"
+    "关闭": "turn_off"
+    "启动": "turn_on"
+    "停止": "turn_off"
+    "调节": "adjust"
+
+    # 移动
+    "移动": "move"
+    "前往": "move"
+    "走到": "move"
+
+    # 查询/检查
+    "查询": "query"
+    "查看": "query"
+    "检查": "inspect"
+    "巡检": "inspect"
+    "监测": "monitor"
+
+    # 语音/通知
+    "播报": "speak"
+    "报告": "speak"
+    "说": "speak"
+    "告诉": "speak"
+    "通知": "notify"
+    "提醒": "remind"
+    "警告": "warn"
+    "询问": "ask"
+
+
+# ============================================================================
+# 高风险动作列表
+# ============================================================================
+  high_risk_actions:
+    - "turn_off"
+    - "emergency_stop"
+    - "shutdown"
+    - "power_off"
+
+
+# ============================================================================
+# 中风险动作列表
+# ============================================================================
+  medium_risk_actions:
+    - "adjust_fan"
+    - "adjust"
+    - "control_light"
+
+
+# ============================================================================
+# 确认规则
+# ============================================================================
+  confirmation_rules:
+    # 重复动作需要确认
+    repeated_action_requires_confirmation: true
+
+    # 执行器不可用时需要确认
+    unavailable_actuator_requires_confirmation: true
+
+    # 高风险需要确认（覆盖默认值）
+    high_risk_requires_confirmation: true
+
+    # 中风险需要确认（覆盖默认值）
+    medium_risk_requires_confirmation: true
+
+
+# ============================================================================
+# Planner Node 运行时参数（也可通过 ROS2 参数传入）
+# ============================================================================
+planner_node:
+  # Topic 配置
+  input_world_topic: "/world/snapshot"
+  input_asr_topic: "/asr"
+  output_plan_topic: "/plan"
+  llm_request_topic: "/planner/llm_request"
+  llm_response_topic: "/planner/llm_response"
+  planner_config_path: "planner_config.yaml"
+
+  # LLM 配置
+  llm_timeout_sec: 10.0
+  use_llm: true
+  default_session_id: "default"
+
+  # 日志配置
+  debug_log: true
+
+
+# ============================================================================
+# Gate 前置治理配置（可选，已迁移到 planner_gate_config.yaml）
+# ============================================================================
+# gate_rules 和 project_env 配置已迁移到独立的 planner_gate_config.yaml
+# 此处保留占位，便于后续扩展
+gate_config:
+  # Gate 配置文件路径
+  gate_config_path: "planner_gate_config.yaml"
+
+  # 是否启用 Gate 前置治理
+  enable_gate: true
+
+  # 是否启用 Teacher Memory
+  enable_teacher_memory: true
+
+
+# ============================================================================
+# 项目环境配置（场景相关，可切换）
+# ============================================================================
+# 此处为基础配置，详细配置见 planner_gate_config.yaml
+project_env:
+  # 场景标识
+  scene: cattle_farm_robot
+
+  # 标准地点列表
+  standard_locations:
+    - "广场"
+    - "大门"
+    - "充电站"
+    - "牛棚1号"
+    - "牛棚2号"
+
+  # 地点别名映射（用户说法 -> 标准地点）
+  location_alias_map:
+    "牛鹏1号": "牛棚1号"
+    "牛鹏2号": "牛棚2号"
+    "牛栏1号": "牛棚1号"
+    "牛栏2号": "牛棚2号"
+
+  # 机器人角色
+  robot_role: "智能农场机器人"
+  robot_name: "农宝"
+
+
+# ============================================================================
+# Teacher Memory 模板（基础模板，详细模板见 teaching_memory_templates.yaml）
+# ============================================================================
+# 此处为基础模板，系统会优先加载 teaching_memory_templates.yaml
+teaching_memory_base:
+  # 查询类基础回复
+  query.position: "我目前在广场。"
+  query.status: "我当前电量充足，状态正常，随时可以工作。"
+  query.weather: "当前天气晴朗，温度28°C，适合户外工作。"
+  query.identity: "我是农宝，智能农场机器人，为您服务。"
+

brain/PlannerNode/src/agint_brain/config/planner_gate_config.yaml

@@ -0,0 +1,285 @@
+# ============================================================================
+# Planner Gate 规则配置
+# ============================================================================
+# 定义 Gate0-Gate5 前置治理层的规则
+# 这些规则用于本地快速处理简单请求，减少不必要的 LLM 调用
+
+# 场景标识
+scene: cattle_farm_robot
+
+# ============================================================================
+# Gate0: 基础解析规则
+# ============================================================================
+
+# 查询类关键词（用于识别 query 类型请求）
+query_keywords:
+  position:
+    - "你在哪里"
+    - "当前位置"
+    - "位置"
+    - "在哪"
+
+  status:
+    - "电量"
+    - "状态"
+    - "当前状态"
+    - "还好吗"
+
+  count:
+    - "几个"
+    - "数量"
+    - "多少"
+
+  weather:
+    - "天气"
+    - "今天天气"
+
+  identity:
+    - "我的名字"
+    - "我叫什么"
+    - "你是谁"
+
+# 动作类关键词（用于识别 action 类型请求）
+action_keywords:
+  move:
+    - "去"
+    - "前往"
+    - "移动"
+    - "走到"
+    - "导航到"
+
+  feed:
+    - "喂"
+    - "喂牛"
+    - "喂食"
+    - "喂养"
+
+  inspect:
+    - "巡检"
+    - "检查"
+    - "查看"
+    - "视察"
+
+  open:
+    - "打开"
+    - "开启"
+    - "启动"
+
+  close:
+    - "关闭"
+    - "关掉"
+    - "停止"
+
+  adjust:
+    - "调节"
+    - "调整"
+    - "设置"
+
+# 复杂任务关键词（用于识别 complex_task）
+complex_keywords:
+  - "如果"
+  - "否则"
+  - "先"
+  - "再"
+  - "然后"
+  - "顺便"
+  - "完成后"
+  - "回来汇报"
+  - "不能去就"
+  - "改去"
+  - "等一下"
+  - "等会儿"
+
+# 缺槽规则（action -> 必需的槽位）
+missing_slot_rules:
+  open: object
+  close: object
+  adjust: object
+  feed: location
+  move: location
+  inspect: location
+
+# ============================================================================
+# Gate1: 表达完整性过滤配置
+# ============================================================================
+
+# 缺槽时的路由建议（可覆盖全局设置）
+missing_slot_route: "local_direct"
+
+# ============================================================================
+# Gate2: 边界判断配置
+# ============================================================================
+
+# 本地直接处理的场景开关
+local_direct_handle:
+  enable_ask: true
+  enable_confirm: true
+  enable_reject: true
+  enable_already_there: true
+
+# ============================================================================
+# Gate3: 复杂任务识别配置
+# ============================================================================
+
+# 复杂任务的路由建议
+complex_task_route: "cloud"
+
+# ============================================================================
+# Gate4: Teacher Memory 配置
+# ============================================================================
+
+# 是否启用 Teacher Memory
+enable_teacher_memory: true
+
+# Teacher Memory 模板文件路径（相对或绝对路径）
+teacher_memory_templates_path: "teaching_memory_templates.yaml"
+
+# ============================================================================
+# Gate5: 路由决策配置
+# ============================================================================
+
+# 各 task_type 的默认路由
+task_type_routes:
+  ask: "local_direct"
+  confirm: "local_direct"
+  reject: "local_direct"
+  already_there: "local_direct"
+  query: "local_direct"
+  simple_task: "local_3b"
+  complex_task: "cloud"
+  unknown: "local_3b"
+
+# ============================================================================
+# 项目环境配置（场景相关，可切换）
+# ============================================================================
+
+project_env:
+  # 标准地点列表
+  standard_locations:
+    - "广场"
+    - "大门"
+    - "充电站"
+    - "牛棚1号"
+    - "牛棚2号"
+    - "1号"
+    - "2号"
+    - "一号"
+    - "二号"
+    - "1号牛棚"
+    - "2号牛棚"
+    - "一号牛棚"
+    - "二号牛棚"
+
+  # 地点别名映射（用户说法 -> 标准地点）
+  location_alias_map:
+    "牛鹏1号": "牛棚1号"
+    "牛鹏2号": "牛棚2号"
+    "牛栏1号": "牛棚1号"
+    "牛栏2号": "牛棚2号"
+    "牛棚一号": "牛棚1号"
+    "牛棚二号": "牛棚2号"
+    "一号棚": "牛棚1号"
+    "二号棚": "牛棚2号"
+    "1号棚": "牛棚1号"
+    "2号棚": "牛棚2号"
+    "广场": "广场"
+    "大门": "大门"
+    "入口": "大门"
+
+  # 未知地点匹配模式
+  unknown_location_patterns:
+    - "农场"
+    - "牛场"
+    - "外面"
+    - "那边"
+    - "这里"
+    - "那边"
+
+  # 已知的对象列表
+  known_objects:
+    - "风扇"
+    - "灯"
+    - "灯光"
+    - "门"
+    - "空调"
+    - "通风"
+
+  # 不可达状态列表
+  unreachable_status:
+    - "repairing"
+    - "unavailable"
+    - "unreachable"
+    - "offline"
+
+  # 地点允许的技能配置（静态配置）
+  # 格式：地点名称 -> 允许的技能列表
+  # 优先级：world snapshot 中的 allowed_skills > 此处配置
+  location_allowed_skills:
+    "广场": ["move", "inspect"]
+    "大门": ["move", "inspect"]
+    "充电站": ["move", "charge", "inspect"]
+    "牛棚1号": ["move", "feed", "inspect"]
+    "牛棚2号": ["move", "feed", "inspect"]
+    "1号": ["move", "feed", "inspect"]
+    "2号": ["move", "feed", "inspect"]
+    "一号": ["move", "feed", "inspect"]
+    "二号": ["move", "feed", "inspect"]
+    "1号牛棚": ["move", "feed", "inspect"]
+    "2号牛棚": ["move", "feed", "inspect"]
+    "一号牛棚": ["move", "feed", "inspect"]
+    "二号牛棚": ["move", "feed", "inspect"]
+
+  # 机器人角色描述
+  robot_role: "智能农场机器人"
+  robot_name: "农宝"
+
+# ============================================================================
+# LLM 统一提示词配置
+# ============================================================================
+# 所有机器人业务提示词统一在此定义，由 PlannerNode 组装后分发给 LLM 节点
+# OmniNode 和 CloudLLMNode 不再内置业务提示词
+
+llm_prompts:
+  # 机器人统一身份和职责
+  system_prompt: |
+    你是一个智能农业机器人，名字叫农宝。
+    你的职责是帮助用户完成农场任务，包括喂食牛群、巡检牛棚、调节环境设备等。
+    你需要基于用户输入和当前环境状态，给出自然语言回复和轻量行动计划。
+
+  # 统一输出协议
+  output_protocol_prompt: |
+    ==输出协议（必须严格遵守）==
+    1. 第一段：自然语言回复（可直接播报给用户）
+    2. 单独一行：<FINAL_JSON>
+    3. 第三段：严格轻量 JSON
+
+    ==禁止事项==
+    - 禁止使用 markdown 代码块
+    - 禁止在 JSON 后追加解释
+    - JSON 必须是合法可解析的
+
+  # 领域规则和约束
+  business_rules_prompt: |
+    ==行为约束==
+    - 只使用可用工具列表中的工具
+    - 不能编造不存在的地点或技能
+    - 缺少必要信息时返回 status=ask
+    - 存在歧义时返回 status=confirm
+    - 不可执行时返回 status=reject
+
+    ==地点说明==
+    - 广场: 开阔区域
+    - 大门: 入口
+    - 充电站: 机器人充电
+    - 牛棚1号、牛棚2号: 养牛区域
+
+    ==可用工具==
+    - move: 移动到指定地点
+    - feed: 喂食
+    - inspect: 巡检
+    - adjust: 调节设备
+    - open/close: 开关设备
+    - speak: 播报
+    - ask_user: 询问用户
+    - query: 查询状态
+

brain/PlannerNode/src/agint_brain/config/teaching_memory_templates.yaml

@@ -0,0 +1,288 @@
+# ============================================================================
+# Teacher Memory 模板配置
+# ============================================================================
+# 定义标准回复模板，用于本地直出 Plan
+# 当请求命中特定场景时，使用对应模板生成回复
+
+# ============================================================================
+# 查询类模板 (query.*)
+# ============================================================================
+
+templates:
+  # 位置查询
+  query.position:
+    reply: "我目前在广场。"
+    command: ""
+    source: "cloud_teacher"
+    verified: true
+    response_type: "speak"
+
+  # 状态查询
+  query.status:
+    reply: "我当前电量充足，状态正常，随时可以工作。"
+    command: ""
+    source: "cloud_teacher"
+    verified: true
+    response_type: "speak"
+
+  # 数量查询
+  query.count:
+    reply: "当前检测到2只牛。"
+    command: ""
+    source: "cloud_teacher"
+    verified: true
+    response_type: "speak"
+
+  # 天气查询
+  query.weather:
+    reply: "当前天气晴朗，温度28°C，适合户外工作。"
+    command: ""
+    source: "cloud_teacher"
+    verified: true
+    response_type: "speak"
+
+  # 身份查询
+  query.identity:
+    reply: "我是农宝，智能农场机器人，为您服务。"
+    command: ""
+    source: "cloud_teacher"
+    verified: true
+    response_type: "speak"
+
+  # ============================================================================
+  # 缺槽类模板 (task.*.missing_*)
+  # ============================================================================
+
+  # 打开设备 - 缺对象
+  task.open.missing_object:
+    reply: "好的，请问您要打开什么设备？"
+    command: "ask=object"
+    source: "template"
+    verified: true
+    response_type: "ask"
+
+  # 关闭设备 - 缺对象
+  task.close.missing_object:
+    reply: "好的，请问您要关闭什么设备？"
+    command: "ask=object"
+    source: "template"
+    verified: true
+    response_type: "ask"
+
+  # 调节设备 - 缺对象
+  task.adjust.missing_object:
+    reply: "好的，请问您要调节什么设备？"
+    command: "ask=object"
+    source: "template"
+    verified: true
+    response_type: "ask"
+
+  # 喂食 - 缺地点
+  task.feed.missing_location:
+    reply: "好的，请告诉我要去哪个牛棚。"
+    command: "ask=location"
+    source: "template"
+    verified: true
+    response_type: "ask"
+
+  # 移动 - 缺地点
+  task.move.missing_location:
+    reply: "好的，请告诉我要去哪里。"
+    command: "ask=location"
+    source: "template"
+    verified: true
+    response_type: "ask"
+
+  # 巡检 - 缺地点
+  task.inspect.missing_location:
+    reply: "好的，请告诉我要去哪个地点巡检。"
+    command: "ask=location"
+    source: "template"
+    verified: true
+    response_type: "ask"
+
+  # ============================================================================
+  # 边界类模板 (task.*.alias_like_location / unknown_location / unreachable_location)
+  # ============================================================================
+
+  # 喂食 - 非标准地点（别名）
+  task.feed.alias_like_location:
+    reply: "我理解您说的可能是牛棚1号，请确认一下是否正确。"
+    command: "ask=location_confirm"
+    source: "template"
+    verified: true
+    response_type: "confirm"
+    confirmation_target: "牛棚1号"
+
+  task.feed.alias_like_location.牛棚2号:
+    reply: "我理解您说的可能是牛棚2号，请确认一下是否正确。"
+    command: "ask=location_confirm"
+    source: "template"
+    verified: true
+    response_type: "confirm"
+    confirmation_target: "牛棚2号"
+
+  # 移动 - 非标准地点（别名）
+  task.move.alias_like_location:
+    reply: "我理解您说的可能是广场，请确认一下是否正确。"
+    command: "ask=location_confirm"
+    source: "template"
+    verified: true
+    response_type: "confirm"
+    confirmation_target: "广场"
+
+  task.move.alias_like_location.大门:
+    reply: "我理解您说的可能是大门，请确认一下是否正确。"
+    command: "ask=location_confirm"
+    source: "template"
+    verified: true
+    response_type: "confirm"
+    confirmation_target: "大门"
+
+  # 巡检 - 非标准地点（别名）
+  task.inspect.alias_like_location:
+    reply: "我理解您说的可能是牛棚1号，请确认一下是否正确。"
+    command: "ask=location_confirm"
+    source: "template"
+    verified: true
+    response_type: "confirm"
+    confirmation_target: "牛棚1号"
+
+  # 喂食 - 未知地点
+  task.feed.unknown_location:
+    reply: "这个地点我暂时无法确认，请您再说明一下具体位置。"
+    command: "ask=location_retry"
+    source: "template"
+    verified: true
+    response_type: "confirm"
+
+  task.move.unknown_location:
+    reply: "这个地点我暂时无法确认，请您再说明一下具体位置。"
+    command: "ask=location_retry"
+    source: "template"
+    verified: true
+    response_type: "confirm"
+
+  task.inspect.unknown_location:
+    reply: "这个地点我暂时无法确认，请您再说明一下具体位置。"
+    command: "ask=location_retry"
+    source: "template"
+    verified: true
+    response_type: "confirm"
+
+  # 喂食 - 不可达地点
+  task.feed.unreachable_location:
+    reply: "抱歉，该地点当前不可到达，不能执行这个任务。"
+    command: "reject"
+    source: "template"
+    verified: true
+    response_type: "reject"
+
+  task.move.unreachable_location:
+    reply: "抱歉，该地点当前不可到达，无法移动到那里。"
+    command: "reject"
+    source: "template"
+    verified: true
+    response_type: "reject"
+
+  task.inspect.unreachable_location:
+    reply: "抱歉，该地点当前不可到达，无法进行巡检。"
+    command: "reject"
+    source: "template"
+    verified: true
+    response_type: "reject"
+
+  # ============================================================================
+  # Already There 类模板 (task.*.already_there)
+  # ============================================================================
+
+  # 已在目标点 - 移动
+  task.move.already_there:
+    reply: "您当前已经在目标地点，无需移动。"
+    command: "noop"
+    source: "template"
+    verified: true
+    response_type: "already_there"
+
+  # 已在目标点 - 喂食
+  task.feed.already_there:
+    reply: "您当前已经在目标牛棚，可以直接执行喂食操作。"
+    command: "noop"
+    source: "template"
+    verified: true
+    response_type: "already_there"
+
+  # 已在目标点 - 巡检
+  task.inspect.already_there:
+    reply: "您当前已经在目标区域，可以直接开始巡检。"
+    command: "noop"
+    source: "template"
+    verified: true
+    response_type: "already_there"
+
+  # ============================================================================
+  # 复杂任务类模板 (task.complex.*)
+  # ============================================================================
+
+  # 条件任务（不直接输出，等待云端处理）
+  task.complex.conditional:
+    reply: ""
+    command: ""
+    source: "template"
+    verified: true
+    response_type: "defer_to_cloud"
+    route_hint: "cloud"
+
+  # 多步骤任务（不直接输出，等待云端处理）
+  task.complex.multistep:
+    reply: ""
+    command: ""
+    source: "template"
+    verified: true
+    response_type: "defer_to_cloud"
+    route_hint: "cloud"
+
+  # ============================================================================
+  # 标准任务类模板 (task.*.standard)
+  # ============================================================================
+
+  # 标准喂食任务
+  task.feed.standard:
+    reply: ""
+    command: "feed"
+    source: "template"
+    verified: true
+    response_type: "execute"
+
+  # 标准移动任务
+  task.move.standard:
+    reply: ""
+    command: "move"
+    source: "template"
+    verified: true
+    response_type: "execute"
+
+  # 标准巡检任务
+  task.inspect.standard:
+    reply: ""
+    command: "inspect"
+    source: "template"
+    verified: true
+    response_type: "execute"
+
+  # 标准打开设备
+  task.open.standard:
+    reply: ""
+    command: "turn_on"
+    source: "template"
+    verified: true
+    response_type: "execute"
+
+  # 标准关闭设备
+  task.close.standard:
+    reply: ""
+    command: "turn_off"
+    source: "template"
+    verified: true
+    response_type: "execute"
+

brain/PlannerNode/src/agint_brain/launch/planner.launch.py

@@ -0,0 +1,188 @@
+#!/usr/bin/env python3
+"""
+agint_brain.planner.launch - Planner Node 启动文件
+
+功能：
+    - 启动 planner_node
+    - 加载 planner_config.yaml
+    - 配置 ROS2 参数
+    - 输出到 screen
+
+使用方式：
+    ros2 launch agint_brain planner.launch.py
+
+可选参数：
+    planner_config_path: 配置文件路径
+    input_world_topic: 世界状态 topic
+    input_user_intent_topic: 用户意图 topic
+    output_plan_topic: Plan 输出 topic
+    llm_request_topic: LLM 请求 topic
+    llm_response_topic: LLM 响应 topic
+    use_llm: 是否启用 LLM
+    llm_timeout_sec: LLM 超时时间
+    debug_log: 是否输出调试日志
+"""
+
+import os
+from pathlib import Path
+
+from ament_index_python.packages import get_package_share_directory
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument, LogInfo, RegisterEventHandler
+from launch.event_handlers import OnProcessExit
+from launch.substitutions import LaunchConfiguration, PathJoinSubstitution
+from launch_ros.actions import Node
+
+
+def generate_launch_description() -> LaunchDescription:
+    """生成 launch description"""
+    
+    # 获取包共享目录
+    pkg_share = get_package_share_directory("agint_brain")
+    
+    # 默认配置文件路径
+    default_config_path = PathJoinSubstitution([
+        pkg_share,
+        "config",
+        "planner_config.yaml"
+    ])
+
+    # =========================================================================
+    # Launch Arguments
+    # =========================================================================
+    planner_config_path_arg = DeclareLaunchArgument(
+        "planner_config_path",
+        default_value=default_config_path,
+        description="planner_config.yaml 配置文件路径",
+    )
+
+    gate_config_path_arg = DeclareLaunchArgument(
+        "gate_config_path",
+        default_value=PathJoinSubstitution([
+            pkg_share,
+            "config",
+            "planner_gate_config.yaml"
+        ]),
+        description="planner_gate_config.yaml 配置文件路径（Gate 前置治理配置）",
+    )
+    
+    input_world_topic_arg = DeclareLaunchArgument(
+        "input_world_topic",
+        default_value="/world/snapshot",
+        description="World snapshot 输入 topic",
+    )
+    
+    input_user_intent_topic_arg = DeclareLaunchArgument(
+        "input_user_intent_topic",
+        default_value="/planner/user_intent",
+        description="用户意图输入 topic（已废弃，使用 input_asr_topic）",
+    )
+
+    input_asr_topic_arg = DeclareLaunchArgument(
+        "input_asr_topic",
+        default_value="/asr",
+        description="ASR 语音识别输入 topic",
+    )
+    
+    output_plan_topic_arg = DeclareLaunchArgument(
+        "output_plan_topic",
+        default_value="/plan",
+        description="Plan 输出 topic",
+    )
+    
+    llm_request_topic_arg = DeclareLaunchArgument(
+        "llm_request_topic",
+        default_value="/planner/llm_request",
+        description="LLM 请求 topic",
+    )
+    
+    llm_response_topic_arg = DeclareLaunchArgument(
+        "llm_response_topic",
+        default_value="/planner/llm_response",
+        description="LLM 响应 topic",
+    )
+    
+    use_llm_arg = DeclareLaunchArgument(
+        "use_llm",
+        default_value="true",
+        description="是否启用 LLM",
+    )
+    
+    llm_timeout_sec_arg = DeclareLaunchArgument(
+        "llm_timeout_sec",
+        default_value="10.0",
+        description="LLM 请求超时时间（秒）",
+    )
+    
+    debug_log_arg = DeclareLaunchArgument(
+        "debug_log",
+        default_value="true",
+        description="是否输出调试日志",
+    )
+
+    default_session_id_arg = DeclareLaunchArgument(
+        "default_session_id",
+        default_value="default",
+        description="默认会话 ID",
+    )
+
+    # =========================================================================
+    # Planner Node
+    # =========================================================================
+    planner_node = Node(
+        package="agint_brain",
+        executable="planner_node",
+        name="planner_node",
+        output="screen",
+        emulate_tty=True,
+        parameters=[
+            {
+                "planner_config_path": LaunchConfiguration("planner_config_path"),
+                "gate_config_path": LaunchConfiguration("gate_config_path"),
+                "input_world_topic": LaunchConfiguration("input_world_topic"),
+                "input_asr_topic": LaunchConfiguration("input_asr_topic"),
+                "output_plan_topic": LaunchConfiguration("output_plan_topic"),
+                "llm_request_topic": LaunchConfiguration("llm_request_topic"),
+                "llm_response_topic": LaunchConfiguration("llm_response_topic"),
+                "use_llm": LaunchConfiguration("use_llm"),
+                "llm_timeout_sec": LaunchConfiguration("llm_timeout_sec"),
+                "debug_log": LaunchConfiguration("debug_log"),
+                "default_session_id": LaunchConfiguration("default_session_id", default="default"),
+            }
+        ],
+    )
+
+    # =========================================================================
+    # Event Handlers
+    # =========================================================================
+    exit_event = RegisterEventHandler(
+        event_handler=OnProcessExit(
+            target_action=planner_node,
+            on_exit=[LogInfo(msg="Planner Node 已退出")],
+        )
+    )
+
+    # =========================================================================
+    # Launch Description
+    # =========================================================================
+    ld = LaunchDescription([
+        LogInfo(msg="========================================"),
+        LogInfo(msg="正在启动 Planner Node (含 Gate 前置治理层)..."),
+        LogInfo(msg="========================================"),
+        planner_config_path_arg,
+        gate_config_path_arg,
+        input_world_topic_arg,
+        input_asr_topic_arg,
+        output_plan_topic_arg,
+        llm_request_topic_arg,
+        llm_response_topic_arg,
+        use_llm_arg,
+        llm_timeout_sec_arg,
+        debug_log_arg,
+        default_session_id_arg,
+        planner_node,
+        exit_event,
+    ])
+
+    return ld
+

brain/PlannerNode/src/agint_brain/package.xml

@@ -0,0 +1,23 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format_3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>agint_brain</name>
+  <version>0.1.0</version>
+  <description>AI Agent Brain Package - Planner Node and Core Logic</description>
+  <maintainer email="jonath@example.com">Jonathan</maintainer>
+  <license>MIT</license>
+
+  <buildtool_depend>ament_python</buildtool_depend>
+
+  <depend>rclpy</depend>
+  <depend>std_msgs</depend>
+  <exec_depend>pyyaml</exec_depend>
+
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>ament_lint_common</test_depend>
+
+  <export>
+    <build_type>ament_python</build_type>
+  </export>
+</package>
+

brain/PlannerNode/src/agint_brain/planner_node.py

@@ -0,0 +1,2952 @@
+#!/usr/bin/env python3
+"""
+agint_brain.planner_node - ROS2 Planner Node with Gate 前置治理层
+
+ROS2 节点，作为 Planner 的运行时接入层，负责：
+    - 订阅 /world/snapshot（世界状态快照）
+    - 订阅 /asr（用户语音识别文本）【主入口】
+    - 订阅 /planner/llm_response（OmniNode LLM 响应）
+    - 统一编排后发布 /planner/llm_request 给 OmniNode
+    - 发布 /plan（标准化执行计划）
+
+【新增】Gate 前置治理层：
+    - Gate0: 基础解析
+    - Gate1: 表达完整性过滤
+    - Gate2: 边界判断
+    - Gate3: 复杂任务识别
+    - Gate4: Teacher Memory 检索
+    - Gate5: 本地/云端路由
+
+Topic 设计：
+    订阅：
+        - /world/snapshot (std_msgs/String) - 世界状态 JSON
+        - /asr (std_msgs/String) - 用户语音转文字（主入口）
+        - /planner/llm_response (std_msgs/String) - OmniNode LLM 结构化响应
+    发布：
+        - /plan (std_msgs/String) - 生成的 Plan JSON
+        - /planner/llm_request (std_msgs/String) - 发给 OmniNode 的统一请求 JSON
+
+Author: Jonathan
+"""
+
+import json
+import os
+import re
+import sys
+import time
+import uuid
+from typing import Any, Dict, List, Optional, Union
+
+import rclpy
+from rclpy.node import Node
+from rclpy.qos import QoSProfile, ReliabilityPolicy, HistoryPolicy
+from std_msgs.msg import String
+
+try:
+    import yaml
+except ImportError:
+    yaml = None
+
+# 尝试导入核心模块
+try:
+    from planner import Planner, PlannerConfig
+    from tool_protocol import Plan, PlanStep, PlanStatus, RiskLevel, ToolCallType, StepStatus
+    from planner_config_loader import load_planner_runtime_config, PlannerRuntimeConfig
+    CORE_MODULES_AVAILABLE = True
+except ImportError as e:
+    CORE_MODULES_AVAILABLE = False
+    CORE_IMPORT_ERROR = str(e)
+
+
+class PlannerNode(Node):
+    """ROS2 Planner Node - OmniNode 前端的统一编排层（带 Gate 前置治理）
+
+    核心职责：
+        1. 订阅 /world/snapshot 获取世界状态
+        2. 订阅 /asr 获取用户语音输入（主入口）
+        3. Gate 前置治理：本地快速处理简单请求
+        4. 收集 world/tools/state 构造统一请求 JSON
+        5. 发布 /planner/llm_request 给 OmniNode
+        6. 接收 /planner/llm_response 解析为 Plan
+        7. 发布 /plan 给 executor
+        8. 处理 LLM 超时、fallback、请求覆盖
+    """
+
+    def __init__(self) -> None:
+        """节点初始化"""
+        super().__init__("planner_node")
+
+        # 检查核心模块是否可用
+        if not CORE_MODULES_AVAILABLE:
+            self.get_logger().error(
+                f"核心模块导入失败: {CORE_IMPORT_ERROR}\n"
+                "请确保 planner.py, tool_protocol.py, planner_config_loader.py 在 PYTHONPATH 中，"
+                "或设置 ROS2 工作空间的 PYTHONPATH。"
+            )
+            raise ImportError(f"核心模块不可用: {CORE_IMPORT_ERROR}")
+
+        # =====================================================================
+        # 1. 声明并读取 ROS2 参数
+        # =====================================================================
+        self.declare_parameter("input_world_topic", "/world/snapshot")
+        self.declare_parameter("input_asr_topic", "/asr")
+        self.declare_parameter("output_plan_topic", "/plan")
+        self.declare_parameter("llm_request_topic", "/planner/llm_request")
+        self.declare_parameter("llm_response_topic", "/planner/llm_response")
+        self.declare_parameter("memory_context_topic", "/memory/context")
+        self.declare_parameter("planner_config_path", "planner_config.yaml")
+        self.declare_parameter("gate_config_path", "planner_gate_config.yaml")
+        self.declare_parameter("use_llm", True)
+        self.declare_parameter("llm_timeout_sec", 10.0)
+        self.declare_parameter("debug_log", True)
+        self.declare_parameter("default_session_id", "default")
+
+        self.input_world_topic = self.get_parameter("input_world_topic").value
+        self.input_asr_topic = self.get_parameter("input_asr_topic").value
+        self.output_plan_topic = self.get_parameter("output_plan_topic").value
+        self.llm_request_topic = self.get_parameter("llm_request_topic").value
+        self.llm_response_topic = self.get_parameter("llm_response_topic").value
+        self.memory_context_topic = self.get_parameter("memory_context_topic").value
+        self.planner_config_path = self.get_parameter("planner_config_path").value
+        self.gate_config_path = self.get_parameter("gate_config_path").value
+        self.use_llm = self.get_parameter("use_llm").value
+        self.llm_timeout_sec = float(self.get_parameter("llm_timeout_sec").value)
+        self.debug_log = self.get_parameter("debug_log").value
+        self.default_session_id = self.get_parameter("default_session_id").value
+
+        # =====================================================================
+        # 2. 加载 planner 配置
+        # =====================================================================
+        self.get_logger().info(f"正在加载 Planner 配置: {self.planner_config_path}")
+        try:
+            self.runtime_config: PlannerRuntimeConfig = load_planner_runtime_config(
+                self.planner_config_path
+            )
+            self.get_logger().info(
+                f"Planner 模式: {self.runtime_config.planner_mode}, "
+                f"可用工具: {len(self.runtime_config.available_tools)} 个, "
+                f"工具描述: {len(self.runtime_config.tool_descriptions)} 个"
+            )
+        except FileNotFoundError:
+            self.get_logger().warn(
+                f"配置文件不存在: {self.planner_config_path}, 使用默认配置"
+            )
+            self.runtime_config = self._create_default_runtime_config()
+        except Exception as e:
+            self.get_logger().error(f"加载 planner 配置失败: {e}")
+            raise
+
+        # =====================================================================
+        # 3. 加载 Gate 规则配置（新增）
+        # =====================================================================
+        self.gate_rules = self.load_gate_rules()
+
+        # =====================================================================
+        # 4. 加载 Teacher Memory 模板（新增）
+        # =====================================================================
+        self.teaching_memory_templates = self.load_teaching_memory_templates()
+
+        # =====================================================================
+        # 5. 初始化 Planner 实例（用于 fallback 和可能的本地决策）
+        # =====================================================================
+        self.planner = Planner(
+            config=self.runtime_config.planner_config,
+            intent_to_action=self.runtime_config.intent_to_action,
+        )
+
+        # =====================================================================
+        # 6. 初始化 QoS 配置
+        # =====================================================================
+        qos = QoSProfile(
+            reliability=ReliabilityPolicy.RELIABLE,
+            history=HistoryPolicy.KEEP_LAST,
+            depth=10,
+        )
+
+        # =====================================================================
+        # 7. 创建 Publisher
+        # =====================================================================
+        self.plan_pub = self.create_publisher(String, self.output_plan_topic, qos)
+        self.llm_request_pub = self.create_publisher(String, self.llm_request_topic, qos)
+
+        # =====================================================================
+        # 8. 创建 Subscriber
+        # =====================================================================
+        self.world_sub = self.create_subscription(
+            String,
+            self.input_world_topic,
+            self.on_world_snapshot,
+            qos,
+        )
+        self.asr_sub = self.create_subscription(
+            String,
+            self.input_asr_topic,
+            self.on_asr,
+            qos,
+        )
+        self.llm_response_sub = self.create_subscription(
+            String,
+            self.llm_response_topic,
+            self.on_llm_response,
+            qos,
+        )
+        self.memory_context_sub = self.create_subscription(
+            String,
+            self.memory_context_topic,
+            self.on_memory_context,
+            qos,
+        )
+
+        # =====================================================================
+        # 9. 创建 Timer（用于 LLM 超时检测）
+        # =====================================================================
+        self.timer = self.create_timer(0.5, self._check_llm_timeout)
+
+        # =====================================================================
+        # 10. 本地缓存状态（支持最小打断和会话跟踪）
+        # =====================================================================
+        self.latest_world_snapshot: Optional[dict] = None
+        self.pending_user_text: Optional[str] = None       # 当前待处理的用户文本
+        self.pending_request_id: Optional[str] = None      # 当前有效 request_id
+        self.current_session_id: str = self.default_session_id
+        self.waiting_for_llm: bool = False
+        self.llm_request_timestamp: Optional[float] = None
+        self.last_request_time: Optional[float] = None     # 上次请求时间
+        self.has_pending_plan: bool = False               # 是否有待执行的 Plan
+
+        # =====================================================================
+        # 11. Memory 缓存（来自 MemoryNode）
+        # =====================================================================
+        self.latest_memory_context: Optional[dict] = None
+        self.latest_memory_text: str = ""
+
+        # =====================================================================
+        # 12. Gate 相关缓存（新增）
+        # =====================================================================
+        self.latest_world_facts: Optional[dict] = None  # 简化的 world facts
+
+        self.get_logger().info("Planner Node 初始化完成 (含 Gate 前置治理层)")
+        self.get_logger().info(f"  - World Topic: {self.input_world_topic}")
+        self.get_logger().info(f"  - ASR Topic: {self.input_asr_topic}")
+        self.get_logger().info(f"  - Plan Topic: {self.output_plan_topic}")
+        self.get_logger().info(f"  - LLM Request Topic: {self.llm_request_topic}")
+        self.get_logger().info(f"  - LLM Response Topic: {self.llm_response_topic}")
+        self.get_logger().info(f"  - Memory Context Topic: {self.memory_context_topic}")
+        self.get_logger().info(f"  - Gate Config: {self.gate_config_path}")
+        self.get_logger().info(f"  - Session ID: {self.current_session_id}")
+
+    # =========================================================================
+    # Gate 前置治理层 - 配置加载
+    # =========================================================================
+
+    def load_gate_rules(self) -> dict:
+        """加载 Gate 规则配置
+
+        从 gate_config_path 加载 Gate 规则配置。
+        配置不存在或解析失败时返回默认配置。
+
+        Returns:
+            dict: Gate 规则配置字典
+        """
+        default_rules = self._get_default_gate_rules()
+
+        if yaml is None:
+            self.get_logger().warn("PyYAML 未安装，使用默认 Gate 规则")
+            return default_rules
+
+        try:
+            # 尝试多种路径查找配置文件
+            config_paths = [
+                self.gate_config_path,
+                os.path.join(os.path.dirname(__file__), "config", "planner_gate_config.yaml"),
+                os.path.join(os.path.dirname(__file__), "..", "config", "planner_gate_config.yaml"),
+            ]
+
+            for config_path in config_paths:
+                if os.path.exists(config_path):
+                    with open(config_path, 'r', encoding='utf-8') as f:
+                        rules = yaml.safe_load(f)
+                    self.get_logger().info(f"成功加载 Gate 规则配置: {config_path}")
+                    
+                    # 确保 llm_prompts 存在（如果没有，使用默认值）
+                    if "llm_prompts" not in rules:
+                        rules["llm_prompts"] = self._get_default_llm_prompts()
+                        self.get_logger().warn("llm_prompts 配置缺失，使用默认配置")
+                    
+                    return rules
+
+            self.get_logger().warn(
+                f"Gate 配置文件未找到: {self.gate_config_path}, 使用默认配置"
+            )
+            return default_rules
+
+        except yaml.YAMLError as e:
+            self.get_logger().error(f"Gate 配置 YAML 解析失败: {e}, 使用默认配置")
+            return default_rules
+        except Exception as e:
+            self.get_logger().error(f"加载 Gate 配置时发生错误: {e}, 使用默认配置")
+            return default_rules
+
+    def _get_default_gate_rules(self) -> dict:
+        """获取默认 Gate 规则配置"""
+        return {
+            "scene": "default_robot",
+            "query_keywords": {
+                "position": ["你在哪里", "当前位置", "位置", "在哪"],
+                "status": ["电量", "状态", "当前状态"],
+                "count": ["几个", "数量", "多少"],
+                "weather": ["天气"],
+                "identity": ["我的名字", "你是谁"],
+            },
+            "action_keywords": {
+                "move": ["去", "前往", "移动", "走到"],
+                "feed": ["喂", "喂牛", "喂食", "喂养"],
+                "inspect": ["巡检", "检查", "查看", "视察"],
+                "open": ["打开", "开启", "启动"],
+                "close": ["关闭", "关掉", "停止"],
+                "adjust": ["调节", "调整", "设置"],
+            },
+            "complex_keywords": [
+                "如果", "否则", "先", "再", "然后", "顺便",
+                "完成后", "回来汇报", "不能去就", "改去"
+            ],
+            "missing_slot_rules": {
+                "open": "object", "close": "object", "adjust": "object",
+                "feed": "location", "move": "location", "inspect": "location",
+            },
+            "local_direct_handle": {
+                "enable_ask": True, "enable_confirm": True,
+                "enable_reject": True, "enable_already_there": True,
+            },
+            "complex_task_route": "cloud",
+            "enable_teacher_memory": True,
+            "task_type_routes": {
+                "ask": "local_direct", "confirm": "local_direct",
+                "reject": "local_direct", "already_there": "local_direct",
+                "query": "local_direct", "simple_task": "local_3b",
+                "complex_task": "cloud", "unknown": "local_3b",
+            },
+            "project_env": {
+                "standard_locations": ["广场", "大门", "充电站", "牛棚1号", "牛棚2号"],
+                "location_alias_map": {
+                    "牛鹏1号": "牛棚1号", "牛鹏2号": "牛棚2号",
+                    "牛栏1号": "牛棚1号", "牛栏2号": "牛棚2号",
+                },
+                "unknown_location_patterns": ["农场", "牛场", "外面", "那边"],
+                "known_objects": ["风扇", "灯", "灯光", "门", "空调"],
+                "unreachable_status": ["repairing", "unavailable", "unreachable", "offline"],
+                "robot_role": "智能机器人",
+                "robot_name": "农宝",
+                "location_allowed_skills": {
+                    "广场": ["move", "inspect"],
+                    "大门": ["move", "inspect"],
+                    "充电站": ["move", "charge", "inspect"],
+                    "牛棚1号": ["move", "feed", "inspect"],
+                    "牛棚2号": ["move", "feed", "inspect"],
+                },
+            },
+        }
+
+    def load_teaching_memory_templates(self) -> dict:
+        """加载 Teacher Memory 模板
+
+        从配置文件加载标准回复模板。
+        配置不存在或解析失败时返回默认模板。
+
+        Returns:
+            dict: Teacher Memory 模板字典
+        """
+        default_templates = self._get_default_teaching_memory_templates()
+
+        if yaml is None:
+            self.get_logger().warn("PyYAML 未安装，使用默认 Teacher Memory 模板")
+            return default_templates
+
+        try:
+            # 获取模板文件路径
+            templates_path = self.gate_rules.get("teacher_memory_templates_path", "teaching_memory_templates.yaml")
+
+            # 尝试多种路径
+            config_paths = [
+                templates_path,
+                os.path.join(os.path.dirname(__file__), "config", "teaching_memory_templates.yaml"),
+                os.path.join(os.path.dirname(__file__), "..", "config", "teaching_memory_templates.yaml"),
+            ]
+
+            for config_path in config_paths:
+                if os.path.exists(config_path):
+                    with open(config_path, 'r', encoding='utf-8') as f:
+                        templates = yaml.safe_load(f)
+                    self.get_logger().info(f"成功加载 Teacher Memory 模板: {config_path}")
+                    return templates.get("templates", default_templates)
+
+            self.get_logger().warn(
+                f"Teacher Memory 模板文件未找到, 使用默认模板"
+            )
+            return default_templates
+
+        except Exception as e:
+            self.get_logger().error(f"加载 Teacher Memory 模板失败: {e}, 使用默认模板")
+            return default_templates
+
+    def _get_default_teaching_memory_templates(self) -> dict:
+        """获取默认 Teacher Memory 模板"""
+        return {
+            "task.open.missing_object": {
+                "reply": "好的，请问您要打开什么设备？",
+                "command": "ask=object",
+                "source": "template",
+                "verified": True,
+                "response_type": "ask",
+            },
+            "task.close.missing_object": {
+                "reply": "好的，请问您要关闭什么设备？",
+                "command": "ask=object",
+                "source": "template",
+                "verified": True,
+                "response_type": "ask",
+            },
+            "task.adjust.missing_object": {
+                "reply": "好的，请问您要调节什么设备？",
+                "command": "ask=object",
+                "source": "template",
+                "verified": True,
+                "response_type": "ask",
+            },
+            "task.feed.missing_location": {
+                "reply": "好的，请告诉我要去哪个牛棚。",
+                "command": "ask=location",
+                "source": "template",
+                "verified": True,
+                "response_type": "ask",
+            },
+            "task.move.missing_location": {
+                "reply": "好的，请告诉我要去哪里。",
+                "command": "ask=location",
+                "source": "template",
+                "verified": True,
+                "response_type": "ask",
+            },
+            "task.inspect.missing_location": {
+                "reply": "好的，请告诉我要去哪个地点巡检。",
+                "command": "ask=location",
+                "source": "template",
+                "verified": True,
+                "response_type": "ask",
+            },
+            "task.feed.unreachable_location": {
+                "reply": "抱歉，该地点当前不可到达，不能执行这个任务。",
+                "command": "reject",
+                "source": "template",
+                "verified": True,
+                "response_type": "reject",
+            },
+            "task.move.unreachable_location": {
+                "reply": "抱歉，该地点当前不可到达，无法移动到那里。",
+                "command": "reject",
+                "source": "template",
+                "verified": True,
+                "response_type": "reject",
+            },
+            "task.move.already_there": {
+                "reply": "您当前已经在目标地点，无需移动。",
+                "command": "noop",
+                "source": "template",
+                "verified": True,
+                "response_type": "already_there",
+            },
+            "query.position": {
+                "reply": "我目前在广场。",
+                "command": "",
+                "source": "cloud_teacher",
+                "verified": True,
+                "response_type": "speak",
+            },
+            "query.status": {
+                "reply": "我当前电量充足，状态正常，随时可以工作。",
+                "command": "",
+                "source": "cloud_teacher",
+                "verified": True,
+                "response_type": "speak",
+            },
+        }
+
+    def _create_default_runtime_config(self) -> 'PlannerRuntimeConfig':
+        """创建默认运行时配置（当配置文件缺失时使用）"""
+        from planner_config_loader import PlannerRuntimeConfig as RuntimeConfigClass
+        from planner import PlannerConfig
+        from tool_protocol import RiskLevel
+
+        return RuntimeConfigClass(
+            planner_mode="hybrid",
+            planner_config=PlannerConfig(
+                default_risk_level=RiskLevel.LOW,
+                require_confirmation_on_medium_risk=True,
+                require_confirmation_on_high_risk=True,
+                max_plan_steps=10,
+                default_source="hybrid",
+            ),
+            available_tools=["feed", "adjust_fan", "speak", "query", "move", "inspect"],
+            tool_descriptions=[
+                {"name": "feed", "description": "用于执行喂食动作", "tool_call_type": "execute", "category": "action"},
+                {"name": "adjust_fan", "description": "用于调整风扇风力档位或开关状态", "tool_call_type": "execute", "category": "action"},
+                {"name": "speak", "description": "用于播报自然语言给用户", "tool_call_type": "speak", "category": "dialog"},
+                {"name": "query", "description": "用于查询机器人系统内部状态", "tool_call_type": "query_world", "category": "query"},
+                {"name": "move", "description": "用于控制机器人移动到指定位置", "tool_call_type": "execute", "category": "action"},
+                {"name": "inspect", "description": "用于巡检、查看环境或检查状态", "tool_call_type": "execute", "category": "action"},
+            ],
+            intent_to_action={
+                "喂": "feed",
+                "喂食": "feed",
+                "降温": "adjust_fan",
+                "升温": "adjust_fan",
+                "打开风扇": "adjust_fan",
+                "关闭风扇": "adjust_fan",
+                "播报": "speak",
+            },
+            domain_rules={
+                "high_risk_actions": ["turn_off", "emergency_stop"],
+                "medium_risk_actions": ["adjust_fan", "adjust"],
+                "confirmation_rules": {},
+            },
+        )
+
+    # =========================================================================
+    # Gate 前置治理层 - World Facts 提取
+    # =========================================================================
+
+    def extract_world_facts(self, snapshot: Optional[dict] = None) -> dict:
+        """从 world_snapshot 提取简化的 facts
+
+        用于 Gate 规则判断，不需要完整的 world description。
+
+        Args:
+            snapshot: 世界状态快照，如果为 None 则使用缓存的 latest_world_snapshot
+
+        Returns:
+            dict: 简化的 facts，包含：
+                - current_location: str - 当前位置
+                - reachable_locations: list - 可达地点列表
+                - unreachable_locations: list - 不可达地点列表
+                - location_status: dict - 地点状态映射
+                - location_accessible: dict - 地点可达性映射
+                - location_skills: dict - 地点可用技能映射
+                - available_skills: list - 全局技能集合
+                - available_tools: list - 可用工具
+        """
+        if snapshot is None:
+            snapshot = self.latest_world_snapshot
+
+        facts = {
+            "current_location": None,
+            "reachable_locations": [],
+            "unreachable_locations": [],
+            "location_status": {},
+            "location_accessible": {},
+            "location_skills": {},
+            "available_skills": [],
+            "available_tools": self.runtime_config.available_tools if hasattr(self, 'runtime_config') else [],
+        }
+
+        if not snapshot:
+            return facts
+
+        try:
+            project_env = self.gate_rules.get("project_env", {})
+            standard_locations = project_env.get("standard_locations", [])
+            location_alias_map = project_env.get("location_alias_map", {})
+            unreachable_status = project_env.get("unreachable_status", [])
+            location_allowed_skills_config = project_env.get("location_allowed_skills", {})
+
+            # 判断是 WorldModel 格式还是旧格式
+            if "entities" in snapshot:
+                facts = self._extract_world_facts_v2(
+                    snapshot, facts, standard_locations, location_alias_map,
+                    unreachable_status, location_allowed_skills_config
+                )
+            else:
+                facts = self._extract_world_facts_v1(
+                    snapshot, facts, standard_locations, location_alias_map,
+                    unreachable_status, location_allowed_skills_config
+                )
+
+            # 合并所有地点技能为全局技能集合
+            all_skills = set()
+            for skills in facts["location_skills"].values():
+                if skills:
+                    all_skills.update(skills)
+            facts["available_skills"] = sorted(list(all_skills))
+
+            self.latest_world_facts = facts
+
+        except Exception as e:
+            if self.debug_log:
+                self.get_logger().warn(f"提取 world facts 失败: {e}")
+
+        return facts
+
+    def _extract_world_facts_v1(self, snapshot: dict, facts: dict,
+                                  standard_locations: list, location_alias_map: dict,
+                                  unreachable_status: list,
+                                  location_allowed_skills_config: dict) -> dict:
+        """解析旧格式 snapshot 提取 facts"""
+        # 提取 POI 地点
+        pois = snapshot.get("poi", [])
+        for poi in pois:
+            name = poi.get("name", poi.get("entity_id", ""))
+            index = poi.get("index", "")
+            location_name = f"{name}（{index}号）" if index else name
+
+            # 检查是否当前地点
+            if poi.get("is_current_location", False):
+                facts["current_location"] = location_name
+
+            # 检查可达性
+            properties = poi.get("properties", {})
+            accessible = properties.get("accessible", True)
+            status = poi.get("status", "available")
+
+            facts["location_status"][location_name] = status
+            facts["location_accessible"][location_name] = accessible
+
+            # 提取 allowed_skills（优先从 world snapshot，备用从配置回填）
+            allowed_skills = poi.get("allowed_skills", [])
+            if not allowed_skills and location_name in location_allowed_skills_config:
+                allowed_skills = location_allowed_skills_config.get(location_name, [])
+            facts["location_skills"][location_name] = allowed_skills
+
+            # 同时注册别名映射的技能
+            for alias, standard in location_alias_map.items():
+                if alias == name or alias == location_name:
+                    if standard not in facts["location_skills"]:
+                        standard_skills = poi.get("allowed_skills", [])
+                        if not standard_skills and standard in location_allowed_skills_config:
+                            standard_skills = location_allowed_skills_config.get(standard, [])
+                        facts["location_skills"][standard] = standard_skills
+
+            if status in unreachable_status or not accessible:
+                facts["unreachable_locations"].append(location_name)
+            else:
+                facts["reachable_locations"].append(location_name)
+
+        # 提取机器人状态
+        robot = snapshot.get("robot", [])
+        if robot and facts["current_location"] is None:
+            robot_info = robot[0] if isinstance(robot, list) else robot
+            # 如果 robot 有位置信息
+            position = robot_info.get("position", {})
+            if isinstance(position, dict) and position.get("is_current", False):
+                facts["current_location"] = "当前位置"
+
+        return facts
+
+    def _extract_world_facts_v2(self, snapshot: dict, facts: dict,
+                                  standard_locations: list, location_alias_map: dict,
+                                  unreachable_status: list,
+                                  location_allowed_skills_config: dict) -> dict:
+        """解析 WorldModel 格式 snapshot 提取 facts"""
+        entities = snapshot.get("entities", {})
+
+        # 按 type 分组
+        by_type = {}
+        for entity_id, entity_data in entities.items():
+            entity_type = entity_data.get("type", "unknown")
+            if entity_type not in by_type:
+                by_type[entity_type] = []
+            by_type[entity_type].append((entity_id, entity_data))
+
+        # 提取 POI
+        pois = by_type.get("poi", [])
+        for entity_id, entity_data in pois:
+            state = entity_data.get("state", {})
+            name = state.get("name", entity_id)
+            index = state.get("index", "")
+            location_name = f"{name}（{index}号）" if index else name
+
+            if state.get("is_current_location", False):
+                facts["current_location"] = location_name
+
+            properties = state.get("properties", {})
+            accessible = properties.get("accessible", True)
+            status = state.get("status", "available")
+
+            facts["location_status"][location_name] = status
+            facts["location_accessible"][location_name] = accessible
+
+            # 提取 allowed_skills（优先从 world snapshot，备用从配置回填）
+            allowed_skills = state.get("allowed_skills", [])
+            if not allowed_skills and location_name in location_allowed_skills_config:
+                allowed_skills = location_allowed_skills_config.get(location_name, [])
+            facts["location_skills"][location_name] = allowed_skills
+
+            # 同时注册别名映射的技能
+            for alias, standard in location_alias_map.items():
+                if alias == name or alias == location_name:
+                    if standard not in facts["location_skills"]:
+                        standard_skills = state.get("allowed_skills", [])
+                        if not standard_skills and standard in location_allowed_skills_config:
+                            standard_skills = location_allowed_skills_config.get(standard, [])
+                        facts["location_skills"][standard] = standard_skills
+
+            if status in unreachable_status or not accessible:
+                facts["unreachable_locations"].append(location_name)
+            else:
+                facts["reachable_locations"].append(location_name)
+
+        return facts
+
+    # =========================================================================
+    # Gate 前置治理层 - Gate0: 基础解析
+    # =========================================================================
+
+    def classify_user_request(self, user_text: str) -> dict:
+        """Gate0: 基础解析 - 对用户输入做轻量结构化解析
+
+        不依赖大模型，规则优先。
+
+        Args:
+            user_text: 用户原始文本
+
+        Returns:
+            dict: 解析结果，包含：
+                - raw_text: 原始文本
+                - normalized_text: 归一化文本
+                - intent: task / query / unknown
+                - action: move / feed / inspect / open / close / adjust / query / unknown
+                - location: str or None
+                - object: str or None
+                - quantity: str or None
+                - has_condition: bool
+                - condition_type: conditional / multistep / none
+                - is_complex: bool
+                - missing_slot: location / object / None
+                - task_type: query / ask / confirm / reject / already_there /
+                             simple_task / complex_task / unknown
+                - reason: str
+        """
+        result = {
+            "raw_text": user_text,
+            "normalized_text": user_text,
+            "intent": "unknown",
+            "action": "unknown",
+            "location": None,
+            "object": None,
+            "quantity": None,
+            "has_condition": False,
+            "condition_type": "none",
+            "is_complex": False,
+            "missing_slot": None,
+            "task_type": "unknown",
+            "reason": "",
+            "teacher_memory_key": "",
+            "query_subtype": None,
+        }
+
+        try:
+            # 1. 槽位提取
+            slot_info = self.extract_basic_slots(user_text)
+            result.update(slot_info)
+
+            # 2. 判断是否为查询类
+            if result["intent"] == "query":
+                result["task_type"] = "query"
+                result["reason"] = f"查询类: {result.get('query_subtype', 'unknown')}"
+                result["teacher_memory_key"] = f"query.{result.get('query_subtype', 'unknown')}"
+                return result
+
+            # 3. 提取归一化文本
+            result["normalized_text"] = self.generate_normalized_text(user_text, result)
+
+            # 4. 判断复杂性
+            if self._is_complex_request(user_text):
+                result["has_condition"] = True
+                result["is_complex"] = True
+                result["task_type"] = "complex_task"
+                result["condition_type"] = self._detect_condition_type(user_text)
+                result["reason"] = f"复杂任务: {result['condition_type']}"
+                result["teacher_memory_key"] = f"task.complex.{result['condition_type']}"
+
+            # 5. 判断缺槽
+            elif result["action"] != "unknown" and result["action"] != "query":
+                missing_slot = self._check_missing_slot(result["action"], result)
+                if missing_slot:
+                    result["missing_slot"] = missing_slot
+                    result["task_type"] = "ask"
+                    result["reason"] = f"缺槽: {missing_slot}"
+                    result["teacher_memory_key"] = f"task.{result['action']}.missing_{missing_slot}"
+
+            # 6. 最终 task_type 决策
+            if result["task_type"] == "unknown" and result["action"] != "unknown":
+                result["task_type"] = "simple_task"
+                result["reason"] = f"标准任务: {result['action']}"
+                result["teacher_memory_key"] = f"task.{result['action']}.standard"
+
+            if self.debug_log:
+                self.get_logger().debug(
+                    f"[Gate0] 解析结果: action={result['action']}, "
+                    f"task_type={result['task_type']}, "
+                    f"location={result['location']}, "
+                    f"missing_slot={result['missing_slot']}"
+                )
+
+        except Exception as e:
+            self.get_logger().warn(f"[Gate0] 解析失败: {e}")
+            result["reason"] = f"解析异常: {e}"
+
+        return result
+
+    def extract_basic_slots(self, user_text: str) -> dict:
+        """基础槽位抽取 - 从用户文本中提取 action/location/object
+
+        规则优先，从 gate_rules 读取动作词、查询词等。
+
+        Args:
+            user_text: 用户原始文本
+
+        Returns:
+            dict: 槽位信息
+        """
+        slot_info = {
+            "intent": "unknown",
+            "action": "unknown",
+            "location": None,
+            "object": None,
+            "quantity": None,
+            "query_subtype": None,
+        }
+
+        gate_rules = self.gate_rules
+
+        # 1. 检查查询类关键词
+        query_keywords = gate_rules.get("query_keywords", {})
+        for subtype, keywords in query_keywords.items():
+            for keyword in keywords:
+                if keyword in user_text:
+                    slot_info["intent"] = "query"
+                    slot_info["action"] = "query"
+                    slot_info["query_subtype"] = subtype
+                    return slot_info
+
+        # 2. 检查动作类关键词
+        action_keywords = gate_rules.get("action_keywords", {})
+        for action, keywords in action_keywords.items():
+            for keyword in keywords:
+                if keyword in user_text:
+                    slot_info["intent"] = "task"
+                    slot_info["action"] = action
+                    break
+            if slot_info["action"] != "unknown":
+                break
+
+        # 3. 提取 location
+        slot_info["location"] = self._extract_location(user_text)
+
+        # 4. 提取 object
+        slot_info["object"] = self._extract_object(user_text)
+
+        # 5. 提取 quantity（简单实现）
+        quantity_match = re.search(r'(\d+)\s*(个|只|次|台|号)?', user_text)
+        if quantity_match:
+            slot_info["quantity"] = quantity_match.group(0)
+
+        return slot_info
+
+    def _extract_location(self, user_text: str) -> Optional[str]:
+        """从文本中提取 location
+
+        优先从标准地点和别名中匹配。
+        """
+        project_env = self.gate_rules.get("project_env", {})
+        standard_locations = project_env.get("standard_locations", [])
+        location_alias_map = project_env.get("location_alias_map", {})
+
+        # 先检查别名
+        for alias, standard in location_alias_map.items():
+            if alias in user_text:
+                return standard
+
+        # 再检查标准地点
+        for location in standard_locations:
+            if location in user_text:
+                return location
+
+        return None
+
+    def _extract_object(self, user_text: str) -> Optional[str]:
+        """从文本中提取 object"""
+        project_env = self.gate_rules.get("project_env", {})
+        known_objects = project_env.get("known_objects", [])
+
+        for obj in known_objects:
+            if obj in user_text:
+                return obj
+
+        return None
+
+    def _is_complex_request(self, user_text: str) -> bool:
+        """判断是否为复杂请求"""
+        complex_keywords = self.gate_rules.get("complex_keywords", [])
+
+        for keyword in complex_keywords:
+            if keyword in user_text:
+                return True
+
+        return False
+
+    def _detect_condition_type(self, user_text: str) -> str:
+        """检测条件类型"""
+        conditional_keywords = ["如果", "否则", "不能去就", "改去"]
+        multistep_keywords = ["先", "再", "然后", "顺便", "完成后", "回来汇报"]
+
+        for keyword in conditional_keywords:
+            if keyword in user_text:
+                return "conditional"
+
+        for keyword in multistep_keywords:
+            if keyword in user_text:
+                return "multistep"
+
+        return "conditional"  # 默认
+
+    def _check_missing_slot(self, action: str, slot_info: dict) -> Optional[str]:
+        """检查是否缺槽"""
+        missing_slot_rules = self.gate_rules.get("missing_slot_rules", {})
+
+        required_slot = missing_slot_rules.get(action)
+        if not required_slot:
+            return None
+
+        # 检查该槽位是否缺失
+        if required_slot == "object" and not slot_info.get("object"):
+            return "object"
+        elif required_slot == "location" and not slot_info.get("location"):
+            return "location"
+
+        return None
+
+    def generate_normalized_text(self, user_text: str, slot_info: dict) -> str:
+        """生成归一化文本
+
+        用于生成 Teacher Memory Key。
+        做最基本的归一化：去掉语气词、保留核心语义。
+
+        Args:
+            user_text: 原始文本
+            slot_info: 槽位信息
+
+        Returns:
+            str: 归一化后的文本
+        """
+        # 基本归一化规则
+        normalized = user_text.strip()
+
+        # 去除语气词和助词
+        filler_words = ["一下", "好吗", "帮我", "麻烦", "请", "能不能", "可以"]
+        for word in filler_words:
+            normalized = normalized.replace(word, "")
+
+        # 去除多余空格
+        normalized = re.sub(r'\s+', '', normalized)
+
+        return normalized if normalized else user_text
+
+    # =========================================================================
+    # Gate 前置治理层 - Gate1/Gate2/Gate3: 边界判断和复杂识别
+    # =========================================================================
+
+    def evaluate_local_gates(self, intent_info: dict, world_facts: dict) -> dict:
+        """Gate1/Gate2/Gate3 统一评估
+
+        评估是否需要本地处理。
+
+        Args:
+            intent_info: classify_user_request 的输出
+            world_facts: extract_world_facts 的输出
+
+        Returns:
+            dict: 评估结果，包含：
+                - handled: bool - 是否已被 Gate 处理
+                - route: local_direct / local_3b / cloud
+                - local_action: ask / confirm / reject / already_there / None
+                - reason: str - 处理原因
+                - memory_key: str - Teacher Memory Key
+        """
+        result = {
+            "handled": False,
+            "route": "local_3b",  # 默认走 LLM
+            "local_action": None,
+            "reason": "",
+            "memory_key": intent_info.get("teacher_memory_key", ""),
+            "location_match_type": "none",
+        }
+
+        try:
+            action = intent_info.get("action", "unknown")
+            task_type = intent_info.get("task_type", "unknown")
+            location = intent_info.get("location")
+            missing_slot = intent_info.get("missing_slot")
+
+            # =================================================================
+            # Gate1: 表达完整性过滤 - 缺槽拦截
+            # =================================================================
+            if missing_slot:
+                if self.gate_rules.get("local_direct_handle", {}).get("enable_ask", True):
+                    result["handled"] = True
+                    result["route"] = "local_direct"
+                    result["local_action"] = "ask"
+                    result["reason"] = f"Gate1-缺槽: {missing_slot}"
+                    result["memory_key"] = f"task.{action}.missing_{missing_slot}"
+                    return result
+
+            # =================================================================
+            # Gate3: 复杂任务识别
+            # =================================================================
+            if task_type == "complex_task":
+                result["route"] = "cloud"
+                result["reason"] = "Gate3-复杂任务"
+                return result
+
+            # =================================================================
+            # Gate2: 边界判断
+            # =================================================================
+            if location:
+                # 地点匹配类型判断
+                location_match_type = self.detect_location_match_type(location, world_facts)
+                result["location_match_type"] = location_match_type
+
+                # 非标准地点确认
+                if location_match_type == "alias_like":
+                    if self.gate_rules.get("local_direct_handle", {}).get("enable_confirm", True):
+                        result["handled"] = True
+                        result["route"] = "local_direct"
+                        result["local_action"] = "confirm"
+                        result["reason"] = "Gate2-别名地点确认"
+                        result["memory_key"] = f"task.{action}.alias_like_location"
+                        return result
+
+                # 未知地点确认
+                elif location_match_type == "unknown":
+                    if self.gate_rules.get("local_direct_handle", {}).get("enable_confirm", True):
+                        result["handled"] = True
+                        result["route"] = "local_direct"
+                        result["local_action"] = "confirm"
+                        result["reason"] = "Gate2-未知地点"
+                        result["memory_key"] = f"task.{action}.unknown_location"
+                        return result
+
+                # 不可达地点拒绝
+                elif location_match_type in ["unreachable", "not_accessible"]:
+                    if self.gate_rules.get("local_direct_handle", {}).get("enable_reject", True):
+                        result["handled"] = True
+                        result["route"] = "local_direct"
+                        result["local_action"] = "reject"
+                        result["reason"] = "Gate2-不可达地点"
+                        result["memory_key"] = f"task.{action}.unreachable_location"
+                        return result
+
+            # =================================================================
+            # 特殊场景：已在目标点
+            # =================================================================
+            if action == "move" and location:
+                current_loc = world_facts.get("current_location")
+                if current_loc and self._normalize_location(current_loc) == self._normalize_location(location):
+                    if self.gate_rules.get("local_direct_handle", {}).get("enable_already_there", True):
+                        result["handled"] = True
+                        result["route"] = "local_direct"
+                        result["local_action"] = "already_there"
+                        result["reason"] = "Gate2-已在目标点"
+                        result["memory_key"] = f"task.{action}.already_there"
+                        return result
+
+            # =================================================================
+            # 查询类直接本地处理
+            # =================================================================
+            if task_type == "query":
+                result["handled"] = True
+                result["route"] = "local_direct"
+                result["local_action"] = "query"
+                result["reason"] = "Gate0-查询类"
+                return result
+
+        except Exception as e:
+            self.get_logger().warn(f"[Gate1-3] 评估异常: {e}")
+            result["reason"] = f"评估异常: {e}"
+
+        return result
+
+    def detect_location_match_type(self, location_text: str, world_facts: dict) -> str:
+        """Gate2: 判断地点匹配类型
+
+        Args:
+            location_text: 地点文本
+            world_facts: 世界 facts
+
+        Returns:
+            str: standard / alias_like / unknown / none
+        """
+        if not location_text:
+            return "none"
+
+        project_env = self.gate_rules.get("project_env", {})
+        standard_locations = project_env.get("standard_locations", [])
+        location_alias_map = project_env.get("location_alias_map", {})
+        unknown_patterns = project_env.get("unknown_location_patterns", [])
+        unreachable_status = project_env.get("unreachable_status", [])
+
+        # 归一化地点
+        normalized = self._normalize_location(location_text)
+
+        # 1. 检查是否为标准地点
+        for loc in standard_locations:
+            if self._normalize_location(loc) == normalized:
+                # 检查是否可达
+                location_status = world_facts.get("location_status", {})
+                if location_text in location_status:
+                    status = location_status[location_text]
+                    if status in unreachable_status:
+                        return "unreachable"
+                if location_text in world_facts.get("unreachable_locations", []):
+                    return "unreachable"
+                return "standard"
+
+        # 2. 检查是否为别名
+        for alias, standard in location_alias_map.items():
+            if alias in location_text or location_text in alias:
+                # 别名映射到标准地点后再次检查
+                for loc in standard_locations:
+                    if self._normalize_location(loc) == self._normalize_location(standard):
+                        return "alias_like"
+
+        # 3. 检查是否为未知地点模式
+        for pattern in unknown_patterns:
+            if pattern in location_text:
+                return "unknown"
+
+        # 4. 检查是否不可达（不在标准地点列表中但有状态信息）
+        location_status = world_facts.get("location_status", {})
+        if location_text in location_status:
+            status = location_status[location_text]
+            if status in unreachable_status:
+                return "unreachable"
+            return "unknown"
+
+        return "unknown"
+
+    def _normalize_location(self, location: str) -> str:
+        """归一化地点名称"""
+        if not location:
+            return ""
+
+        # 移除括号内容
+        normalized = re.sub(r'[（(].*?[）)]', '', location)
+
+        # 移除空格
+        normalized = normalized.replace(" ", "").replace("号", "")
+
+        # 统一数字格式
+        normalized = normalized.replace("1号", "1号").replace("一号", "1号")
+        normalized = normalized.replace("2号", "2号").replace("二号", "2号")
+
+        return normalized
+
+    # =========================================================================
+    # Gate 前置治理层 - Gate4: Teacher Memory
+    # =========================================================================
+
+    def build_teacher_memory_key(self, intent_info: dict, world_facts: dict) -> str:
+        """Gate4: 生成 Teacher Memory Key
+
+        K = normalized_text|action|location_context|condition_flag
+
+        Args:
+            intent_info: 意图信息
+            world_facts: 世界 facts
+
+        Returns:
+            str: Memory Key
+        """
+        normalized_text = intent_info.get("normalized_text", "")
+        action = intent_info.get("action", "unknown")
+        location = intent_info.get("location")
+        has_condition = intent_info.get("has_condition", False)
+        condition_type = intent_info.get("condition_type", "none")
+
+        # location_context 逻辑
+        location_context = "none"
+        if location:
+            current_loc = world_facts.get("current_location")
+            # 只有当前地点与目标地点相关时才放
+            if current_loc and self._normalize_location(current_loc) == self._normalize_location(location):
+                location_context = location
+            else:
+                location_context = "none"  # 默认不放当前地点
+
+        # condition_flag
+        condition_flag = "none"
+        if has_condition:
+            condition_flag = condition_type
+
+        # 组合 Key
+        key_parts = [normalized_text, action, location_context, condition_flag]
+        return "|".join(str(p) for p in key_parts)
+
+    def lookup_teacher_memory(self, memory_key: str, fallback_key: str = "") -> Optional[dict]:
+        """Gate4: 查找 Teacher Memory
+
+        优先精确匹配，次选 fallback_key 匹配。
+
+        Args:
+            memory_key: 精确的 Memory Key
+            fallback_key: 回退用的 Key（如 task.move.missing_location）
+
+        Returns:
+            dict or None: 命中的模板
+        """
+        templates = self.teaching_memory_templates
+
+        # 1. 精确 Key 查找
+        if memory_key and memory_key in templates:
+            if self.debug_log:
+                self.get_logger().debug(f"[Gate4] 精确命中: {memory_key}")
+            return templates[memory_key]
+
+        # 2. Fallback Key 查找
+        if fallback_key and fallback_key in templates:
+            if self.debug_log:
+                self.get_logger().debug(f"[Gate4] Fallback命中: {fallback_key}")
+            return templates[fallback_key]
+
+        # 3. 查询类特殊处理
+        if fallback_key.startswith("query."):
+            subtype = fallback_key.replace("query.", "")
+            query_key = f"query.{subtype}"
+            if query_key in templates:
+                if self.debug_log:
+                    self.get_logger().debug(f"[Gate4] 查询类命中: {query_key}")
+                return templates[query_key]
+
+        if self.debug_log:
+            self.get_logger().debug(f"[Gate4] 未命中 Memory Key: {memory_key}, fallback: {fallback_key}")
+
+        return None
+
+    # =========================================================================
+    # Gate 前置治理层 - Gate5: 路由决策
+    # =========================================================================
+
+    def decide_route(self, intent_info: dict, gate_result: dict,
+                     memory_hit: Optional[dict]) -> str:
+        """Gate5: 统一决定路由
+
+        Args:
+            intent_info: 意图信息
+            gate_result: Gate1-3 评估结果
+            memory_hit: Teacher Memory 命中结果
+
+        Returns:
+            str: local_direct / local_3b / cloud
+        """
+        # 已被 Gate 处理，直接返回
+        if gate_result.get("handled"):
+            return gate_result.get("route", "local_direct")
+
+        task_type = intent_info.get("task_type", "unknown")
+
+        # 查询类直接本地
+        if task_type == "query":
+            return "local_direct"
+
+        # 复杂任务走云端
+        if task_type == "complex_task":
+            return "cloud"
+
+        # 根据 task_type 获取路由
+        task_type_routes = self.gate_rules.get("task_type_routes", {})
+        route = task_type_routes.get(task_type, "local_3b")
+
+        return route
+
+    # =========================================================================
+    # Gate 前置治理层 - 本地处理
+    # =========================================================================
+
+    def try_handle_locally(self, intent_info: dict, gate_result: dict,
+                           memory_hit: Optional[dict]) -> Optional['Plan']:
+        """尝试本地处理请求
+
+        根据 Gate 结果生成本地安全 Plan。
+
+        Args:
+            intent_info: 意图信息
+            gate_result: Gate1-3 评估结果
+            memory_hit: Teacher Memory 命中结果
+
+        Returns:
+            Plan or None: 本地生成的 Plan
+        """
+        local_action = gate_result.get("local_action")
+
+        if not local_action:
+            return None
+
+        try:
+            # 优先使用 memory_hit 的 reply
+            reply_text = ""
+            if memory_hit and memory_hit.get("reply"):
+                reply_text = memory_hit["reply"]
+            else:
+                reply_text = self._get_default_reply(local_action, intent_info)
+
+            # 根据 action_type 生成不同类型的 Plan
+            if local_action == "ask":
+                return self._build_ask_plan(intent_info, reply_text, gate_result, memory_hit)
+            elif local_action == "confirm":
+                return self._build_confirm_plan(intent_info, reply_text, gate_result, memory_hit)
+            elif local_action == "reject":
+                return self._build_reject_plan(intent_info, reply_text, gate_result, memory_hit)
+            elif local_action == "already_there":
+                return self._build_already_there_plan(intent_info, reply_text, gate_result, memory_hit)
+            elif local_action == "query":
+                return self._build_query_plan(intent_info, reply_text, gate_result, memory_hit)
+
+        except Exception as e:
+            self.get_logger().error(f"[try_handle_locally] 生成 Plan 失败: {e}")
+
+        return None
+
+    def _get_default_reply(self, action_type: str, intent_info: dict) -> str:
+        """获取默认回复"""
+        action = intent_info.get("action", "unknown")
+
+        defaults = {
+            "ask": f"好的，请问您要{action}什么？",
+            "confirm": "我无法确认您说的地点，请您再说明一下。",
+            "reject": "抱歉，无法执行这个任务。",
+            "already_there": "您当前已经在目标地点。",
+            "query": "抱歉，我暂时无法回答这个问题。",
+        }
+
+        return defaults.get(action_type, "好的，我明白了。")
+
+    def _build_ask_plan(self, intent_info: dict, reply_text: str,
+                        gate_result: dict, memory_hit: Optional[dict]) -> 'Plan':
+        """生成 ask 类型 Plan"""
+        plan_id = f"plan_gate_ask_{uuid.uuid4().hex[:8]}"
+        action = intent_info.get("action", "unknown")
+        missing_slot = intent_info.get("missing_slot", "object")
+
+        plan = Plan(
+            plan_id=plan_id,
+            goal=f"询问用户补充信息: {action}",
+            reasoning=f"Gate前置治理: 缺{missing_slot}，本地询问",
+            risk_level=RiskLevel.LOW,
+            requires_confirmation=False,
+            confirmation_message=None,
+            status=PlanStatus.CREATED,
+            source="gate_local_direct",
+            metadata={"gate": True, "local_action": "ask", "reason": gate_result.get("reason", "")},
+        )
+
+        # 生成询问问题
+        question = reply_text if reply_text else f"请问您要{action}什么？"
+
+        step = PlanStep(
+            step_id=1,
+            action="ask_user",
+            tool_call_type=ToolCallType.ASK_USER,
+            parameters={
+                "question": question,
+                "original_intent": intent_info.get("raw_text", ""),
+                "missing_slot": missing_slot,
+                "action": action,
+            },
+            preconditions={},
+            fallback=None,
+            status=StepStatus.PENDING,
+            description=f"询问用户补充{missing_slot}",
+            requires_confirmation=False,
+            confirmation_message=None,
+            metadata={"gate_source": "local_direct"},
+        )
+        plan.add_step(step)
+
+        return plan
+
+    def _build_confirm_plan(self, intent_info: dict, reply_text: str,
+                            gate_result: dict, memory_hit: Optional[dict]) -> 'Plan':
+        """生成 confirm 类型 Plan"""
+        plan_id = f"plan_gate_confirm_{uuid.uuid4().hex[:8]}"
+        action = intent_info.get("action", "unknown")
+        location = intent_info.get("location", "该地点")
+
+        plan = Plan(
+            plan_id=plan_id,
+            goal=f"确认地点: {location}",
+            reasoning=f"Gate前置治理: 地点需确认，本地询问",
+            risk_level=RiskLevel.LOW,
+            requires_confirmation=False,
+            confirmation_message=None,
+            status=PlanStatus.CREATED,
+            source="gate_local_direct",
+            metadata={"gate": True, "local_action": "confirm", "reason": gate_result.get("reason", "")},
+        )
+
+        # 从 memory_hit 获取确认目标
+        confirmation_target = location
+        if memory_hit and memory_hit.get("confirmation_target"):
+            confirmation_target = memory_hit["confirmation_target"]
+
+        question = reply_text if reply_text else f"我理解您说的是{confirmation_target}，请确认是否正确？"
+
+        step = PlanStep(
+            step_id=1,
+            action="ask_user",
+            tool_call_type=ToolCallType.ASK_USER,
+            parameters={
+                "question": question,
+                "original_intent": intent_info.get("raw_text", ""),
+                "confirmation_target": confirmation_target,
+                "action": action,
+            },
+            preconditions={},
+            fallback=None,
+            status=StepStatus.PENDING,
+            description=f"确认地点: {confirmation_target}",
+            requires_confirmation=False,
+            confirmation_message=None,
+            metadata={"gate_source": "local_direct"},
+        )
+        plan.add_step(step)
+
+        return plan
+
+    def _build_reject_plan(self, intent_info: dict, reply_text: str,
+                           gate_result: dict, memory_hit: Optional[dict]) -> 'Plan':
+        """生成 reject 类型 Plan"""
+        plan_id = f"plan_gate_reject_{uuid.uuid4().hex[:8]}"
+        action = intent_info.get("action", "unknown")
+        location = intent_info.get("location", "目标地点")
+
+        plan = Plan(
+            plan_id=plan_id,
+            goal=f"拒绝任务: {action} 到 {location}",
+            reasoning=f"Gate前置治理: 不可达地点，本地拒绝",
+            risk_level=RiskLevel.LOW,
+            requires_confirmation=False,
+            confirmation_message=None,
+            status=PlanStatus.CREATED,
+            source="gate_local_direct",
+            metadata={"gate": True, "local_action": "reject", "reason": gate_result.get("reason", "")},
+        )
+
+        # 使用 ask_user 风格的安全提示
+        message = reply_text if reply_text else f"抱歉，{location}当前不可到达，无法执行{action}任务。"
+
+        step = PlanStep(
+            step_id=1,
+            action="ask_user",
+            tool_call_type=ToolCallType.ASK_USER,
+            parameters={
+                "question": message,
+                "original_intent": intent_info.get("raw_text", ""),
+                "rejected": True,
+                "reason": gate_result.get("reason", ""),
+            },
+            preconditions={},
+            fallback=None,
+            status=StepStatus.PENDING,
+            description=f"安全拒绝: {location}不可达",
+            requires_confirmation=False,
+            confirmation_message=None,
+            metadata={"gate_source": "local_direct"},
+        )
+        plan.add_step(step)
+
+        return plan
+
+    def _build_already_there_plan(self, intent_info: dict, reply_text: str,
+                                   gate_result: dict, memory_hit: Optional[dict]) -> 'Plan':
+        """生成 already_there 类型 Plan"""
+        plan_id = f"plan_gate_already_{uuid.uuid4().hex[:8]}"
+        action = intent_info.get("action", "unknown")
+        location = intent_info.get("location", "目标地点")
+
+        plan = Plan(
+            plan_id=plan_id,
+            goal=f"无需执行: 已在{location}",
+            reasoning=f"Gate前置治理: 已在目标点，无需移动",
+            risk_level=RiskLevel.LOW,
+            requires_confirmation=False,
+            confirmation_message=None,
+            status=PlanStatus.CREATED,
+            source="gate_local_direct",
+            metadata={"gate": True, "local_action": "already_there", "reason": gate_result.get("reason", "")},
+        )
+
+        message = reply_text if reply_text else f"您当前已经在{location}，无需执行{action}。"
+
+        step = PlanStep(
+            step_id=1,
+            action="ask_user",
+            tool_call_type=ToolCallType.ASK_USER,
+            parameters={
+                "question": message,
+                "original_intent": intent_info.get("raw_text", ""),
+                "no_action_needed": True,
+            },
+            preconditions={},
+            fallback=None,
+            status=StepStatus.PENDING,
+            description=f"已在目标点: {location}",
+            requires_confirmation=False,
+            confirmation_message=None,
+            metadata={"gate_source": "local_direct"},
+        )
+        plan.add_step(step)
+
+        return plan
+
+    def _build_query_plan(self, intent_info: dict, reply_text: str,
+                          gate_result: dict, memory_hit: Optional[dict]) -> 'Plan':
+        """生成 query 类型 Plan（本地回答查询）"""
+        plan_id = f"plan_gate_query_{uuid.uuid4().hex[:8]}"
+        query_subtype = intent_info.get("query_subtype", "unknown")
+
+        plan = Plan(
+            plan_id=plan_id,
+            goal=f"回答查询: {query_subtype}",
+            reasoning=f"Gate前置治理: 查询类请求，本地回答",
+            risk_level=RiskLevel.LOW,
+            requires_confirmation=False,
+            confirmation_message=None,
+            status=PlanStatus.CREATED,
+            source="gate_local_direct",
+            metadata={"gate": True, "local_action": "query", "query_subtype": query_subtype},
+        )
+
+        # 从 world_facts 获取实时信息
+        world_facts = self.latest_world_facts or {}
+        reply = self._generate_query_reply(query_subtype, world_facts, reply_text)
+
+        step = PlanStep(
+            step_id=1,
+            action="speak",
+            tool_call_type=ToolCallType.SPEAK,
+            parameters={
+                "text": reply,
+                "query_subtype": query_subtype,
+            },
+            preconditions={},
+            fallback=None,
+            status=StepStatus.PENDING,
+            description=f"查询回答: {query_subtype}",
+            requires_confirmation=False,
+            confirmation_message=None,
+            metadata={"gate_source": "local_direct"},
+        )
+        plan.add_step(step)
+
+        return plan
+
+    def _generate_query_reply(self, query_subtype: str, world_facts: dict,
+                               template_reply: str) -> str:
+        """生成查询回复"""
+        # 如果有模板回复，优先使用
+        if template_reply:
+            return template_reply
+
+        replies = {
+            "position": "我目前在广场。",
+            "status": "我当前电量充足，状态正常，随时可以工作。",
+            "count": "当前检测到2只牛。",
+            "weather": "当前天气晴朗，温度28°C，适合户外工作。",
+            "identity": f"我是{self.gate_rules.get('project_env', {}).get('robot_name', '农宝')}，智能农场机器人。",
+        }
+
+        # 如果能获取实时信息，替换
+        if query_subtype == "position":
+            current_loc = world_facts.get("current_location")
+            if current_loc:
+                return f"我目前在{current_loc}。"
+        elif query_subtype == "status":
+            return "我当前电量充足，状态正常，随时可以工作。"
+
+        return replies.get(query_subtype, "抱歉，我暂时无法回答这个问题。")
+
+    # =========================================================================
+    # 订阅回调 - Memory Context
+    # =========================================================================
+    def on_memory_context(self, msg: String) -> None:
+        """处理 MemoryNode 发布的记忆上下文
+
+        缓存 memory_text，在构造 /planner/llm_request 时使用。
+
+        Args:
+            msg: std_msgs/String，Memory 上下文 JSON
+        """
+        try:
+            data = json.loads(msg.data)
+            self.latest_memory_context = data
+            self.latest_memory_text = data.get("memory_text", "")
+
+            if self.debug_log:
+                turn_count = data.get("turn_count", 0)
+                memory_len = len(self.latest_memory_text)
+                self.get_logger().debug(
+                    f"[planner_node] memory_text updated: "
+                    f"turns={turn_count}, memory_text_len={memory_len}"
+                )
+        except json.JSONDecodeError as e:
+            self.get_logger().warn(f"Memory context JSON 解析失败: {e}")
+        except Exception as e:
+            self.get_logger().warn(f"处理 memory context 时发生错误: {e}")
+
+    # =========================================================================
+    # 订阅回调 - World Snapshot
+    # =========================================================================
+    def on_world_snapshot(self, msg: String) -> None:
+        """处理 world snapshot 消息
+
+        解析 JSON 并保存到本地缓存。解析失败时记录 error 日志但不崩溃。
+
+        Args:
+            msg: std_msgs/String，JSON 格式的世界状态快照
+        """
+        try:
+            snapshot = json.loads(msg.data)
+            self.latest_world_snapshot = snapshot
+
+            # 更新 world facts 缓存
+            self.extract_world_facts(snapshot)
+
+            if self.debug_log:
+                keys = list(snapshot.keys()) if isinstance(snapshot, dict) else "N/A"
+                self.get_logger().debug(f"收到 world snapshot，keys: {keys}")
+        except json.JSONDecodeError as e:
+            self.get_logger().error(f"World snapshot JSON 解析失败: {e}")
+        except Exception as e:
+            self.get_logger().error(f"处理 world snapshot 时发生错误: {e}")
+
+    # =========================================================================
+    # 订阅回调 - ASR（主入口）- 包含 Gate 前置治理
+    # =========================================================================
+    def on_asr(self, msg: String) -> None:
+        """处理 ASR 语音识别结果（主入口）- 含 Gate 前置治理
+
+        核心流程：
+            1. 校验空文本和 world snapshot
+            2. 最小打断：若正在等待 LLM，覆盖旧请求
+            3. Gate0: 基础解析
+            4. 提取 world facts
+            5. Gate1/Gate2/Gate3: 评估
+            6. Gate4: Teacher Memory 检索
+            7. Gate5: 路由决策
+            8. 分支处理：local_direct / local_3b / cloud
+
+        Args:
+            msg: std_msgs/String，用户语音转文字后的文本
+        """
+        user_text = msg.data.strip()
+        if not user_text:
+            self.get_logger().warning("收到空 ASR 文本，忽略")
+            return
+
+        if self.latest_world_snapshot is None:
+            self.get_logger().warning("尚未收到 world snapshot，无法处理 ASR 请求")
+            return
+
+        if self.debug_log:
+            self.get_logger().info(
+                f"收到 ASR 输入: '{user_text}', "
+                f"session={self.current_session_id}"
+            )
+
+        # =====================================================================
+        # 最小打断逻辑：若正在等待 LLM 响应，新请求直接覆盖
+        # =====================================================================
+        if self.waiting_for_llm:
+            old_request_id = self.pending_request_id
+            self.get_logger().warning(
+                f"检测到新 ASR 请求，覆盖旧请求 [old_request_id={old_request_id}]"
+            )
+            self._clear_waiting_state()  # 清理旧状态（但不发布 fallback）
+
+        # =====================================================================
+        # Gate 前置治理流程
+        # =====================================================================
+        try:
+            # Gate0: 基础解析
+            intent_info = self.classify_user_request(user_text)
+
+            if self.debug_log:
+                self.get_logger().debug(
+                    f"[Gate0] intent={intent_info['intent']}, "
+                    f"action={intent_info['action']}, "
+                    f"task_type={intent_info['task_type']}, "
+                    f"location={intent_info['location']}, "
+                    f"missing_slot={intent_info['missing_slot']}"
+                )
+
+            # 提取 world facts
+            world_facts = self.extract_world_facts()
+
+            # Gate1/Gate2/Gate3: 评估
+            gate_result = self.evaluate_local_gates(intent_info, world_facts)
+
+            if self.debug_log:
+                self.get_logger().debug(
+                    f"[Gate1-3] handled={gate_result['handled']}, "
+                    f"route={gate_result['route']}, "
+                    f"local_action={gate_result['local_action']}, "
+                    f"reason={gate_result['reason']}"
+                )
+
+            # Gate4: Teacher Memory 检索
+            memory_key = self.build_teacher_memory_key(intent_info, world_facts)
+            fallback_key = intent_info.get("teacher_memory_key", "")
+            memory_hit = self.lookup_teacher_memory(memory_key, fallback_key)
+
+            if self.debug_log and memory_hit:
+                self.get_logger().debug(
+                    f"[Gate4] Memory命中: key={memory_key}, reply={memory_hit.get('reply', '')[:50]}"
+                )
+
+            # Gate5: 路由决策
+            route = self.decide_route(intent_info, gate_result, memory_hit)
+
+            self.get_logger().info(
+                f"[Gate5] 路由决策: route={route}, "
+                f"task_type={intent_info['task_type']}, "
+                f"reason={gate_result['reason'] or intent_info['reason']}"
+            )
+
+            # =================================================================
+            # 分支处理
+            # =================================================================
+
+            # 分支 A: local_direct - 本地直接处理
+            if route == "local_direct":
+                self.get_logger().info(f"[Gate] 本地直接处理: {gate_result['local_action']}")
+
+                plan = self.try_handle_locally(intent_info, gate_result, memory_hit)
+                if plan:
+                    self.publish_plan(plan)
+                    return
+                else:
+                    # 生成失败，回退到 LLM
+                    self.get_logger().warn("[Gate] 本地处理失败，回退到 LLM")
+
+            # 分支 B/C: local_3b 或 cloud - 走 LLM
+            llm_request = self.build_request(user_text, intent_info, route, memory_hit)
+            self.publish_llm_request(llm_request)
+
+        except Exception as e:
+            self.get_logger().error(f"Gate 前置治理异常: {e}，回退到原有流程")
+            # 异常时回退到原有 LLM 流程
+            try:
+                llm_request = self.build_request(user_text)
+                self.publish_llm_request(llm_request)
+            except Exception as e2:
+                self.get_logger().error(f"回退流程也失败: {e2}")
+                fallback_plan = self.build_fallback_plan(
+                    user_intent=user_text,
+                    reason=f"gate_error: {e}",
+                )
+                self.publish_plan(fallback_plan)
+
+    # =========================================================================
+    # 构造 LLM 请求（统一方法）
+    # =========================================================================
+    def build_request(self, user_text: str,
+                     intent_info: Optional[dict] = None,
+                     route_hint: str = "",
+                     memory_hit: Optional[dict] = None) -> dict:
+        """构造发给 OmniNode/CloudLLMNode 的统一请求 JSON
+
+        核心变更：
+        1. 统一组装 messages，所有业务提示词由 PlannerNode 组织
+        2. 统一 prompt 来源，OmniNode 和 CloudLLMNode 只使用 request 中的 messages
+
+        Args:
+            user_text: 用户原始文本（ASR 结果）
+            intent_info: Gate0 解析结果（可选）
+            route_hint: 路由提示（可选）
+            memory_hit: Teacher Memory 命中结果（可选）
+
+        Returns:
+            dict: 请求 JSON 对象
+        """
+        request_id = str(uuid.uuid4())
+        timestamp = time.time()
+
+        request = {
+            "request_id": request_id,
+            "session_id": self.current_session_id,
+            "mode": "dialog_and_decision",
+            "user_text": user_text,
+            "memory_text": self.latest_memory_text,
+            "world_snapshot": self._build_world_description(),
+            "available_tools": self.runtime_config.available_tools,
+            "tool_descriptions": self.runtime_config.tool_descriptions,
+            "domain_rules": self.runtime_config.domain_rules,
+            "planner_mode": self.runtime_config.planner_mode,
+            "current_state": {
+                "waiting_for_llm": self.waiting_for_llm,
+                "has_pending_plan": self.has_pending_plan,
+            },
+            "timestamp": timestamp,
+        }
+
+        # 新增字段（Gate 相关）
+        if intent_info:
+            request["intent_info"] = {
+                "action": intent_info.get("action", "unknown"),
+                "task_type": intent_info.get("task_type", "unknown"),
+                "location": intent_info.get("location"),
+                "object": intent_info.get("object"),
+                "missing_slot": intent_info.get("missing_slot"),
+                "has_condition": intent_info.get("has_condition", False),
+                "is_complex": intent_info.get("is_complex", False),
+            }
+
+        if route_hint:
+            request["route_hint"] = route_hint
+
+        if memory_hit:
+            request["teacher_memory_hint"] = {
+                "reply": memory_hit.get("reply", ""),
+                "command": memory_hit.get("command", ""),
+                "source": memory_hit.get("source", ""),
+            }
+
+        # ========== 核心变更：统一组装 messages ==========
+        request["messages"] = self._build_unified_messages(request)
+
+        self.get_logger().debug(
+            f"构造 LLM 请求: request_id={request_id}, "
+            f"tools={len(self.runtime_config.available_tools)}, "
+            f"tool_descriptions={len(self.runtime_config.tool_descriptions)}, "
+            f"mode=dialog_and_decision, "
+            f"memory_text_len={len(self.latest_memory_text)}, "
+            f"route_hint={route_hint}, "
+            f"messages_count={len(request['messages'])}"
+        )
+
+        return request
+
+    def _build_unified_messages(self, request_data: dict) -> List[dict]:
+        """统一组装 messages - 这是唯一的 prompt 组装出口
+
+        所有业务提示词、协议约束、领域规则统一由 PlannerNode 组织。
+        OmniNode 和 CloudLLMNode 必须使用此处组装的 messages。
+
+        注意：提示词必须从配置文件加载，不使用任何硬编码默认值。
+
+        Returns:
+            List[dict]: messages 列表，包含 system 和 user 消息
+        """
+        # 获取配置的提示词
+        llm_prompts = self.gate_rules.get("llm_prompts", {})
+
+        # 如果配置缺失，记录错误
+        if not llm_prompts:
+            self.get_logger().error(
+                "llm_prompts 配置缺失！请在 planner_gate_config.yaml 中配置 llm_prompts 节。"
+            )
+            raise ValueError("llm_prompts 配置缺失，请在配置文件中添加")
+
+        # 1. System prompt 组合
+        system_parts = []
+
+        # 1.1 机器人身份和职责（必须配置）
+        system_prompt = llm_prompts.get("system_prompt", "")
+        if system_prompt:
+            system_parts.append(system_prompt)
+        else:
+            self.get_logger().warn("system_prompt 未配置")
+
+        # 1.2 输出协议（必须配置）
+        output_protocol = llm_prompts.get("output_protocol_prompt", "")
+        if output_protocol:
+            system_parts.append(output_protocol)
+        else:
+            self.get_logger().warn("output_protocol_prompt 未配置")
+
+        # 1.3 业务规则（可选）
+        business_rules = llm_prompts.get("business_rules_prompt", "")
+        if business_rules:
+            system_parts.append(business_rules)
+
+        # 如果没有任何提示词，报错
+        if not system_parts:
+            self.get_logger().error(
+                "llm_prompts 配置为空！请在配置文件中至少配置 system_prompt 和 output_protocol_prompt。"
+            )
+            raise ValueError("llm_prompts 配置为空")
+
+        system_content = "\n\n".join(system_parts)
+
+        # 2. User context 组装
+        user_content = self._build_user_context(request_data)
+
+        return [
+            {"role": "system", "content": system_content},
+            {"role": "user", "content": user_content}
+        ]
+
+    def _build_user_context(self, request_data: dict) -> str:
+        """组装用户上下文 - 最小字段转发，不做业务加工
+
+        格式简单清晰，只做原样字段转发。
+        """
+        parts = []
+
+        # 当前请求
+        parts.append("=== 当前请求 ===")
+        parts.append(f"user_text: {request_data.get('user_text', '')}")
+        parts.append(f"session_id: {request_data.get('session_id', 'default')}")
+        parts.append(f"mode: {request_data.get('mode', 'dialog_and_decision')}")
+        parts.append(f"planner_mode: {request_data.get('planner_mode', 'hybrid')}")
+        parts.append("")
+
+        # 意图信息
+        intent_info = request_data.get("intent_info", {})
+        if intent_info:
+            parts.append("=== 意图信息 ===")
+            parts.append(f"action: {intent_info.get('action', 'unknown')}")
+            parts.append(f"task_type: {intent_info.get('task_type', 'unknown')}")
+            parts.append(f"location: {intent_info.get('location', 'none')}")
+            parts.append(f"object: {intent_info.get('object', 'none')}")
+            parts.append(f"missing_slot: {intent_info.get('missing_slot', 'none')}")
+            parts.append("")
+
+        # 记忆上下文
+        memory_text = request_data.get("memory_text", "")
+        if memory_text:
+            parts.append("=== 记忆上下文 ===")
+            parts.append(memory_text)
+            parts.append("")
+
+        # 世界状态
+        world_snapshot = request_data.get("world_snapshot", "")
+        if world_snapshot:
+            parts.append("=== 世界状态 ===")
+            parts.append(world_snapshot)
+            parts.append("")
+
+        # 老师记忆
+        teacher_hint = request_data.get("teacher_memory_hint", {})
+        if teacher_hint:
+            parts.append("=== 老师记忆 ===")
+            if teacher_hint.get("reply"):
+                parts.append(f"建议回复: {teacher_hint['reply']}")
+            if teacher_hint.get("command"):
+                parts.append(f"建议命令: {teacher_hint['command']}")
+            parts.append("")
+
+        # 可用工具
+        available_tools = request_data.get("available_tools", [])
+        if available_tools:
+            parts.append("=== 可用工具 ===")
+            parts.append(", ".join(available_tools))
+            parts.append("")
+
+        # 工具描述
+        tool_descriptions = request_data.get("tool_descriptions", [])
+        if tool_descriptions:
+            parts.append("=== 工具描述 ===")
+            for t in tool_descriptions:
+                if isinstance(t, dict):
+                    name = t.get("name", "")
+                    desc = t.get("description", "")
+                    parts.append(f"- {name}: {desc}")
+            parts.append("")
+
+        # 领域规则
+        domain_rules = request_data.get("domain_rules", {})
+        if domain_rules:
+            parts.append("=== 领域规则 ===")
+            if isinstance(domain_rules, dict):
+                for k, v in domain_rules.items():
+                    parts.append(f"- {k}: {v}")
+            parts.append("")
+
+        # 系统状态
+        current_state = request_data.get("current_state", {})
+        if current_state:
+            parts.append("=== 系统状态 ===")
+            parts.append(f"waiting_for_llm: {current_state.get('waiting_for_llm', False)}")
+            parts.append(f"has_pending_plan: {current_state.get('has_pending_plan', False)}")
+            parts.append("")
+
+        parts.append("请严格基于以上内容，按协议要求输出。")
+
+        return "\n".join(parts)
+
+    # =========================================================================
+    # 构建中文可读的世界描述
+    # =========================================================================
+    def _build_world_description(self) -> str:
+        """将 world_snapshot 转换为中文可读描述
+
+        支持两种格式：
+        1. WorldModel 格式：{ entities: { id: { type, state } } }
+        2. 旧格式：{ robot: [...], poi: [...], weather: {} }
+
+        Returns:
+            str: 中文描述字符串
+        """
+        if not self.latest_world_snapshot:
+            return "暂无世界数据"
+
+        parts = []
+        snapshot = self.latest_world_snapshot
+
+        if "entities" in snapshot:
+            parts = self._build_world_description_v2(snapshot)
+        else:
+            parts = self._build_world_description_v1(snapshot)
+
+        if not parts:
+            return "暂无详细世界数据"
+
+        return "\n".join(parts)
+
+    def _build_world_description_v1(self, snapshot: dict) -> list:
+        """解析旧格式 snapshot"""
+        parts = []
+
+        # === POI 地点 ===
+        pois = snapshot.get("poi", [])
+        if pois:
+            poi_parts = []
+            current_location = None
+
+            for poi in pois:
+                name = poi.get("name", poi.get("entity_id", "未知"))
+                poi_type = self._translate_poi_type(poi.get("poi_type", ""))
+                status = self._translate_status(poi.get("status", ""))
+                index = poi.get("index", "")
+                is_current = poi.get("is_current_location", False)
+                allowed_skills = poi.get("allowed_skills", [])
+
+                if is_current:
+                    current_location = f"{name}（{index}号）"
+                else:
+                    accessible = "可访问" if poi.get("properties", {}).get("accessible", True) else "不可访问"
+                    poi_desc = f"{name}（{index}号）- {poi_type}，{status}，{accessible}"
+                    # 增加 allowed_skills 描述
+                    if allowed_skills:
+                        skills_str = ", ".join(allowed_skills)
+                        poi_desc += f"，支持技能：{skills_str}"
+                    poi_parts.append(poi_desc)
+
+            poi_desc = "可抵达地点：\n  - " + "\n  - ".join(poi_parts) if poi_parts else "无地点信息"
+            if current_location:
+                parts.append(f"当前位置：{current_location}")
+            parts.append(poi_desc)
+
+        # === 机器人状态 ===
+        robot = snapshot.get("robot", [])
+        if robot:
+            robot_info = robot[0] if isinstance(robot, list) else robot
+            battery = robot_info.get("battery", robot_info.get("battery_level", "未知"))
+            status = self._translate_robot_status(robot_info.get("status", ""))
+            position = robot_info.get("position", {})
+            if isinstance(position, dict):
+                pos_str = f"坐标({position.get('x', 0):.1f}, {position.get('y', 0):.1f})"
+            else:
+                pos_str = str(position)
+            parts.append(f"机器人状态：电量{battery}%，{status}，{pos_str}")
+
+        # === 天气 ===
+        weather = snapshot.get("weather", {})
+        if weather:
+            condition = self._translate_weather(weather.get("condition", ""))
+            temp = weather.get("temperature", "未知")
+            humidity = weather.get("humidity", "")
+            wind = weather.get("wind_speed", "")
+            desc = f"{condition}，温度{temp}°C"
+            if humidity:
+                desc += f"，湿度{humidity}%"
+            if wind:
+                desc += f"，风速{wind}m/s"
+            parts.append(f"天气：{desc}")
+
+        # === 气体传感器 ===
+        gas = snapshot.get("gas", {})
+        if gas:
+            warnings = []
+            ch4 = gas.get("ch4", gas.get("methane", 0))
+            co2 = gas.get("co2", gas.get("carbon_dioxide", 0))
+            o2 = gas.get("o2", gas.get("oxygen", 0))
+            if ch4 > 1.0:
+                warnings.append(f"甲烷(CH4)浓度偏高：{ch4}%")
+            if co2 > 0.5:
+                warnings.append(f"二氧化碳(CO2)浓度偏高：{co2}%")
+            if o2 < 18.0:
+                warnings.append(f"氧气(O2)浓度偏低：{o2}%")
+            if warnings:
+                parts.append("气体警告：\n  - " + "\n  - ".join(warnings))
+
+        # === 动物检测 ===
+        animals = snapshot.get("animal", [])
+        if animals:
+            animal_parts = []
+            for animal in animals[:5]:
+                species = animal.get("species", "未知")
+                behavior = animal.get("behavior", "未知")
+                health = animal.get("health_status", "未知")
+                pos = animal.get("position", {})
+                if isinstance(pos, dict):
+                    pos_str = f"坐标({pos.get('x', 0):.1f}, {pos.get('y', 0):.1f})"
+                else:
+                    pos_str = ""
+                animal_parts.append(f"{species}，行为：{behavior}，健康：{health}，{pos_str}")
+            parts.append(f"检测到动物（{len(animals)}只）：\n  - " + "\n  - ".join(animal_parts))
+
+        # === 执行器状态 ===
+        actuators = snapshot.get("actuator", [])
+        if actuators:
+            actuator_parts = []
+            for actuator in actuators[:5]:
+                name = actuator.get("entity_id", actuator.get("name", "未知"))
+                status = self._translate_status(actuator.get("status", ""))
+                available = "可用" if actuator.get("available", False) else "不可用"
+                actuator_parts.append(f"{name}，状态：{status}，{available}")
+            parts.append(f"执行器（{len(actuators)}个）：\n  - " + "\n  - ".join(actuator_parts))
+
+        return parts
+
+    def _build_world_description_v2(self, snapshot: dict) -> list:
+        """解析 WorldModel 格式 snapshot"""
+        parts = []
+        entities = snapshot.get("entities", {})
+
+        by_type: dict = {}
+        for entity_id, entity_data in entities.items():
+            entity_type = entity_data.get("type", "unknown")
+            if entity_type not in by_type:
+                by_type[entity_type] = []
+            by_type[entity_type].append((entity_id, entity_data))
+
+        # === POI 地点 ===
+        pois = by_type.get("poi", [])
+        if pois:
+            poi_parts = []
+            current_location = None
+
+            for entity_id, entity_data in pois:
+                state = entity_data.get("state", {})
+                name = state.get("name", entity_id)
+                poi_type = self._translate_poi_type(state.get("type", ""))
+                status = self._translate_status(state.get("status", ""))
+                index = state.get("index", "")
+                is_current = state.get("is_current_location", False)
+                properties = state.get("properties", {})
+                allowed_skills = state.get("allowed_skills", [])
+
+                if is_current:
+                    current_location = f"{name}（{index}号）"
+                else:
+                    accessible = "可访问" if properties.get("accessible", True) else "不可访问"
+                    poi_desc = f"{name}（{index}号）- {poi_type}，{status}，{accessible}"
+                    # 增加 allowed_skills 描述
+                    if allowed_skills:
+                        skills_str = ", ".join(allowed_skills)
+                        poi_desc += f"，支持技能：{skills_str}"
+                    poi_parts.append(poi_desc)
+
+            poi_desc = "可抵达地点：\n  - " + "\n  - ".join(poi_parts) if poi_parts else "无地点信息"
+            if current_location:
+                parts.append(f"当前位置：{current_location}")
+            parts.append(poi_desc)
+
+        # === 机器人状态 ===
+        robots = by_type.get("robot", [])
+        if robots:
+            entity_id, entity_data = robots[0]
+            state = entity_data.get("state", {})
+            battery = state.get("battery", "未知")
+            status = self._translate_robot_status(state.get("status", ""))
+            position = state.get("position", {})
+            if isinstance(position, dict):
+                pos_str = f"坐标({position.get('x', 0):.1f}, {position.get('y', 0):.1f})"
+            else:
+                pos_str = str(position)
+            parts.append(f"机器人状态：电量{battery}%，{status}，{pos_str}")
+
+        # === 环境状态（天气） ===
+        env = snapshot.get("environment", {})
+        if env:
+            weather = env.get("weather", env.get("condition", ""))
+            if weather and weather not in ("", "unknown"):
+                condition = self._translate_weather(weather)
+                temp = env.get("temperature", "未知")
+                humidity = env.get("humidity", "")
+                wind = env.get("wind_speed", "")
+                desc = f"{condition}，温度{temp}°C"
+                if humidity:
+                    desc += f"，湿度{humidity}%"
+                if wind:
+                    desc += f"，风速{wind}m/s"
+                parts.append(f"天气：{desc}")
+
+            ch4 = env.get("ch4", env.get("methane", 0))
+            co2 = env.get("co2", env.get("carbon_dioxide", 0))
+            o2 = env.get("o2", env.get("oxygen", 0))
+            if ch4 or co2 or o2:
+                warnings = []
+                if ch4 > 1.0:
+                    warnings.append(f"甲烷(CH4)浓度偏高：{ch4}%")
+                if co2 > 0.5:
+                    warnings.append(f"二氧化碳(CO2)浓度偏高：{co2}%")
+                if o2 and o2 < 18.0:
+                    warnings.append(f"氧气(O2)浓度偏低：{o2}%")
+                if warnings:
+                    parts.append("气体警告：\n  - " + "\n  - ".join(warnings))
+
+        # === 动物检测 ===
+        animals = by_type.get("animal", [])
+        if animals:
+            animal_parts = []
+            for entity_id, entity_data in animals[:5]:
+                state = entity_data.get("state", {})
+                species = state.get("species", entity_id)
+                behavior = state.get("behavior", "未知")
+                health = state.get("health_status", "未知")
+                pos = state.get("position", {})
+                if isinstance(pos, dict):
+                    pos_str = f"坐标({pos.get('x', 0):.1f}, {pos.get('y', 0):.1f})"
+                else:
+                    pos_str = ""
+                animal_parts.append(f"{species}，行为：{behavior}，健康：{health}，{pos_str}")
+            parts.append(f"检测到动物（{len(animals)}只）：\n  - " + "\n  - ".join(animal_parts))
+
+        # === 执行器状态 ===
+        actuators = by_type.get("actuator", [])
+        if actuators:
+            actuator_parts = []
+            for entity_id, entity_data in actuators[:5]:
+                state = entity_data.get("state", {})
+                name = state.get("name", entity_id)
+                status = self._translate_status(state.get("status", ""))
+                available = "可用" if state.get("available", False) else "不可用"
+                actuator_parts.append(f"{name}，状态：{status}，{available}")
+            parts.append(f"执行器（{len(actuators)}个）：\n  - " + "\n  - ".join(actuator_parts))
+
+        return parts
+
+    def _translate_poi_type(self, poi_type: str) -> str:
+        """翻译 POI 类型"""
+        mapping = {
+            "area": "开阔区域",
+            "building": "建筑",
+            "facility": "设施",
+            "entrance": "入口",
+            "charging_station": "充电桩",
+            "barn": "牛棚",
+        }
+        return mapping.get(poi_type, poi_type)
+
+    def _translate_status(self, status: str) -> str:
+        """翻译状态"""
+        mapping = {
+            "available": "状态正常",
+            "repairing": "正在维修",
+            "unavailable": "不可用",
+            "offline": "离线",
+            "active": "运行中",
+            "idle": "空闲",
+            "error": "故障",
+        }
+        return mapping.get(status, f"状态：{status}")
+
+    def _translate_robot_status(self, status: str) -> str:
+        """翻译机器人状态"""
+        mapping = {
+            "idle": "空闲",
+            "working": "工作中",
+            "charging": "充电中",
+            "error": "故障",
+            "moving": "移动中",
+            "patrolling": "巡检中",
+        }
+        return mapping.get(status, status)
+
+    def _translate_weather(self, condition: str) -> str:
+        """翻译天气状况"""
+        mapping = {
+            "sunny": "晴天",
+            "cloudy": "多云",
+            "rainy": "雨天",
+            "foggy": "雾天",
+            "windy": "大风",
+            "snowy": "雪天",
+        }
+        return mapping.get(condition, condition)
+
+    # =========================================================================
+    # 发布 LLM 请求
+    # =========================================================================
+    def publish_llm_request(self, llm_request: dict) -> None:
+        """发布 LLM 请求到 /planner/llm_request
+
+        Args:
+            llm_request: 请求字典
+        """
+        request_id = llm_request["request_id"]
+        self.pending_request_id = request_id
+        self.pending_user_text = llm_request["user_text"]
+        self.llm_request_timestamp = time.time()
+        self.waiting_for_llm = True
+        self.last_request_time = self.llm_request_timestamp
+
+        msg = String()
+        msg.data = json.dumps(llm_request, ensure_ascii=False)
+        self.llm_request_pub.publish(msg)
+
+        route_hint = llm_request.get("route_hint", "")
+        self.get_logger().info(
+            f"已发布 LLM 请求 [request_id={request_id}, "
+            f"session_id={llm_request['session_id']}], "
+            f"tools={len(llm_request['available_tools'])}, "
+            f"tool_descriptions={len(llm_request['tool_descriptions'])}, "
+            f"route_hint={route_hint}, "
+            f"messages_count={len(llm_request.get('messages', []))}, "
+            f"等待响应（超时 {self.llm_timeout_sec}s）"
+        )
+
+    # =========================================================================
+    # 轻 JSON 转换方法
+    # =========================================================================
+    def convert_light_json_to_plan(self, light_json: dict, request_id: str,
+                                   session_id: str, natural_response: str = "") -> 'Plan':
+        """将轻 JSON 转换为标准 Plan
+
+        输入格式:
+        {
+          "status": "ok / ask / confirm / reject / fail",
+          "actions": [
+            {"location": "地点", "tasks": ["动作", "动作:参数"]}
+          ]
+        }
+
+        Args:
+            light_json: 轻 JSON 字典
+            request_id: 请求 ID
+            session_id: 会话 ID
+            natural_response: 自然语言回复
+
+        Returns:
+            Plan: 标准 Plan 对象
+        """
+        plan_id = f"plan_light_{uuid.uuid4().hex[:8]}"
+        status = light_json.get("status", "ok")
+        actions = light_json.get("actions", [])
+
+        # 根据 status 决定 goal 和处理方式
+        plan_goal = "执行用户任务"
+        steps = []
+
+        if status == "ok":
+            plan_goal = "正常执行任务"
+            steps = self._convert_actions_to_steps(actions, start_step_id=1)
+
+        elif status == "ask":
+            plan_goal = "询问用户补充信息"
+            ask_question = self._extract_ask_question(actions)
+            steps = [
+                self._create_ask_user_step(
+                    step_id=1,
+                    question=ask_question,
+                    original_intent=self.pending_user_text or ""
+                )
+            ]
+
+        elif status == "confirm":
+            plan_goal = "确认用户意图"
+            confirm_question = self._extract_confirm_question(actions)
+            steps = [
+                self._create_ask_user_step(
+                    step_id=1,
+                    question=confirm_question,
+                    original_intent=self.pending_user_text or ""
+                )
+            ]
+
+        elif status == "reject":
+            plan_goal = "安全拒绝任务"
+            reject_message = self._extract_reject_message(actions)
+            steps = [
+                self._create_speak_step(step_id=1, text=reject_message)
+            ]
+
+        elif status == "fail":
+            plan_goal = "无法理解用户请求"
+            steps = [
+                self._create_ask_user_step(
+                    step_id=1,
+                    question="抱歉，我没有完全理解您的请求，请再说一次。",
+                    original_intent=self.pending_user_text or ""
+                )
+            ]
+
+        else:
+            plan_goal = f"未知状态: {status}"
+            steps = [
+                self._create_ask_user_step(
+                    step_id=1,
+                    question="抱歉，我无法处理这个请求。",
+                    original_intent=self.pending_user_text or ""
+                )
+            ]
+
+        # 构建 Plan
+        plan = Plan(
+            plan_id=plan_id,
+            goal=plan_goal,
+            reasoning=f"轻 JSON 转换: status={status}, actions_count={len(actions)}",
+            risk_level=RiskLevel.LOW,
+            requires_confirmation=False,
+            confirmation_message=None,
+            status=PlanStatus.CREATED,
+            source="llm_light_json",
+            metadata={
+                "light_json_source": True,
+                "original_status": status,
+                "natural_response": natural_response,
+            },
+        )
+
+        for step in steps:
+            plan.add_step(step)
+
+        return plan
+
+    def _convert_actions_to_steps(self, actions: list, start_step_id: int = 1) -> List['PlanStep']:
+        """将 actions 数组转换为 PlanStep 列表
+
+        Args:
+            actions: 地点任务块数组
+            start_step_id: 起始 step_id
+
+        Returns:
+            List[PlanStep]: PlanStep 列表
+        """
+        steps = []
+        step_id = start_step_id
+
+        for action_block in actions:
+            location = action_block.get("location", "none")
+            tasks = action_block.get("tasks", [])
+
+            for task in tasks:
+                step = self._parse_task_to_step(task, location, step_id)
+                if step:
+                    steps.append(step)
+                    step_id += 1
+
+        return steps
+
+    def _parse_task_to_step(self, task: str, location: str, step_id: int) -> Optional['PlanStep']:
+        """解析单个 task 为 PlanStep
+
+        任务格式:
+        - move_to -> action=move, location
+        - feed -> action=feed, location
+        - inspect -> action=inspect, location
+        - open:风扇 -> action=open, target=风扇
+        - close:灯 -> action=close, target=灯
+        - adjust:风扇:低速 -> action=adjust, target=风扇, value=低速
+        - speak:xxx -> action=speak, text=xxx
+        - ask_user:xxx -> action=ask_user, question=xxx
+        - noop:原因 -> action=ask_user, question=原因
+
+        Args:
+            task: 任务字符串
+            location: 地点名称
+            step_id: step ID
+
+        Returns:
+            PlanStep or None
+        """
+        if not task:
+            return None
+
+        # 解析 task 格式
+        parts = task.split(":")
+        base_action = parts[0].strip().lower()
+        params = parts[1:] if len(parts) > 1 else []
+
+        # 根据动作类型创建 step
+        if base_action == "move_to":
+            return self._create_move_step(step_id, location, is_move_to=True)
+
+        elif base_action in ["feed", "inspect", "move"]:
+            return self._create_action_step(step_id, base_action, location)
+
+        elif base_action == "open":
+            target = params[0].strip() if params else "未知设备"
+            return self._create_device_step(step_id, "open", target)
+
+        elif base_action == "close":
+            target = params[0].strip() if params else "未知设备"
+            return self._create_device_step(step_id, "close", target)
+
+        elif base_action == "adjust":
+            target = params[0].strip() if params else "未知设备"
+            value = params[1].strip() if len(params) > 1 else "默认"
+            return self._create_adjust_step(step_id, target, value)
+
+        elif base_action == "speak":
+            text = ":".join(params).strip() if params else ""
+            return self._create_speak_step(step_id, text)
+
+        elif base_action == "ask_user":
+            question = ":".join(params).strip() if params else "请再说一次"
+            return self._create_ask_user_step(step_id, question, "")
+
+        elif base_action == "noop":
+            reason = ":".join(params).strip() if params else "无需操作"
+            return self._create_ask_user_step(step_id, f"提示: {reason}", "")
+
+        elif base_action == "query":
+            query_type = params[0].strip() if params else "status"
+            return self._create_query_step(step_id, query_type)
+
+        else:
+            # 未知动作，返回 speak
+            return self._create_speak_step(step_id, f"将执行: {task}")
+
+    def _create_move_step(self, step_id: int, location: str, is_move_to: bool = False) -> 'PlanStep':
+        """创建移动步骤"""
+        return PlanStep(
+            step_id=step_id,
+            action="move",
+            tool_call_type=ToolCallType.EXECUTE,
+            parameters={
+                "location": location if location != "none" else "",
+                "destination": location if location != "none" else "",
+            },
+            preconditions={},
+            fallback=None,
+            status=StepStatus.PENDING,
+            description=f"移动到{location}" if location != "none" else "移动",
+            requires_confirmation=False,
+            confirmation_message=None,
+            metadata={"location_block": location, "is_move_to": is_move_to},
+        )
+
+    def _create_action_step(self, step_id: int, action: str, location: str) -> 'PlanStep':
+        """创建普通动作步骤"""
+        tool_call_type = ToolCallType.EXECUTE
+        description = f"{action}@{location}" if location != "none" else action
+
+        return PlanStep(
+            step_id=step_id,
+            action=action,
+            tool_call_type=tool_call_type,
+            parameters={
+                "location": location if location != "none" else "",
+            },
+            preconditions={},
+            fallback=None,
+            status=StepStatus.PENDING,
+            description=description,
+            requires_confirmation=False,
+            confirmation_message=None,
+            metadata={"location_block": location},
+        )
+
+    def _create_device_step(self, step_id: int, action: str, target: str) -> 'PlanStep':
+        """创建设备控制步骤"""
+        return PlanStep(
+            step_id=step_id,
+            action=action,
+            tool_call_type=ToolCallType.EXECUTE,
+            parameters={
+                "target": target,
+                "device": target,
+            },
+            preconditions={},
+            fallback=None,
+            status=StepStatus.PENDING,
+            description=f"{action} {target}",
+            requires_confirmation=False,
+            confirmation_message=None,
+            metadata={"device_target": target},
+        )
+
+    def _create_adjust_step(self, step_id: int, target: str, value: str) -> 'PlanStep':
+        """创建调节步骤"""
+        return PlanStep(
+            step_id=step_id,
+            action="adjust",
+            tool_call_type=ToolCallType.EXECUTE,
+            parameters={
+                "target": target,
+                "device": target,
+                "value": value,
+                "level": value,
+            },
+            preconditions={},
+            fallback=None,
+            status=StepStatus.PENDING,
+            description=f"调节 {target} 为 {value}",
+            requires_confirmation=False,
+            confirmation_message=None,
+            metadata={"device_target": target, "adjust_value": value},
+        )
+
+    def _create_speak_step(self, step_id: int, text: str) -> 'PlanStep':
+        """创建播报步骤"""
+        return PlanStep(
+            step_id=step_id,
+            action="speak",
+            tool_call_type=ToolCallType.SPEAK,
+            parameters={
+                "text": text,
+                "message": text,
+            },
+            preconditions={},
+            fallback=None,
+            status=StepStatus.PENDING,
+            description=f"播报: {text[:30]}...",
+            requires_confirmation=False,
+            confirmation_message=None,
+            metadata={},
+        )
+
+    def _create_ask_user_step(self, step_id: int, question: str, original_intent: str) -> 'PlanStep':
+        """创建询问用户步骤"""
+        return PlanStep(
+            step_id=step_id,
+            action="ask_user",
+            tool_call_type=ToolCallType.ASK_USER,
+            parameters={
+                "question": question,
+                "original_intent": original_intent,
+            },
+            preconditions={},
+            fallback=None,
+            status=StepStatus.PENDING,
+            description=f"询问: {question[:30]}...",
+            requires_confirmation=False,
+            confirmation_message=None,
+            metadata={"response_type": "ask"},
+        )
+
+    def _create_query_step(self, step_id: int, query_type: str) -> 'PlanStep':
+        """创建查询步骤"""
+        return PlanStep(
+            step_id=step_id,
+            action="query",
+            tool_call_type=ToolCallType.QUERY_WORLD,
+            parameters={
+                "query_type": query_type,
+            },
+            preconditions={},
+            fallback=None,
+            status=StepStatus.PENDING,
+            description=f"查询: {query_type}",
+            requires_confirmation=False,
+            confirmation_message=None,
+            metadata={},
+        )
+
+    def _extract_ask_question(self, actions: list) -> str:
+        """从 actions 中提取询问内容"""
+        for block in actions:
+            for task in block.get("tasks", []):
+                if task.startswith("ask_user:"):
+                    return task.replace("ask_user:", "").strip()
+        return "请问您具体想要什么？"
+
+    def _extract_confirm_question(self, actions: list) -> str:
+        """从 actions 中提取确认问题"""
+        for block in actions:
+            for task in block.get("tasks", []):
+                if task.startswith("ask_user:"):
+                    return task.replace("ask_user:", "").strip()
+        return "请确认您的意图。"
+
+    def _extract_reject_message(self, actions: list) -> str:
+        """从 actions 中提取拒绝消息"""
+        for block in actions:
+            for task in block.get("tasks", []):
+                if task.startswith("speak:"):
+                    return task.replace("speak:", "").strip()
+        return "抱歉，无法执行该任务。"
+
+    # =========================================================================
+    # 订阅回调 - LLM Response（支持轻 JSON 和重 Plan）
+    # =========================================================================
+    def on_llm_response(self, msg: String) -> None:
+        """处理 LLM 结构化响应
+
+        支持两种响应格式：
+            1. 轻 JSON 格式: {"status": "ok/ask/confirm/reject/fail", "actions": [...]}
+            2. 重 Plan 格式: {"plan": {...}} 或 {"decision": {...}}
+
+        流程：
+            1. 检查是否在 waiting_for_llm 状态
+            2. 校验 request_id 匹配
+            3. 判断响应格式
+            4. 转换为 Plan 对象
+            5. 发布 /plan
+
+        Args:
+            msg: std_msgs/String，LLM 响应 JSON
+        """
+        if not self.waiting_for_llm:
+            return
+
+        try:
+            response = json.loads(msg.data)
+        except json.JSONDecodeError as e:
+            self.get_logger().error(f"LLM 响应 JSON 解析失败: {e}")
+            self._handle_llm_failure("invalid_json")
+            return
+
+        response_id = response.get("request_id")
+        if response_id != self.pending_request_id:
+            self.get_logger().warning(
+                f"LLM 响应 request_id 不匹配：期望 {self.pending_request_id}，"
+                f"收到 {response_id}，忽略（可能是旧响应）"
+            )
+            return
+
+        # ========== 方案1: 轻 JSON 格式 ==========
+        # 检查是否为轻 JSON 格式: {"status": "...", "actions": [...]}
+        if "status" in response and "actions" in response:
+            light_json = response
+            natural_response = response.get("natural_response", "")
+
+            self.get_logger().debug(
+                f"检测到轻 JSON 格式: status={light_json.get('status')}, "
+                f"actions_count={len(light_json.get('actions', []))}"
+            )
+
+            try:
+                plan = self.convert_light_json_to_plan(
+                    light_json,
+                    response_id,
+                    self.current_session_id,
+                    natural_response
+                )
+                if not plan.source or plan.source == "":
+                    plan.source = "llm_light_json"
+
+                self.get_logger().info(
+                    f"轻 JSON 转换为 Plan [plan_id={plan.plan_id}], "
+                    f"steps={len(plan.steps)}, source={plan.source}"
+                )
+                self.publish_plan(plan)
+                self._clear_waiting_state()
+                return
+            except Exception as e:
+                self.get_logger().warn(f"轻 JSON 转换失败: {e}，尝试重 Plan 格式")
+                # 继续尝试重 Plan 格式
+
+        # ========== 方案2: 重 Plan 格式 ==========
+        plan_data = None
+        source = "unknown"
+        if "plan" in response:
+            plan_data = response["plan"]
+            source = "plan"
+            self.get_logger().debug("响应中包含 plan 字段")
+        elif "decision" in response:
+            plan_data = response["decision"]
+            source = "decision"
+            self.get_logger().debug("响应中包含 decision 字段，将转换为 Plan")
+        else:
+            # 尝试从 natural_response 中提取轻 JSON
+            natural_response = response.get("natural_response", "")
+            if natural_response and "<FINAL_JSON>" in natural_response:
+                json_str = natural_response.split("<FINAL_JSON>")[-1].strip()
+                try:
+                    parsed = json.loads(json_str)
+                    if "status" in parsed and "actions" in parsed:
+                        self.get_logger().debug("从 natural_response 提取轻 JSON")
+                        plan = self.convert_light_json_to_plan(
+                            parsed,
+                            response_id,
+                            self.current_session_id,
+                            natural_response.split("<FINAL_JSON>")[0].strip()
+                        )
+                        if not plan.source or plan.source == "":
+                            plan.source = "llm_light_json"
+                        self.get_logger().info(
+                            f"从 natural_response 提取并转换 Plan [plan_id={plan.plan_id}]"
+                        )
+                        self.publish_plan(plan)
+                        self._clear_waiting_state()
+                        return
+                except json.JSONDecodeError:
+                    self.get_logger().warn("无法从 natural_response 提取 JSON")
+
+            self.get_logger().warning("LLM 响应中既无轻 JSON 也无 plan/decision 字段")
+            self._handle_llm_failure("missing_data")
+            return
+
+        try:
+            plan = Plan.from_dict(plan_data)
+            if not plan.source or plan.source == "":
+                plan.source = f"llm_{source}"
+            self.get_logger().info(
+                f"LLM 返回有效 Plan [plan_id={plan.plan_id}], "
+                f"steps={len(plan.steps)}, source={plan.source}"
+            )
+            self.publish_plan(plan)
+            self._clear_waiting_state()
+        except Exception as e:
+            self.get_logger().error(f"Plan.from_dict() 失败: {e}")
+            self._handle_llm_failure("invalid_plan_format")
+
+    # =========================================================================
+    # LLM 失败处理
+    # =========================================================================
+    def _handle_llm_failure(self, reason: str) -> None:
+        """LLM 失败处理
+
+        生成并发布 fallback Plan，清理等待状态。
+
+        Args:
+            reason: 失败原因标识
+        """
+        user_text = self.pending_user_text or "未知输入"
+        fallback_plan = self.build_fallback_plan(
+            user_intent=user_text,
+            reason=reason,
+        )
+        self.publish_plan(fallback_plan)
+        self._clear_waiting_state()
+
+    def _clear_waiting_state(self) -> None:
+        """清理 LLM 等待状态和相关缓存"""
+        self.waiting_for_llm = False
+        self.pending_request_id = None
+        self.llm_request_timestamp = None
+        self.pending_user_text = None
+
+    # =========================================================================
+    # LLM 超时检测（Timer 回调）
+    # =========================================================================
+    def _check_llm_timeout(self) -> None:
+        """定时检查 LLM 是否超时
+
+        当等待时间超过 llm_timeout_sec 时，自动触发 fallback。
+        """
+        if not self.waiting_for_llm or self.llm_request_timestamp is None:
+            return
+
+        elapsed = time.time() - self.llm_request_timestamp
+        if elapsed > self.llm_timeout_sec:
+            self.get_logger().warning(
+                f"LLM 请求超时（已等待 {elapsed:.1f}s > {self.llm_timeout_sec}s），"
+                "生成 fallback Plan"
+            )
+            self._handle_llm_failure("timeout")
+
+    # =========================================================================
+    # 发布 Plan
+    # =========================================================================
+    def publish_plan(self, plan: 'Plan') -> None:
+        """发布 Plan 到 /plan topic
+
+        将 plan.to_dict() 转为 JSON 字符串后发布。
+
+        Args:
+            plan: Plan 对象
+        """
+        try:
+            plan_dict = plan.to_dict()
+            plan_json = json.dumps(plan_dict, ensure_ascii=False)
+
+            msg = String()
+            msg.data = plan_json
+            self.plan_pub.publish(msg)
+
+            self.has_pending_plan = True
+
+            self.get_logger().info(
+                f"发布 Plan [plan_id={plan.plan_id}, source={plan.source}, "
+                f"goal='{plan.goal}', risk={plan.risk_level.value}, "
+                f"steps={len(plan.steps)}]"
+            )
+        except Exception as e:
+            self.get_logger().error(f"发布 Plan 失败: {e}")
+
+    # =========================================================================
+    # 构建 Fallback Plan
+    # =========================================================================
+    def build_fallback_plan(self, user_intent: str, reason: str = "") -> 'Plan':
+        """生成安全的 fallback Plan
+
+        优先使用 ASK_USER，不直接生成 EXECUTE 步骤。
+
+        Args:
+            user_intent: 用户原始意图
+            reason: 回退原因
+
+        Returns:
+            Plan: 安全的 fallback Plan 对象
+        """
+        plan_id = f"plan_fallback_{uuid.uuid4().hex[:8]}"
+
+        plan = Plan(
+            plan_id=plan_id,
+            goal=f"无法处理的请求: {user_intent}",
+            reasoning=f"Planner 遇到问题，生成安全 fallback (reason={reason})",
+            risk_level=RiskLevel.LOW,
+            requires_confirmation=False,
+            confirmation_message=None,
+            status=PlanStatus.CREATED,
+            source="planner_node_fallback",
+            metadata={"fallback": True, "reason": reason},
+        )
+
+        step = PlanStep(
+            step_id=1,
+            action="ask_user",
+            tool_call_type=ToolCallType.ASK_USER,
+            parameters={
+                "question": f"抱歉，我无法完全理解您的请求: {user_intent}。"
+                           f"请用更明确的词语描述您想要执行的操作。",
+                "original_intent": user_intent,
+            },
+            preconditions={},
+            fallback=None,
+            status=StepStatus.PENDING,
+            description="fallback - 请求用户澄清意图",
+            requires_confirmation=False,
+            confirmation_message=None,
+            metadata={"fallback_reason": reason},
+        )
+        plan.add_step(step)
+
+        self.get_logger().warning(
+            f"生成 Fallback Plan [plan_id={plan_id}], reason={reason}"
+        )
+        return plan
+
+
+def main(args=None) -> None:
+    """节点主入口"""
+    rclpy.init(args=args)
+    try:
+        node = PlannerNode()
+        try:
+            rclpy.spin(node)
+        except KeyboardInterrupt:
+            node.get_logger().info("收到 KeyboardInterrupt，正在退出...")
+        finally:
+            node.destroy_node()
+    finally:
+        rclpy.shutdown()
+
+
+if __name__ == "__main__":
+    main()
+

brain/PlannerNode/src/agint_brain/setup.cfg

@@ -0,0 +1,6 @@
+[develop]
+script_dir=$base/lib/agint_brain
+
+[install]
+install_scripts=$base/lib/agint_brain
+

brain/PlannerNode/src/agint_brain/setup.py

@@ -0,0 +1,29 @@
+from setuptools import setup
+from glob import glob
+import os
+
+package_name = 'agint_brain'
+
+setup(
+    name=package_name,
+    version='0.1.0',
+    py_modules=['planner_node'],
+    data_files=[
+        ('share/ament_index/resource_index/packages', ['resource/' + package_name]),
+        ('share/' + package_name, ['package.xml']),
+        (os.path.join('share', package_name, 'launch'), glob('launch/*.py')),
+        (os.path.join('share', package_name, 'config'), glob('config/*.yaml')),
+    ],
+    install_requires=['setuptools'],
+    zip_safe=True,
+    maintainer='sunrise',
+    maintainer_email='sunrise@example.com',
+    description='AGINT brain planner node',
+    license='Apache License 2.0',
+    tests_require=['pytest'],
+    entry_points={
+        'console_scripts': [
+            'planner_node = planner_node:main',
+        ],
+    },
+)

brain/PlannerNode2/config_node/config/database.yaml

@@ -0,0 +1,40 @@
+# 数据库连接配置
+# Database connection configuration
+
+# MySQL 连接参数
+mysql:
+  host: "localhost"
+  port: 3306
+  user: "root"
+  password: "your_password"
+  database: "robot_config"
+  charset: "utf8mb4"
+
+  # 连接池配置
+  pool:
+    min_size: 1
+    max_size: 5
+    max_overflow: 10
+    pool_timeout: 30
+    pool_recycle: 3600
+
+  # 重连配置
+  reconnect:
+    max_attempts: 5
+    retry_interval: 5  # 秒
+
+# 配置刷新策略
+refresh:
+  enabled: true
+  interval_seconds: 1  # 定时刷新间隔
+
+# Fallback 配置 (当 MySQL 不可用时使用)
+fallback:
+  enabled: true
+  use_yaml_fallback: true
+
+# Topic 发布配置
+publish:
+  topic_name: "/ai/config"
+  qos_depth: 10
+

brain/PlannerNode2/config_node/config_node/__init__.py

@@ -0,0 +1 @@
+# config_node package

brain/PlannerNode2/config_node/config_node/config_node.py

@@ -0,0 +1,430 @@
+#!/usr/bin/env python3
+"""
+配置管理节点 (Config Node)
+从 MySQL 读取配置并发布到 /ai/config topic
+
+功能:
+1. 从数据库读取配置（当前阶段使用模拟数据）
+2. 定时刷新配置
+3. 发布配置到 /ai/config topic (JSON 格式)
+
+Author: sunrise
+"""
+
+import rclpy
+from rclpy.node import Node
+from std_msgs.msg import String
+import json
+import time
+import yaml
+import os
+from datetime import datetime
+from ament_index_python.packages import get_package_share_directory
+
+
+class ConfigNode(Node):
+    """
+    配置管理节点
+
+    从 MySQL 读取配置，定时发布到 /ai/config topic
+    当前阶段使用模拟数据，后续接入 MySQL
+    """
+
+    def __init__(self):
+        super().__init__('config_node')
+
+        # ========== 参数声明 ==========
+        self.declare_parameter('config_file', '')
+        self.declare_parameter('refresh_interval', 1.0)  # 秒
+        self.declare_parameter('use_mock_data', True)  # 使用模拟数据
+        self.declare_parameter('use_yaml_fallback', True)  # 使用 YAML fallback
+        self.declare_parameter('topic_name', '/ai/config')
+
+        # 获取参数
+        pkg_share = get_package_share_directory('config_node')
+        default_config_file = os.path.join(pkg_share, 'config', 'database.yaml')
+        self.config_file = self.get_parameter('config_file').value or default_config_file
+        self.refresh_interval = self.get_parameter('refresh_interval').value
+        self.use_mock_data = self.get_parameter('use_mock_data').value
+        self.use_yaml_fallback = self.get_parameter('use_yaml_fallback').value
+        self.topic_name = self.get_parameter('topic_name').value
+
+        # ========== 数据库连接管理器 (预留) ==========
+        self.db_manager = None
+        self.db_config = {}
+
+        # ========== 配置状态 ==========
+        self.current_config = {}
+        self.config_version = 0
+        self.last_update_time = None
+
+        # ========== 加载配置 ==========
+        self.load_database_config()
+        self.load_config()
+
+        # ========== Publisher ==========
+        self.config_publisher = self.create_publisher(
+            String,
+            self.topic_name,
+            10
+        )
+
+        # ========== 定时器 (定时发布配置) ==========
+        self.timer = self.create_timer(
+            self.refresh_interval,
+            self.timer_callback
+        )
+
+        # ========== 初始化时立即发布一次 ==========
+        self.publish_config()
+
+        self.get_logger().info('=' * 60)
+        self.get_logger().info('Config Node 启动成功')
+        self.get_logger().info(f'配置文件: {self.config_file}')
+        self.get_logger().info(f'刷新间隔: {self.refresh_interval} 秒')
+        self.get_logger().info(f'使用模拟数据: {self.use_mock_data}')
+        self.get_logger().info(f'发布 Topic: {self.topic_name}')
+        self.get_logger().info('=' * 60)
+
+    def load_database_config(self):
+        """加载数据库配置文件"""
+        if not os.path.exists(self.config_file):
+            self.get_logger().warn(f'配置文件不存在: {self.config_file}，使用默认配置')
+            return
+
+        try:
+            with open(self.config_file, 'r', encoding='utf-8') as f:
+                config = yaml.safe_load(f)
+
+            self.db_config = config.get('mysql', {})
+            refresh_config = config.get('refresh', {})
+            self.refresh_interval = refresh_config.get('interval_seconds', 1.0)
+
+            self.get_logger().info(f'成功加载数据库配置: {self.config_file}')
+
+        except Exception as e:
+            self.get_logger().error(f'加载数据库配置失败: {e}')
+
+    def load_config(self):
+        """加载配置数据"""
+        if self.use_mock_data:
+            self.load_mock_config()
+        else:
+            self.load_from_database()
+
+    def load_mock_config(self):
+        """加载模拟配置数据"""
+        self.get_logger().info('加载模拟配置数据...')
+
+        self.current_config = {
+            "version": "1.0.0",
+            "timestamp": self.get_timestamp(),
+            "source": "config_node (mock)",
+            "config": {
+                # ========== ASR 节点配置 ==========
+                "asr": {
+                    "VAD_MODE": 2,
+                    "sample_rate": 16000,
+                    "frame_duration_ms": 30,
+                    "use_oline_asr": False,
+                    "mic_serial_port": "/dev/ttyUSB0",
+                    "mic_index": -2,
+                    "language": "zh",
+                    "regional_setting": "China"
+                },
+
+                # ========== Action Service 节点配置 ==========
+                "action_service": {
+                    "Speed_topic": "/cmd_vel",
+                    "text_chat_mode": True,
+                    "image_topic": "/camera/color/image_raw",
+                    "useolinetts": False,
+                    "language": "zh",
+                    "regional_setting": "China"
+                },
+
+                # ========== Model Service 节点配置 ==========
+                "model_service": {
+                    "language": "zh",
+                    "regional_setting": "China",
+                    "text_chat_mode": True
+                },
+
+                # ========== 大模型接口配置 ==========
+                "large_model": {
+                    # 阿里百炼配置
+                    "tongyi_api_key": "sk-3b05bc8f1bdc40cc87bd05f864890bd8",
+                    "tongyi_app_id": "6ed9f00173214e7883af7310731a5d7b",
+
+                    # 多模态模型
+                    "multimodel": "qwen-vl-max-2025-04-08",
+
+                    # TTS 配置
+                    "tts_supplier": "aliyun",
+                    "tts_language": "zh",
+                    "oline_tts_model": "cosyvoice-v2",
+                    "voice_tone": "longwan_v2",
+
+                    # ASR 配置
+                    "oline_asr_sample_rate": 16000,
+                    "oline_asr_model": "paraformer-realtime-v2",
+
+                    # 百度 TTS 配置
+                    "baidu_API_KEY": "Ppprf0XqOyQ6uOv2rGg34oR7",
+                    "baidu_SECRET_KEY": "1jGl14dF6efRcMOCgiBKwSO8CnlPjBdz",
+                    "CUID": "nLSB0tSszSlc2vxM9gQ96FksFuSrQ2cp",
+                    "PER": 103,
+                    "SPD": 5,
+                    "PIT": 5,
+                    "VOL": 5,
+
+                    # 网络适配器
+                    "network_adapter": "wlP1p1s0",
+                },
+
+                # ========== 国际版配置 ==========
+                "international": {
+                    "decision_AI_api_key": "app-E7cPag5diUJSo4VmRLQXCJoB",
+                    "execution_AI_api_key": "app-2DomQwAo3VdUhjf6qiR9NPmP"
+                },
+
+                # ========== 本地模型路径配置 ==========
+                "model_paths": {
+                    "zh_tts_model": "/home/sunrise/opt/app/yahboom_ws/src/largemodel/MODELS/tts/zh/zh_CN-huayan-medium.onnx",
+                    "zh_tts_json": "/home/sunrise/opt/app/yahboom_ws/src/largemodel/MODELS/tts/zh/zh_CN-huayan-medium.onnx.json",
+                    "en_tts_model": "/home/sunrise/opt/app/yahboom_ws/src/largemodel/MODELS/tts/en/en_US-libritts-high.onnx",
+                    "en_tts_json": "/home/sunrise/opt/app/yahboom_ws/src/largemodel/MODELS/tts/en/en_US-libritts-high.onnx.json",
+                    "local_asr_model": "/home/sunrise/opt/app/yahboom_ws/src/largemodel/MODELS/asr/SenseVoiceSmall"
+                },
+
+                # ========== 系统配置 ==========
+                "system": {
+                    "tongyi_base_url": "https://dashscope.aliyuncs.com/compatible-mode/v1",
+                    "local_tts_enabled": True,
+                    "local_asr_enabled": True
+                },
+
+                # ========== Topic 配置 ==========
+                "topics": {
+                    # action_service 节点
+                    "action_service": {
+                        "Speed_topic": "/cmd_vel",           # 速度控制话题
+                        "image_topic": "/camera/color/image_raw",  # 图像话题
+                        "tts_topic": "tts_topic",            # TTS 语音合成话题
+                        "reset_flag": "reset_flag",          # 复位标志话题
+                        "interrupt_flag": "interrupt_flag",  # 中断标志话题
+                        "arm_done_topic": "/largemodel_arm_done",  # 机械臂完成话题
+                        "wakeup_topic": "wakeup",            # 唤醒话题
+                        "record_status_topic": "record_status"  # 录音状态话题
+                    },
+
+                    # model_service 节点
+                    "model_service": {
+                        "actionstatus_topic": "actionstatus",  # 动作状态话题
+                        "asr_topic": "asr",                 # ASR 语音识别话题
+                        "seewhat_topic": "seewhat_handle",  # 视觉检测话题
+                        "text_response_topic": "text_response"  # 文字回复话题
+                    },
+
+                    # environment_node 节点
+                    "environment_node": {
+                        "environment_topic": "/ai/env"  # 与 environment.publish_topic 保持一致
+                    }
+                },
+
+                # ========== Environment 节点配置 ==========
+                "environment": {
+                    # 发布配置
+                    "publish_topic": "/ai/env",
+                    "intervals": {
+                        "battery_seconds": 1,
+                        "temperature_seconds": 1,
+                        "weather_seconds": 1,
+                        "map_seconds": 1
+                    }
+                }
+            }
+        }
+
+        self.config_version += 1
+        self.last_update_time = datetime.now()
+        self.get_logger().info('模拟配置数据加载完成')
+
+    def load_from_database(self):
+        """
+        从 MySQL 数据库加载配置
+        预留接口，后续实现
+        """
+        self.get_logger().info('准备从数据库加载配置...')
+
+        # ========== TODO: 实现 MySQL 连接 ==========
+        # 1. 建立数据库连接 (长连接)
+        # 2. 查询配置表
+        # 3. 解析配置数据
+        # 4. 更新 current_config
+        # 5. 错误处理和重连
+
+        # 当前返回模拟数据作为 fallback
+        self.get_logger().warn('数据库功能尚未实现，使用 YAML fallback 配置')
+        self.load_yaml_fallback()
+
+    def load_yaml_fallback(self):
+        """从 YAML 文件加载 fallback 配置"""
+        self.get_logger().info('从 YAML 文件加载 fallback 配置...')
+
+        # 尝试从 largemodel 包加载配置
+        try:
+            import subprocess
+            result = subprocess.run(
+                ['find', '/home/sunrise/opt/dev/project/aiagent/brain/PlannerNode2', '-name', 'yahboom.yaml'],
+                capture_output=True, text=True, timeout=5
+            )
+            yaml_paths = result.stdout.strip().split('\n')
+
+            if yaml_paths and yaml_paths[0]:
+                self.get_logger().info(f'找到 YAML 配置文件: {yaml_paths[0]}')
+                with open(yaml_paths[0], 'r', encoding='utf-8') as f:
+                    yaml_config = yaml.safe_load(f)
+
+                # 转换格式
+                self.current_config = self.convert_yaml_to_json(yaml_config)
+                self.config_version += 1
+                self.last_update_time = datetime.now()
+                self.get_logger().info('YAML fallback 配置加载完成')
+            else:
+                self.get_logger().warn('未找到 YAML 配置文件，使用默认模拟数据')
+                self.load_mock_config()
+
+        except Exception as e:
+            self.get_logger().error(f'加载 YAML fallback 配置失败: {e}')
+            self.get_logger().warn('使用默认模拟数据')
+            self.load_mock_config()
+
+    def convert_yaml_to_json(self, yaml_config):
+        """将 YAML 配置转换为统一格式"""
+        config = {
+            "version": "1.0.0",
+            "timestamp": self.get_timestamp(),
+            "source": "config_node (yaml_fallback)",
+            "config": {}
+        }
+
+        # 提取 asr 配置
+        if 'asr' in yaml_config:
+            config['config']['asr'] = yaml_config['asr'].get('ros__parameters', {})
+
+        # 提取 action_service 配置
+        if 'action_service' in yaml_config:
+            config['config']['action_service'] = yaml_config['action_service'].get('ros__parameters', {})
+
+        # 提取 model_service 配置
+        if 'model_service' in yaml_config:
+            config['config']['model_service'] = yaml_config['model_service'].get('ros__parameters', {})
+
+        return config
+
+    def get_timestamp(self):
+        """获取当前时间戳"""
+        return datetime.now().strftime('%Y-%m-%dT%H:%M:%SZ')
+
+    def timer_callback(self):
+        """定时器回调 - 刷新并发布配置"""
+        # 重新加载配置
+        self.load_config()
+
+        # 发布配置
+        self.publish_config()
+
+    def publish_config(self):
+        """发布配置到 /ai/config topic"""
+        if not self.current_config:
+            self.get_logger().warn('配置为空，跳过发布')
+            return
+
+        # 更新时间戳
+        self.current_config['timestamp'] = self.get_timestamp()
+        self.current_config['version'] = f"1.0.{self.config_version}"
+
+        # 序列化为 JSON
+        try:
+            json_str = json.dumps(self.current_config, ensure_ascii=False, indent=2)
+
+            # 发布消息
+            msg = String()
+            msg.data = json_str
+            self.config_publisher.publish(msg)
+
+            self.get_logger().info(f'配置已发布 (version: {self.current_config["version"]})')
+            self.get_logger().debug(f'配置内容: {json_str[:200]}...' if len(json_str) > 200 else f'配置内容: {json_str}')
+
+        except Exception as e:
+            self.get_logger().error(f'发布配置失败: {e}')
+
+    def connect_database(self):
+        """
+        建立 MySQL 数据库连接
+        预留接口
+        """
+        # ========== TODO: 实现长连接 ==========
+        # import pymysql
+        # self.db_manager = pymysql.connect(
+        #     host=self.db_config.get('host', 'localhost'),
+        #     port=self.db_config.get('port', 3306),
+        #     user=self.db_config.get('user', 'root'),
+        #     password=self.db_config.get('password', ''),
+        #     database=self.db_config.get('database', 'robot_config'),
+        #     charset=self.db_config.get('charset', 'utf8mb4'),
+        #     connect_timeout=30
+        # )
+        pass
+
+    def reconnect_database(self):
+        """
+        重新连接数据库
+        预留接口
+        """
+        # ========== TODO: 实现重连逻辑 ==========
+        # max_attempts = self.db_config.get('reconnect', {}).get('max_attempts', 5)
+        # retry_interval = self.db_config.get('reconnect', {}).get('retry_interval', 5)
+        #
+        # for attempt in range(max_attempts):
+        #     try:
+        #         self.connect_database()
+        #         self.get_logger().info('数据库重连成功')
+        #         return True
+        #     except Exception as e:
+        #         self.get_logger().warn(f'数据库重连失败 (尝试 {attempt + 1}/{max_attempts}): {e}')
+        #         time.sleep(retry_interval)
+        #
+        # self.get_logger().error('数据库重连失败，使用 fallback 配置')
+        # return False
+        pass
+
+    def shutdown(self):
+        """关闭节点时清理资源"""
+        self.get_logger().info('关闭 Config Node...')
+        if self.db_manager:
+            try:
+                self.db_manager.close()
+                self.get_logger().info('数据库连接已关闭')
+            except Exception as e:
+                self.get_logger().warn(f'关闭数据库连接时出错: {e}')
+
+
+def main(args=None):
+    rclpy.init(args=args)
+
+    node = ConfigNode()
+
+    try:
+        rclpy.spin(node)
+    except KeyboardInterrupt:
+        pass
+    finally:
+        node.shutdown()
+        node.destroy_node()
+        rclpy.shutdown()
+
+
+if __name__ == '__main__':
+    main()

brain/PlannerNode2/config_node/launch/config_node.launch.py

@@ -0,0 +1,54 @@
+"""
+配置管理节点启动文件
+Config Node Launch File
+
+启动配置管理节点，从 MySQL 读取配置并发布到 /ai/config topic
+"""
+
+from launch import LaunchDescription
+from launch_ros.actions import Node
+from launch.actions import DeclareLaunchArgument
+from launch.substitutions import LaunchConfiguration
+
+
+def generate_launch_description():
+    """生成 launch 描述"""
+
+    # 声明启动参数
+    use_mock_data_arg = DeclareLaunchArgument(
+        'use_mock_data',
+        default_value='true',
+        description='是否使用模拟数据 (true/false)'
+    )
+
+    refresh_interval_arg = DeclareLaunchArgument(
+        'refresh_interval',
+        default_value='30.0',
+        description='配置刷新间隔 (秒)'
+    )
+
+    topic_name_arg = DeclareLaunchArgument(
+        'topic_name',
+        default_value='/ai/config',
+        description='配置发布的话题名称'
+    )
+
+    # 定义节点
+    config_node = Node(
+        package='config_node',
+        executable='config_node',
+        name='config_node',
+        output='screen',
+        parameters=[{
+            'use_mock_data': LaunchConfiguration('use_mock_data'),
+            'refresh_interval': LaunchConfiguration('refresh_interval'),
+            'topic_name': LaunchConfiguration('topic_name'),
+        }],
+    )
+
+    return LaunchDescription([
+        use_mock_data_arg,
+        refresh_interval_arg,
+        topic_name_arg,
+        config_node,
+    ])

brain/PlannerNode2/config_node/package.xml

@@ -0,0 +1,23 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>config_node</name>
+  <version>0.0.0</version>
+  <description>配置管理节点 - 从 MySQL 读取配置并发布到 /ai/config topic</description>
+  <maintainer email="sunrise@todo.todo">sunrise</maintainer>
+  <license>TODO: License declaration</license>
+
+  <build_depend>ament_python</build_depend>
+
+  <exec_depend>python3-yaml</exec_depend>
+  <exec_depend>python3-pymysql</exec_depend>
+
+  <test_depend>ament_copyright</test_depend>
+  <test_depend>ament_flake8</test_depend>
+  <test_depend>ament_pep257</test_depend>
+  <test_depend>python3-pytest</test_depend>
+
+  <export>
+    <build_type>ament_python</build_type>
+  </export>
+</package>

brain/PlannerNode2/config_node/setup.cfg

@@ -0,0 +1,5 @@
+[develop]
+script_dir=$base/lib/config_node
+
+[install]
+install_scripts=$base/lib/config_node

brain/PlannerNode2/config_node/setup.py

@@ -0,0 +1,28 @@
+from setuptools import setup
+
+package_name = 'config_node'
+
+setup(
+    name=package_name,
+    version='0.0.0',
+    packages=[package_name],
+    data_files=[
+        ('share/ament_index/resource_index/packages',
+            ['resource/' + package_name]),
+        ('share/' + package_name, ['package.xml']),
+        ('share/' + package_name + '/launch', ['launch/config_node.launch.py']),
+        ('share/' + package_name + '/config', ['config/database.yaml']),
+    ],
+    install_requires=['setuptools'],
+    zip_safe=True,
+    maintainer='sunrise',
+    maintainer_email='sunrise@todo.todo',
+    description='配置管理节点 - 从 MySQL 读取配置并发布到 /ai/config topic',
+    license='TODO: License declaration',
+    tests_require=['pytest'],
+    entry_points={
+        'console_scripts': [
+            'config_node = config_node.config_node:main',
+        ],
+    },
+)

brain/PlannerNode2/environment_node/config/environment.yaml

@@ -0,0 +1,28 @@
+# Environment Node 配置文件
+# 此文件作为 fallback 配置，实际配置由 config_node 统一管理
+
+# 发布配置
+publish_topic: "/ai/env"
+
+# 发布间隔 (秒)
+intervals:
+  battery_seconds: 1
+  temperature_seconds: 1
+  weather_seconds: 1
+  map_seconds: 1
+
+
+map:
+  points:
+    - id: "A"
+      name: "办公室"
+      position: {x: 1.633, y: 3.490, z: 0.0}
+      orientation: {x: 0.0, y: 0.0, z: 0.618, w: 0.786}
+    - id: "B"
+      name: "大堂接待"
+      position: {x: 2.436, y: -0.574, z: 0.0}
+      orientation: {x: 0.0, y: 0.0, z: -0.090, w: 0.996}
+    - id: "C"
+      name: "园区"
+      position: {x: 0.024, y: -1.820, z: 0.0}
+      orientation: {x: 0.0, y: 0.0, z: 0.031, w: 1.000}

brain/PlannerNode2/environment_node/environment_node/__init__.py

@@ -0,0 +1 @@
+# Environment Node Package

brain/PlannerNode2/environment_node/environment_node/environment_node.py

@@ -0,0 +1,363 @@
+#!/usr/bin/env python3
+"""
+环境节点 (Environment Node)
+统一发布机器人环境相关数据，供其他节点订阅使用
+
+功能:
+1. 订阅 config_node 获取配置
+2. 发布电池数据、温度数据、天气数据、地图导航点
+3. 统一发布到 /ai/environment topic
+
+Author: sunrise
+"""
+
+import rclpy
+from rclpy.node import Node
+from std_msgs.msg import String
+import json
+import time
+from datetime import datetime
+from threading import Thread, Event
+
+
+class EnvironmentNode(Node):
+    """
+    环境节点
+
+    订阅 config_node 配置，统一发布环境数据到 /ai/environment topic
+    """
+
+    def __init__(self):
+        super().__init__('environment_node')
+
+        # ========== 参数声明 ==========
+        self.declare_parameter('config_topic', '/ai/config')
+        self.declare_parameter('environment_topic', '/ai/env')
+        self.declare_parameter('use_mock_data', True)
+
+        # 获取参数
+        self.config_topic = self.get_parameter('config_topic').value
+        self.environment_topic = self.get_parameter('environment_topic').value
+        self.use_mock_data = self.get_parameter('use_mock_data').value
+
+        # ========== 配置数据 (从 config_node 获取) ==========
+        self.config_data = None
+        self.environment_config = {}
+
+        # ========== 环境数据状态 (模拟数据，由节点自己生成) ==========
+        self.battery_data = {}
+        self.temperature_data = {}
+        self.weather_data = {}
+        self.map_data = {}
+
+        # ========== 发布频率 (秒) - 等待配置更新 ==========
+        self.intervals = {
+            'battery': 5,
+            'temperature': 30,
+            'weather': 3600,
+            'map': 60
+        }
+
+        # ========== 定时器状态 ==========
+        self.publish_timers = {}
+        self.last_publish_time = {
+            'battery': 0,
+            'temperature': 0,
+            'weather': 0,
+            'map': 0
+        }
+
+        # ========== 线程和事件 ==========
+        self.stop_event = Event()
+
+        # ========== 初始化 ==========
+        self.init_subscriber()  # 订阅 config_node
+        self.init_publisher()   # 发布环境数据
+        self.init_default_data()  # 初始化默认数据（生成模拟数据）
+
+        self.get_logger().info('=' * 60)
+        self.get_logger().info('Environment Node 启动成功')
+        self.get_logger().info(f'订阅配置 Topic: {self.config_topic}')
+        self.get_logger().info(f'发布环境 Topic: {self.environment_topic}')
+        self.get_logger().info('等待 config_node 配置...')
+        self.get_logger().info('=' * 60)
+
+    def init_subscriber(self):
+        """初始化订阅者 - 订阅 config_node 配置"""
+        self.config_subscriber = self.create_subscription(
+            String,
+            self.config_topic,
+            self.config_callback,
+            10
+        )
+        self.get_logger().info(f'已订阅配置 Topic: {self.config_topic}')
+
+    def init_publisher(self):
+        """初始化发布者 - 发布环境数据"""
+        self.environment_publisher = self.create_publisher(
+            String,
+            self.environment_topic,
+            10
+        )
+        self.get_logger().info(f'已创建发布者: {self.environment_topic}')
+
+        # 添加统一发布定时器：每秒发布一次最新数据
+        self.unified_publish_timer = self.create_timer(
+            1.0,
+            self.publish_latest_data
+        )
+        self.get_logger().info('已创建统一发布定时器 (1秒)')
+
+    def publish_latest_data(self):
+        """每秒发布一次最新环境数据"""
+        env_data = {
+            "timestamp": datetime.now().strftime('%Y-%m-%dT%H:%M:%S'),
+            "battery": self.battery_data,
+            "temperature": self.temperature_data,
+            "weather": self.weather_data,
+            "map": self.map_data
+        }
+        try:
+            json_str = json.dumps(env_data, ensure_ascii=False, indent=2)
+            msg = String()
+            msg.data = json_str
+            self.environment_publisher.publish(msg)
+        except Exception as e:
+            self.get_logger().error(f'发布环境数据失败: {e}')
+
+    def init_default_data(self):
+        """初始化默认环境模拟数据"""
+        self.generate_mock_battery()
+        self.generate_mock_temperature()
+        self.generate_mock_weather()
+        self.generate_mock_map()
+        self.get_logger().info('模拟数据生成完成')
+
+    def generate_mock_battery(self):
+        """生成电池模拟数据"""
+        self.battery_data = {
+            "level": 85,
+            "voltage": 12.6,
+            "is_charging": False,
+            "capacity_wh": 5000,
+            "current_ma": -500
+        }
+
+    def generate_mock_temperature(self):
+        """生成温度模拟数据"""
+        self.temperature_data = {
+            "indoor": 25.5,
+            "outdoor": 28.0,
+            "unit": "celsius",
+            "humidity": 60
+        }
+
+    def generate_mock_weather(self):
+        """生成天气模拟数据"""
+        self.weather_data = {
+            "condition": "sunny",
+            "description": "晴",
+            "temperature": 28,
+            "humidity": 65,
+            "wind_speed": 3.5
+        }
+
+    def generate_mock_map(self):
+        """生成地图导航点模拟数据"""
+        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}}
+            ]
+        }
+
+    def config_callback(self, msg: String):
+        """配置订阅回调 - 解析 config_node 发布的配置"""
+        try:
+            config = json.loads(msg.data)
+            self.get_logger().debug(f'收到配置: version={config.get("version")}')
+
+            # 提取 environment 配置
+            if 'config' in config and 'environment' in config['config']:
+                self.environment_config = config['config']['environment']
+                self.parse_environment_config()
+
+        except json.JSONDecodeError as e:
+            self.get_logger().error(f'配置解析失败: {e}')
+        except Exception as e:
+            self.get_logger().error(f'配置处理异常: {e}')
+
+    def parse_environment_config(self):
+        """解析 environment 配置并启动定时器"""
+        if not self.environment_config:
+            return
+
+        self.get_logger().info('解析环境配置...')
+
+        # 更新发布间隔
+        intervals = self.environment_config.get('intervals', {})
+        self.intervals['battery'] = intervals.get('battery_seconds', 5)
+        self.intervals['temperature'] = intervals.get('temperature_seconds', 30)
+        self.intervals['weather'] = intervals.get('weather_seconds', 3600)
+        self.intervals['map'] = intervals.get('map_seconds', 60)
+
+        # 更新 topic 名称
+        topic = self.environment_config.get('publish_topic')
+        if topic and topic != self.environment_topic:
+            self.environment_topic = topic
+            # 重新创建发布者
+            self.environment_publisher = self.create_publisher(
+                String,
+                self.environment_topic,
+                10
+            )
+            self.get_logger().info(f'环境数据发布 Topic 已更新: {self.environment_topic}')
+
+        # 重新生成模拟数据
+        self.generate_mock_battery()
+        self.generate_mock_temperature()
+        self.generate_mock_weather()
+        self.generate_mock_map()
+
+        # 启动定时器
+        self.start_timers()
+
+        self.get_logger().info('环境配置解析完成，定时器已启动')
+
+    def start_timers(self):
+        """启动各个数据类型的定时发布"""
+        # 电池数据定时器
+        if 'battery' not in self.publish_timers:
+            self.publish_timers['battery'] = self.create_timer(
+                self.intervals['battery'],
+                lambda: self.publish_data('battery')
+            )
+
+        # 温度数据定时器
+        if 'temperature' not in self.publish_timers:
+            self.publish_timers['temperature'] = self.create_timer(
+                self.intervals['temperature'],
+                lambda: self.publish_data('temperature')
+            )
+
+        # 天气数据定时器
+        if 'weather' not in self.publish_timers:
+            self.publish_timers['weather'] = self.create_timer(
+                self.intervals['weather'],
+                lambda: self.publish_data('weather')
+            )
+
+        # 地图数据定时器
+        if 'map' not in self.publish_timers:
+            self.publish_timers['map'] = self.create_timer(
+                self.intervals['map'],
+                lambda: self.publish_data('map')
+            )
+
+    def publish_data(self, data_type: str):
+        """发布指定类型的数据"""
+        current_time = time.time()
+
+        # 检查是否到发布时间
+        if current_time - self.last_publish_time.get(data_type, 0) < self.intervals.get(data_type, 5):
+            return
+
+        self.last_publish_time[data_type] = current_time
+
+        # 构建环境数据
+        env_data = {
+            "timestamp": datetime.now().strftime('%Y-%m-%dT%H:%M:%S'),
+            "battery": self.battery_data if data_type == 'battery' else self.get_latest_data('battery'),
+            "temperature": self.temperature_data if data_type == 'temperature' else self.get_latest_data('temperature'),
+            "weather": self.weather_data if data_type == 'weather' else self.get_latest_data('weather'),
+            "map": self.map_data if data_type == 'map' else self.get_latest_data('map')
+        }
+
+        # 发布数据
+        try:
+            json_str = json.dumps(env_data, ensure_ascii=False, indent=2)
+            msg = String()
+            msg.data = json_str
+            self.environment_publisher.publish(msg)
+
+            self.get_logger().debug(f'发布 {data_type} 数据成功')
+
+        except Exception as e:
+            self.get_logger().error(f'发布 {data_type} 数据失败: {e}')
+
+    def get_latest_data(self, data_type: str):
+        """获取最新数据"""
+        if data_type == 'battery':
+            return self.battery_data
+        elif data_type == 'temperature':
+            return self.temperature_data
+        elif data_type == 'weather':
+            return self.weather_data
+        elif data_type == 'map':
+            return self.map_data
+        return {}
+
+    def publish_all(self):
+        """立即发布所有环境数据"""
+        env_data = {
+            "timestamp": datetime.now().strftime('%Y-%m-%dT%H:%M:%S'),
+            "battery": self.battery_data,
+            "temperature": self.temperature_data,
+            "weather": self.weather_data,
+            "map": self.map_data
+        }
+
+        try:
+            json_str = json.dumps(env_data, ensure_ascii=False, indent=2)
+            msg = String()
+            msg.data = json_str
+            self.environment_publisher.publish(msg)
+            self.get_logger().info('全量环境数据发布成功')
+
+        except Exception as e:
+            self.get_logger().error(f'全量环境数据发布失败: {e}')
+
+    def get_current_environment(self):
+        """获取当前完整环境数据 (供其他节点调用)"""
+        return {
+            "timestamp": datetime.now().strftime('%Y-%m-%dT%H:%M:%S'),
+            "battery": self.battery_data,
+            "temperature": self.temperature_data,
+            "weather": self.weather_data,
+            "map": self.map_data
+        }
+
+    def shutdown(self):
+        """关闭节点"""
+        self.get_logger().info('关闭 Environment Node...')
+        self.stop_event.set()
+
+        # 取消统一发布定时器
+        if hasattr(self, 'unified_publish_timer'):
+            self.unified_publish_timer.cancel()
+
+        # 取消所有定时器
+        for timer in self.publish_timers.values():
+            timer.cancel()
+
+        self.get_logger().info('Environment Node 已关闭')
+
+
+def main(args=None):
+    rclpy.init(args=args)
+
+    node = EnvironmentNode()
+
+    try:
+        rclpy.spin(node)
+    except KeyboardInterrupt:
+        pass
+    finally:
+        node.shutdown()
+        node.destroy_node()
+        rclpy.shutdown()
+
+
+if __name__ == '__main__':
+    main()

brain/PlannerNode2/environment_node/launch/environment.launch.py

@@ -0,0 +1,32 @@
+"""
+Environment Node 启动文件
+
+功能:
+- 启动环境节点，发布电池、温度、天气、地图导航点数据
+
+Author: sunrise
+"""
+
+from launch import LaunchDescription
+from launch_ros.actions import Node
+
+
+def generate_launch_description():
+    """生成启动描述"""
+
+    # Environment Node
+    environment_node = Node(
+        package='environment_node',
+        executable='environment_node',
+        name='environment_node',
+        output='screen',
+        parameters=[{
+            'config_topic': '/ai/config',
+            'environment_topic': '/ai/environment',
+            'use_mock_data': True
+        }]
+    )
+
+    return LaunchDescription([
+        environment_node
+    ])

brain/PlannerNode2/environment_node/package.xml

@@ -0,0 +1,20 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+    <name>environment_node</name>
+    <version>1.0.0</version>
+    <description>Environment Node - 发布机器人环境数据（电池、温度、天气、地图导航点）</description>
+    <maintainer email="sunrise@example.com">sunrise</maintainer>
+    <license>MIT</license>
+
+    <buildtool_depend>ament_python</buildtool_depend>
+
+    <depend>rclpy</depend>
+    <depend>std_msgs</depend>
+
+    <test_depend>python3-pytest</test_depend>
+
+    <export>
+        <build_type>ament_python</build_type>
+    </export>
+</package>

brain/PlannerNode2/environment_node/resource/environment_node

@@ -0,0 +1 @@
+# Resource marker file

brain/PlannerNode2/environment_node/setup.cfg

@@ -0,0 +1,5 @@
+[develop]
+script_dir=$base/lib/environment_node
+
+[install]
+install_scripts=$base/lib/environment_node

brain/PlannerNode2/environment_node/setup.py

@@ -0,0 +1,30 @@
+from setuptools import setup
+import os
+from glob import glob
+
+package_name = 'environment_node'
+
+setup(
+    name=package_name,
+    version='1.0.0',
+    packages=[package_name],
+    data_files=[
+        ('share/ament_index/resource_index/packages',
+            ['resource/' + package_name]),
+        ('share/' + package_name, ['package.xml']),
+        (os.path.join('share', package_name, 'launch'), glob('launch/*.launch.py')),
+        (os.path.join('share', package_name, 'config'), glob('config/*')),
+    ],
+    install_requires=['setuptools'],
+    zip_safe=True,
+    maintainer='sunrise',
+    maintainer_email='sunrise@example.com',
+    description='Environment Node - 发布机器人环境数据',
+    license='MIT',
+    tests_require=['pytest'],
+    entry_points={
+        'console_scripts': [
+            'environment_node = ' + package_name + '.environment_node:main',
+        ],
+    },
+)

brain/PlannerNode2/interfaces/CMakeLists.txt

@@ -0,0 +1,35 @@
+cmake_minimum_required(VERSION 3.8)
+project(interfaces)
+
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+  add_compile_options(-Wall -Wextra -Wpedantic)
+endif()
+
+# find dependencies
+find_package(ament_cmake REQUIRED)
+# uncomment the following section in order to fill in
+# further dependencies manually.
+# find_package(<dependency> REQUIRED)
+find_package(rosidl_default_generators REQUIRED)
+rosidl_generate_interfaces(${PROJECT_NAME}
+  "action/Rot.action"
+  "srv/Qwen25.srv"
+  "srv/Audio.srv"
+  "srv/Vision.srv"
+  "srv/LargeScaleModel.srv"
+  "srv/Audio2.srv"
+)
+
+if(BUILD_TESTING)
+  find_package(ament_lint_auto REQUIRED)
+  # the following line skips the linter which checks for copyrights
+  # comment the line when a copyright and license is added to all source files
+  set(ament_cmake_copyright_FOUND TRUE)
+  # the following line skips cpplint (only works in a git repo)
+  # comment the line when this package is in a git repo and when
+  # a copyright and license is added to all source files
+  set(ament_cmake_cpplint_FOUND TRUE)
+  ament_lint_auto_find_test_dependencies()
+endif()
+
+ament_package()

brain/PlannerNode2/interfaces/action/Rot.action

@@ -0,0 +1,9 @@
+# 请求定义
+string[] actions
+string llm_response
+---
+# 结果定义
+bool success
+---
+# 反馈定义
+string status

brain/PlannerNode2/interfaces/package.xml

@@ -0,0 +1,28 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>interfaces</name>
+  <version>0.0.0</version>
+  <description>TODO: Package description</description>
+  <maintainer email="kaiser@todo.todo">kaiser</maintainer>
+  <license>TODO: License declaration</license>
+
+  <!-- 构建依赖 -->
+  <build_depend>action_msgs</build_depend>
+  <depend>rosidl_default_generators</depend>
+  <buildtool_depend>ament_cmake</buildtool_depend>
+  <member_of_group>rosidl_interface_packages</member_of_group>
+  <!-- 测试依赖 -->
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>ament_lint_common</test_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>
+
+
+
+
+
+

brain/PlannerNode2/interfaces/srv/Audio.srv

@@ -0,0 +1,4 @@
+string audio_name
+---
+bool success
+string message

brain/PlannerNode2/interfaces/srv/Audio2.srv

@@ -0,0 +1,4 @@
+string text
+---
+bool success
+string message

brain/PlannerNode2/interfaces/srv/LargeScaleModel.srv

@@ -0,0 +1,9 @@
+# 请求部分
+string promtrequest
+string infertype
+---
+# 响应部分
+string response
+string[] action_list  
+bool status
+

brain/PlannerNode2/interfaces/srv/Qwen25.srv

@@ -0,0 +1,8 @@
+# 请求部分
+string promtrequest
+---
+# 响应部分
+string response
+string[] action_list  
+bool status
+

brain/PlannerNode2/interfaces/srv/Vision.srv

@@ -0,0 +1,5 @@
+string resquest_type
+string user_promt
+---
+string message
+

brain/PlannerNode2/largemodel/README.md

@@ -0,0 +1,302 @@
+# Largemodel ROS2 Package
+
+## 概述
+
+`largemodel` 是一个基于 ROS2 Humble 的机器人 AI 服务节点包，集成了语音识别（ASR）、大模型任务规划、双层 AI 决策、动作执行和语音合成（TTS）等功能。该包使机器人能够理解用户的自然语言指令，通过大模型进行任务规划，并自主执行导航、机械臂控制、物体抓取等多种动作。
+
+## 系统架构
+
+```
+┌─────────────────────────────────────────────────────────────────┐
+│                     largemodel_control.launch.py                  │
+├─────────────┬─────────────────┬─────────────────┬───────────────┤
+│ camera_arm  │   model_service │  action_service │      asr      │
+│   _kin      │       节点       │       节点       │      节点     │
+├─────────────┴─────────────────┴─────────────────┴───────────────┤
+│                    large_model_interface (共用工具)               │
+└─────────────────────────────────────────────────────────────────┘
+```
+
+## 核心节点
+
+### 1. ASR 节点 (asr)
+
+**功能**：语音唤醒 → 录音 → 语音识别
+
+- 支持 KWS 语音唤醒（串口通信）
+- WebRTC VAD 语音活动检测
+- 多 ASR 引擎：SenseVoiceSmall（本地）/ 讯飞 / 阿里云（在线）
+
+**发布话题**：
+| 话题 | 类型 | 说明 |
+|------|------|------|
+| `/asr` | String | ASR 识别结果 |
+| `/wakeup` | Bool | 唤醒信号 |
+| `/beep` | UInt16 | 蜂鸣器控制 |
+| `/record_status` | Bool | 录音状态 |
+
+### 2. Model Service 节点 (model_service)
+
+**功能**：AI 大脑，调用大模型进行任务规划和执行
+
+**双层 AI 架构**：
+- **决策层**：解析用户意图，生成任务步骤
+- **执行层**：生成具体动作列表和回复
+
+**订阅话题**：
+| 话题 | 说明 |
+|------|------|
+| `/asr` | ASR 识别结果 |
+| `/actionstatus` | 动作执行反馈 |
+| `/seewhat_handle` | 视觉触发信号 |
+
+### 3. Action Service 节点 (action_service)
+
+**功能**：解析并执行机器人动作
+
+**支持的动作函数（18种）**：
+
+| 类别 | 动作 | 函数签名 |
+|------|------|---------|
+| 基础移动 | 左转/右转 | `move_left(x, speed)`, `move_right(x, speed)` |
+| 基础移动 | 漂移/跳舞 | `drift()`, `dance()` |
+| 基础移动 | 速度控制 | `set_cmdvel(x, y, z, duration)` |
+| 导航 | 导航到点 | `navigation(point_name)` |
+| 导航 | 获取位置 | `get_current_pose()` |
+| 机械臂 | 上下/摇头点头鼓掌 | `arm_up()`, `arm_down()`, `arm_shake()`, `arm_nod()`, `arm_applaud()` |
+| 机械臂 | 抓取/放置 | `grasp_obj(x1,y1,x2,y2)`, `putdown()` |
+| 机械臂 | Apriltag 分拣 | `apriltag_sort(id)`, `apriltag_remove_higher(h)` |
+| 机械臂 | 颜色追踪移除 | `color_remove_higher(color, h)` |
+| 视觉 | 追踪/巡线 | `track(x1,y1,x2,y2)`, `follw_line_clear()` |
+| 系统 | 等待/结束 | `wait(seconds)`, `finish_dialogue()`, `finishtask()` |
+
+## 目录结构
+
+```
+largemodel/
+├── config/
+│   ├── yahboom.yaml              # 主配置文件
+│   ├── large_model_interface.yaml # 大模型 API 配置
+│   └── map_mapping.yaml          # 地图导航点配置
+├── launch/
+│   └── largemodel_control.launch.py  # 启动文件
+├── largemodel/
+│   ├── __init__.py
+│   ├── asr.py                   # 语音识别节点
+│   ├── model_service.py         # 模型服务节点
+│   └── action_service.py        # 动作执行节点
+├── utils/
+│   ├── large_model_interface.py # 大模型接口封装
+│   ├── mic_serial.py            # 串口通信工具
+│   ├── promot.py                # Prompt 模板
+│   └── dify_client2.py          # Dify API 客户端
+├── resources_file/
+│   └── system_vioce/            # 系统提示音
+│       ├── zh/                   # 中文提示音
+│       └── en/                   # 英文提示音
+├── test/                         # 测试文件
+├── package.xml
+└── setup.py
+```
+
+## 配置说明
+
+### 主配置 (yahboom.yaml)
+
+支持国内版和国际版两种模式：
+
+```yaml
+# 国内版本参数
+asr:
+  ros__parameters:
+    VAD_MODE: 2
+    sample_rate: 16000
+    use_oline_asr: True
+    language: 'zh'
+    regional_setting: "China"
+
+action_service:
+  ros__parameters:
+    text_chat_mode: True      # True: 文字模式, False: 语音模式
+    useolinetts: True         # True: 在线 TTS, False: 本地 TTS
+
+model_service:
+  ros__parameters:
+    language: 'zh'
+    regional_setting: "China"
+```
+
+### 大模型配置 (large_model_interface.yaml)
+
+```yaml
+# 阿里百炼配置
+tongyi_api_key: "your-api-key"
+tongyi_base_url: "https://dashscope.aliyuncs.com"
+tongyi_app_id: "your-app-id"
+multimodel: "qwen-vl-max"
+
+# TTS 配置
+tts_supplier: "aliyun"  # 或 "baidu"
+oline_tts_model: "cosyvoice-v1"
+voice_tone: "zh_CN-female-shaohan"
+
+# ASR 配置
+oline_asr_model: "paraformer-realtime-v2"
+oline_asr_sample_rate: 16000
+local_asr_model: "iic/SenseVoiceSmall"
+```
+
+### 地图配置 (map_mapping.yaml)
+
+```yaml
+A:
+  name: '水果店'
+  position:
+    x: 1.633
+    y: 3.490
+    z: 0.0
+  orientation:
+    x: 0.0
+    y: 0.0
+    z: 0.618
+    w: 0.786
+```
+
+## 编译命令
+
+### 1. 进入工作空间
+
+```bash
+cd ~/M3Pro_ws
+```
+
+### 2. 编译 largemodel 包
+
+```bash
+colcon build --packages-select largemodel
+```
+
+或编译整个工作空间：
+
+```bash
+colcon build
+```
+
+### 3. Source 环境
+
+```bash
+source install/setup.bash
+```
+
+## 启动命令
+
+### 1. 启动完整功能（国内版 + 文字交互模式）
+
+```bash
+ros2 launch largemodel largemodel_control.launch.py text_chat_mode:=True
+```
+
+### 2. 启动完整功能（国内版 + 语音交互模式）
+
+```bash
+ros2 launch largemodel largemodel_control.launch.py text_chat_mode:=False
+```
+
+### 3. 仅启动特定节点
+
+```bash
+# 启动 ASR 节点
+ros2 run largemodel asr
+
+# 启动模型服务节点
+ros2 run largemodel model_service
+
+# 启动动作服务节点
+ros2 run largemodel action_service
+```
+
+### 4. 带参数启动
+
+```bash
+ros2 launch largemodel largemodel_control.launch.py \
+    text_chat_mode:=True \
+    language:=zh \
+    regional_setting:=China
+```
+
+## 依赖项
+
+### 系统依赖
+
+- ROS2 Humble
+- Python 3.8+
+- pip
+
+### Python 依赖
+
+```bash
+pip install rclpy std_msgs geometry_msgs sensor_msgs nav2_msgs
+pip install pyaudio webrtcvad playsound pygame
+pip install dashscope openai piper funasr
+pip install pyserial cv-bridge pyyaml
+```
+
+### ROS2 依赖
+
+- `interfaces` (自定义 action 包)
+- `arm_msgs` (机械臂消息)
+- `arm_interface` (机械臂接口)
+- `M3Pro_demo` (相机和运动学节点)
+
+## 工作流程
+
+```
+用户语音 ──▶ ASR 节点 ──▶ Model Service ──▶ Action Service ──▶ 机器人执行
+              │              │                 │
+              │              │                 ▼
+              │              │            动作反馈
+              │              │                 │
+              ▼              ▼                 ▼
+           唤醒成功      双层AI规划      执行完成
+           提示音        任务分解        语音播报
+```
+
+## 注意事项
+
+1. **串口权限**：确保 `/dev/mic` 串口设备有正确的读写权限
+2. **API 密钥**：在 `large_model_interface.yaml` 中配置正确的大模型 API 密钥
+3. **地图配置**：根据实际场景修改 `map_mapping.yaml` 中的导航点坐标
+4. **资源文件**：确保 `resources_file` 目录下的音频文件存在
+5. **多线程执行**：Action Service 使用 6 线程执行器，确保节点稳定运行
+
+## 故障排除
+
+### ASR 节点无法启动
+
+```bash
+# 检查串口设备
+ls -l /dev/mic
+
+# 添加串口权限
+sudo usermod -a -G dialout $USER
+```
+
+### 大模型调用失败
+
+```bash
+# 检查 API 密钥配置
+cat config/large_model_interface.yaml
+
+# 检查网络连接
+ping dashscope.aliyuncs.com
+```
+
+### 动作执行异常
+
+```bash
+# 检查导航点配置
+cat config/map_mapping.yaml
+
+# 检查 action 服务是否正常
+ros2 node list | grep action_service
+```

brain/PlannerNode2/largemodel/config/large_model_interface.yaml

@@ -0,0 +1,57 @@
+#大模型接口配置文件Large model interface configuration file
+
+#--------------------------------------------------------国内用户配置选项------------------------------------------------
+#-----------------------------------------------------------必须配置项--------------------------------------------------
+#阿里百炼大模型平台配置                                                          
+tongyi_api_key : "sk-3b05bc8f1bdc40cc87bd05f864890bd8"                           #填写自己的api_key                                                                  
+tongyi_app_id : '6ed9f00173214e7883af7310731a5d7b'                               #填写自己的app_id                                         
+multimodel : "qwen-vl-max-2025-04-08"                                            #执行层大模型，选用多模态模型，测试较稳定模型："qwen-vl-max" 、"qwen-vl-max-2025-04-08"、"qwen-vl-max-2025-04-02"
+
+#----------------------------------------------------------可选配置项（非必须）--------------------------------------------------
+#oline_asr配置
+oline_asr_sample_rate: 16000                                                   #asr音频采样率
+oline_asr_model : 'paraformer-realtime-v2'                                     # 'paraformer-realtime-v2','paraformer-realtime-v1','paraformer-realtime-8k-v2','paraformer-realtime-8k-v1','gummy-realtime-v1','gummy-chat-v1'
+
+#oline_tts配置
+tts_supplier :  "aliyun"                                                        #tts语音合成模型供应商：aliyun/baidu 目前提供两个平台的接口，详见large_model_interface.py接口程序
+
+#通义千问平台语音合成配置
+oline_tts_model : "cosyvoice-v2"                                                #语音模型
+voice_tone : "longwan_v2"
+
+#百度智能云平台语音合成模型
+baidu_API_KEY : 'Ppprf0XqOyQ6uOv2rGg34oR7'                                      #百度平台语音合成API_KEY
+baidu_SECRET_KEY : '1jGl14dF6efRcMOCgiBKwSO8CnlPjBdz'                           #百度平台语音合成SECRET_KEY
+PER : 103                                                                         # 发音人选择, 基础音库：0为度小美，1为度小宇，3为度逍遥，4为度丫丫，精品音库：5为度小娇，103为度米朵，106为度博文，110为度小童，111为度小萌，默认为度小美
+SPD : 5                                                                         # 语速，取值0-15，默认为5中语速
+PIT : 5                                                                         # 音调，取值0-15，默认为5中语调
+VOL : 5                                                                         # 音量，取值0-9，默认为5中音量
+CUID : 'nLSB0tSszSlc2vxM9gQ96FksFuSrQ2cp'                                       # 设备标识，获取地址：https://console.bce.baidu.com/support/?u=aiconsole&timestamp=1749002896844#/api?product=AI&project=%E8%AF%AD%E9%9F%B3%E6%8A%80%E6%9C%AF&parent=%E8%AF%AD%E9%9F%B3%E5%90%88%E6%88%90&api=text2audio&method=post
+
+
+#------------------------------------------------------国外用户配置选项International version configuration options--------------------------------------------
+
+decision_AI_api_key : "app-E7cPag5diUJSo4VmRLQXCJoB"                   #dify决策大模型应用API_KEY
+execution_AI_api_key : "app-2DomQwAo3VdUhjf6qiR9NPmP"                  #dify执行大模型应用API_KEY
+
+#--------------------------------------------------------系统配置（无特殊请勿修改）System configuration (Please do not modify unless otherwise specified)-----------------------------------
+#本地tts模型配置Local tts model configuration
+tts_language : "zh"        #zh：中文 en：英文
+zh_tts_model : "/home/sunrise/opt/app/yahboom_ws/src/largemodel/MODELS/tts/zh/zh_CN-huayan-medium.onnx"              
+zh_tts_json : "/home/sunrise/opt/app/yahboom_ws/src/largemodel/MODELS/tts/zh/zh_CN-huayan-medium.onnx.json"  
+en_tts_model : "/home/sunrise/opt/app/yahboom_ws/src/largemodel/MODELS/tts/en/en_US-libritts-high.onnx"              
+en_tts_json : "/home/sunrise/opt/app/yahboom_ws/src/largemodel/MODELS/tts/en/en_US-libritts-high.onnx.json" 
+#本地asr模型配置Local asr model configuration
+local_asr_model : "/home/sunrise/opt/app/yahboom_ws/src/largemodel/MODELS/asr/SenseVoiceSmall"
+#通义接口地址，无需修改Tongyi interface address, no need to modify
+tongyi_base_url : "https://dashscope.aliyuncs.com/compatible-mode/v1" 
+
+#网络适配器network adapter
+network_adapter : "wlP1p1s0"
+
+
+
+
+
+
+

brain/PlannerNode2/largemodel/config/map_mapping.yaml

@@ -0,0 +1,163 @@
+#根据实际的场景环境，自定义地图中的区域，可以添加任意个区域，注意和大模型的地图映射保持一致即可
+#According to the actual scene environment, customize the areas in the map. You can add any number of areas, just make sure they are consistent with the map mapping of the large model
+#地图映射Map mapping
+
+
+A: 
+  name: '水果店'
+  position:
+    x: 1.633
+    y: 3.490
+    z: 0.0
+  orientation:
+    x: 0.0
+    y: 0.0
+    z: 0.618
+    w: 0.786
+
+B: 
+  name: '五金店'
+  position:
+    x: 2.436
+    y: -0.574 
+    z: 0.0
+  orientation:
+    x: 0.0
+    y: 0.0
+    z: -0.090
+    w: 0.996  
+
+C: 
+  name: '便利店'
+  position:
+    x: 0.024
+    y: -1.820
+    z: 0.0
+  orientation:
+    x: 0.0
+    y: 0.0
+    z: 0.031
+    w: 1.000
+# C: 
+#   name: '不合格区'
+#   position:
+#     x: -1.787
+#     y: -1.433
+#     z: 0.0
+#   orientation:
+#     x: 0.0
+#     y: 0.0
+#     z: -0.765
+#     w: 0.644
+
+# D: 
+#   name: '成品仓库'
+#   position:
+#     x: -1.580
+#     y: 0.178
+#     z: 0.0
+#   orientation:
+#     x: 0.0
+#     y: 0.0
+#     z: 0.997
+#     w: 0.077
+# E: 
+#   name: '装配间'
+#   position:
+#     x: 2.620
+#     y: -0.865
+#     z: 0.0
+#   orientation:
+#     x: 0.0
+#     y: 0.0
+#     z: -0.738
+#     w: 0.675
+
+# F: 
+#   name: '便利店'
+#   position:
+#     x: 2.367471694946289
+#     y: -1.568994164466858
+#     z: 0.0
+#   orientation:
+#     x: 0.0
+#     y: 0.0
+#     z: -0.3106425066197889
+#     w: 0.950526818706855
+
+# H: 
+#   name: '五金店'
+#   position:
+#     x: 2.367471694946289
+#     y: -1.568994164466858
+#     z: 0.0
+#   orientation:
+#     x: 0.0
+#     y: 0.0
+#     z: -0.3106425066197889
+#     w: 0.950526818706855
+
+# I: 
+#   name: '水果店'
+#   position:
+#     x: 2.367471694946289
+#     y: -1.568994164466858
+#     z: 0.0
+#   orientation:
+#     x: 0.0
+#     y: 0.0
+#     z: -0.3106425066197889
+#     w: 0.950526818706855
+
+# J: 
+#   name: '玩具房'
+#   position:
+#     x: 2.367471694946289
+#     y: -1.568994164466858
+#     z: 0.0
+#   orientation:
+#     x: 0.0
+#     y: 0.0
+#     z: -0.3106425066197889
+#     w: 0.950526818706855
+
+# M: 
+#   name: '电脑房'
+#   position:
+#     x: 2.367471694946289
+#     y: -1.568994164466858
+#     z: 0.0
+#   orientation:
+#     x: 0.0
+#     y: 0.0
+#     z: -0.3106425066197889
+#     w: 0.950526818706855
+
+# N: 
+#   name: '厨房'
+#   position:
+#     x: 2.367471694946289
+#     y: -1.568994164466858
+#     z: 0.0
+#   orientation:
+#     x: 0.0
+#     y: 0.0
+#     z: -0.3106425066197889
+#     w: 0.950526818706855
+
+# Z: 
+#   name: '工作间'
+#   position:
+#     x: 2.367471694946289
+#     y: -1.568994164466858
+#     z: 0.0
+#   orientation:
+#     x: 0.0
+#     y: 0.0
+#     z: -0.3106425066197889
+#     w: 0.950526818706855
+
+
+#此处可新增添加区域对应的栅格地图坐标点，注意和上面格式保持一致
+#Here, you can add the raster map coordinate points corresponding to the added area. Please note that the format should be consistent with the above
+

brain/PlannerNode2/largemodel/config/yahboom.yaml

@@ -0,0 +1,63 @@
+
+#按地区实际情况选择国内/国际版本，国内版本注释掉国际版参数，国际版注释掉国内参数
+#Select the domestic or international version based on the actual situation of the region.
+#Comment out the parameters of the international version for the domestic version, and comment out the domestic parameters for the international version
+
+
+#-----------------------------------------------------国内版本参数 Domestic version parameters------------------------------------------------------------
+
+
+asr:                                    #语音节点参数 # Voice node parameters
+  ros__parameters:
+    VAD_MODE: 2                         #vad灵敏度 # VAD sensitivity
+    sample_rate: 16000                  #asr录音音频采样率 # ASR recording audio sample rate
+    frame_duration_ms:  30              #vad帧大小单位ms # VAD frame size in milliseconds
+    use_oline_asr: False                #是否使用在线asr识别 # Whether to use online ASR recognition
+    mic_serial_port: "/dev/ttyUSB0"         #麦克风串口别名 # Microphone serial port alias
+    mic_index: -2                        #麦克风索引 # Microphone index
+    language: 'zh'                      #系统声音语言 # System sound language
+    regional_setting : "China"          #international：国际版  China：国内版 # international: International version, China: Domestic version
+
+action_service:                         #动作服务器节点参数 # Action server node parameters
+  ros__parameters:
+    Speed_topic: "/cmd_vel"             #速度话题 # Speed topic
+    text_chat_mode: True               #文字交互模式 # Text chat mode
+    image_topic: "/camera/color/image_raw" #相机图像话题 # Camera image topic
+    useolinetts: False                   #是否使用在线语音合成 # Whether to use online text-to-speech synthesis
+    language: 'zh'                      #本地语音合成语言 #  local text-to-speech synthesis language  
+    regional_setting : "China"          #international：国际版  China：国内版 # international: International version, China: Domestic version
+model_service:                          #模型服务器节点参数 # Model server node parameters
+  ros__parameters:
+    language: 'zh'                      #大模型接口语言 # Large model API language
+    regional_setting : "China"          #international：国际版  China：国内版 # international: International version, China: Domestic version
+
+
+#-----------------------------------------------------国际版本参数 International version parameters------------------------------------------------------------
+
+# asr:                                    #语音节点参数 # Voice node parameters
+#   ros__parameters:
+#     VAD_MODE: 2                         #vad灵敏度 # VAD sensitivity
+#     sample_rate: 16000                  #asr录音音频采样率 # ASR recording audio sample rate
+#     frame_duration_ms:  30              #vad帧大小单位ms # VAD frame size in milliseconds
+#     use_oline_asr: False                #是否使用在线asr识别 # Whether to use online ASR recognition
+#     mic_serial_port: "/dev/mic"         #麦克风串口别名 # Microphone serial port alias
+#     mic_index: 0                        #麦克风索引 # Microphone index
+#     language: 'en'                      #系统声音语言 # System sound language
+#     regional_setting : "international"          #international：国际版  China：国内版 # international: International version, China: Domestic version
+
+# action_service:                         #动作服务器节点参数 # Action server node parameters
+#   ros__parameters:
+#     Speed_topic: "/cmd_vel"             #速度话题 # Speed topic
+#     text_chat_mode: False               #文字交互模式 # Text chat mode
+#     image_topic: "/camera/color/image_raw" #相机图像话题 # Camera image topic
+#     useolinetts: False                   #是否使用在线语音合成 # Whether to use online text-to-speech synthesis
+#     language: 'en'                      #本地语音合成语言 #  local text-to-speech synthesis language  
+#     regional_setting : "international"          #international：国际版  China：国内版 # international: International version, China: Domestic version
+
+# model_service:                          #模型服务器节点参数 # Model server node parameters
+#   ros__parameters:
+#     language: 'en'                      #大模型接口语言 # Large model API language
+#     regional_setting : "international"          #international：国际版  China：国内版 # international: International version, China: Domestic version
+
+
+

brain/PlannerNode2/largemodel/largemodel/__init__.py

@@ -0,0 +1,7 @@
+import sys
+
+sys.path.append("/home/sunrise/opt/dev/project/aiagent/brain/PlannerNode2/largemodel/utils")
+
+
+
+

brain/PlannerNode2/largemodel/largemodel/action_service.py

@@ -0,0 +1,1398 @@
+import cv2
+import re
+import rclpy
+import subprocess
+from rclpy.action import ActionServer
+from rclpy.node import Node
+from geometry_msgs.msg import Twist
+from rclpy.action import ActionClient
+from geometry_msgs.msg import PoseStamped, PoseWithCovarianceStamped
+import time
+from cv_bridge import CvBridge
+from std_msgs.msg import String
+from sensor_msgs.msg import Image
+from nav2_msgs.action import NavigateToPose
+from std_msgs.msg import Int16MultiArray, Bool
+#from arm_msgs.msg import ArmJoints, ArmJoint
+from interfaces.action import Rot
+import math
+import pygame
+#from arm_interface.msg import CurJoints
+import yaml
+from concurrent.futures import Future
+import psutil
+from ament_index_python.packages import get_package_share_directory
+import os
+from threading import Thread
+from tf2_ros.buffer import Buffer
+from tf2_ros.transform_listener import TransformListener
+from utils import large_model_interface
+import threading
+from rclpy.executors import MultiThreadedExecutor
+import functools
+
+
+class CustomActionServer(Node):
+    def __init__(self):
+        super().__init__("action_service_ndoe")
+        # 初始化参数配置 / Initialize parameter configuration
+        self.init_param_config()
+        # 初始化ROS通信 / Initialize ROS communication
+        self.init_ros_comunication()
+        # 加载地图映射文件 / Load map mapping file
+        self.load_target_points()
+        # 初始化机械臂抓取功能 / Initialize arm grasping function
+        # self.arm_grasp_init()
+        # 初始化语音合成功能 / Initialize text-to-speech synthesis function
+        self.system_sound_init()
+        # 初始化语言设置/Initialize language settings
+        self.init_language()
+        self.get_logger().info("action service started...")
+
+    def init_param_config(self):
+        """
+        初始化参数配置 / Initialize parameter configuration
+        """
+        # 设置夹取启动文件路径 / Set the path for the grasping startup file
+        pkg_share = get_package_share_directory("largemodel")
+        self.map_mapping_config = os.path.join(pkg_share, "config", "map_mapping.yaml")
+        # 声明参数 / Declare parameters
+        self.declare_parameter("Speed_topic", "/cmd_vel")
+        self.declare_parameter("use_double_llm", False)
+        self.declare_parameter("text_chat_mode", False)
+        self.declare_parameter("useolinetts", False)
+        self.declare_parameter("language", "zh")
+        self.declare_parameter("image_topic", "/camera/color/image_raw")
+        self.declare_parameter("regional_setting", "China")
+        # 获取参数值 / Get parameter values
+        self.Speed_topic = (
+            self.get_parameter("Speed_topic").get_parameter_value().string_value
+        )
+        self.use_double_llm = (
+            self.get_parameter("use_double_llm").get_parameter_value().bool_value
+        )
+        self.text_chat_mode = (
+            self.get_parameter("text_chat_mode").get_parameter_value().bool_value
+        )
+        self.useolinetts = (
+            self.get_parameter("useolinetts").get_parameter_value().bool_value
+        )
+        self.language = (
+            self.get_parameter("language").get_parameter_value().string_value
+        )
+        self.image_topic = (
+            self.get_parameter("image_topic").get_parameter_value().string_value
+        )
+        self.regional_setting = (
+            self.get_parameter("regional_setting").get_parameter_value().string_value
+        )
+        self.pkg_path = get_package_share_directory("largemodel")
+        self.image_save_path = os.path.join(
+            self.pkg_path, "resources_file", "image.png"
+        )
+        self.current_pose = PoseWithCovarianceStamped()
+        self.record_pose = PoseStamped()
+        self.combination_mode = False  # 组合模式 / Combination mode
+        self.interrupt_flag = False  # 打断标志 / Interrupt flag
+        self.action_runing = False  # 动作执行状态 / Action execution status
+        self.first_record = True  # 首次记录位置 / First record
+        self.is_recording = False  # 录音状态 / Recording status
+        self.IS_SAVING = False #是否正在保存图像
+        self.joint6 = (
+            140  # 默认机械臂六轴的初始角度 / Default angle of the six-axis arm
+        )
+
+        # 图像处理对象 / Image processing object
+        self.image_msg = None
+        self.bridge = CvBridge()
+        # 创建模型接口客户端 / Create model interface client
+        # 传入 logger 用于调试日志
+        self.model_client = large_model_interface.model_interface(logger=self.get_logger())
+
+    def init_ros_comunication(self):
+        """
+        初始化创建ros通信对象、函数 / Initialize creation of ROS communication objects and functions
+        """
+        # 创建速度话题发布者 / Create velocity topic publisher
+        self.publisher = self.create_publisher(Twist, self.Speed_topic, 10)
+        # 创建导航功能客户端，请求导航动作服务器 / Create navigation function client, request navigation action server
+        self.navclient = ActionClient(self, NavigateToPose, "navigate_to_pose")
+        # 创建动作执行服务器，用于接受动作列表，并执行动作 / Create action execution server to accept action lists and execute actions
+        self._action_server = ActionServer(
+            self, Rot, "action_service", self.execute_callback
+        )
+        # 创建机械臂角度发布者，用于发布arm6_joints，控制机械臂 / Create arm angle publisher to publish arm6_joints and control the arm
+        # self.TargetAngle_pub = self.create_publisher(ArmJoints, "arm6_joints", 100)
+        # 创建关节角度发布者，用于发布arm_joint控制关节 / Create joint angle publisher to publish arm_joint and control joints
+        # self.SingleJoint_pub = self.create_publisher(ArmJoint, "arm_joint", 100)
+        # 创建执行动作状态发布者 / Create action execution status publisher
+        self.actionstatus_pub = self.create_publisher(String, "actionstatus", 3)
+        # 创建发布者，发布 seewhat_handle 话题 / Create publisher to publish seewhat_handle topic
+        self.seewhat_handle_pub = self.create_publisher(String, "seewhat_handle", 1)
+        # 创建物体位置发布者，发布待夹取物体的坐标 / Create object position publisher to publish coordinates of objects to be grasped
+        self.object_position_pub = self.create_publisher(
+            Int16MultiArray, "corner_xy", 1
+        )
+        # 创建JoyCb话题发布者，启动KCF_Tracker_ALM节点测距的功能 / Create JoyCb topic publisher to enable distance measurement functionality of KCF_Tracker_ALM node
+        self.joy_pub = self.create_publisher(Bool, "JoyState", 1)
+        # 创建当前机械臂关节角发布者 / Create current arm joint angle publisher
+        # self.pub_cur_joints = self.create_publisher(CurJoints, "Curjoints", 1)
+        # 创建KCF_Tracker_ALM重置发布者 / Create KCF_Tracker_ALM reset publisher
+        self.reset_pub = self.create_publisher(Bool, "reset_flag", 1)
+        # 创建机械臂抓取完成话题订阅者 / Create subscriber for arm grasping completion topic
+        self.largemodel_arm_done_sub = self.create_subscription(
+            String, "/largemodel_arm_done", self.largemodel_arm_done_callback, 1
+        )
+        # 创建发布者，发布 tts_topic 主题 / Create publisher to publish tts_topic topic
+        self.TTS_publisher = self.create_publisher(String, "tts_topic", 5)
+        # 创建tf监听者，监听坐标变换 / Create tf listener to monitor coordinate transformations
+        self.tf_buffer = Buffer()
+        self.tf_listener = TransformListener(self.tf_buffer, self)
+        # 创建打断状态发布者 / Create interrupt status publisher
+        self.interrupt_flag_pub = self.create_publisher(Bool, "interrupt_flag", 1)
+        # wakeup话题订阅者 / Subscribe to wakeup topic
+        self.wakeup_sub = self.create_subscription(
+            Bool, "wakeup", self.wakeup_callback, 5
+        )
+        # 图像话题订阅者 / Image topic subscriber
+        self.subscription = self.create_subscription(
+            Image, self.image_topic, self.image_callback, 2
+        )
+        # 录音状态话题订阅者 / Record status topic subscriber
+        self.record_status_sub = self.create_subscription(
+            Bool, "record_status", self.record_status_callback, 5
+        )
+
+    def system_sound_init(
+        self,
+    ):  # 初始化系统声音相关的功能 / Initialize system sound-related functions
+        pkg_path = get_package_share_directory("largemodel")
+
+        if self.regional_setting == "China":  # 如果是中国地区 /if it is in China
+            if self.useolinetts:
+                model_type = "oline"
+                self.tts_out_path = os.path.join(
+                    pkg_path, "resources_file", "tts_output.mp3"
+                )
+            else:
+                model_type = "local"
+                self.tts_out_path = os.path.join(
+                    pkg_path, "resources_file", "tts_output.wav"
+                )
+
+        elif (
+            self.regional_setting == "international"
+        ):  # 如果是国际地区 /if it is international
+
+            model_type = "XUNFEI_FOR_INTERNATIONAL"
+            self.tts_out_path = os.path.join(
+                pkg_path, "resources_file", "XUNFEI_TTS.mp3"
+            )
+        else:
+            while True:
+                self.get_logger().info()(
+                    'Please check the regional_setting parameter in yahboom.yaml file, it should be either "China" or "international".'
+                )
+                time.sleep(1)
+
+        self.model_client.tts_model_init(
+            model_type, self.language
+        )  # 初始化语音合成模型 / Initialize TTS model
+        # 初始化音频播放器 / Initialize audio player
+        pygame.mixer.init()
+        self.stop_event = threading.Event()
+
+    def init_language(self):
+        language_list = ["zh", "en"]
+        if self.language not in language_list:
+            while True:
+                self.get_logger().info(
+                    "The language setting is incorrect. Please check the action_service'' language setting in the yahboom.yaml file"
+                )
+                self.get_logger().info(self.language)
+                time.sleep(1)
+
+        self.feedback_largemoel_dict = {
+            "zh": {  # 中文 / Chinese
+                "navigation_1": "机器人反馈:导航目标{point_name}被拒绝",
+                "navigation_2": "机器人反馈:执行navigation({point_name})完成",
+                "navigation_3": "机器人反馈:执行navigation({point_name})失败，目标点不存在",
+                "navigation_4": "机器人反馈:执行navigation({point_name})失败",
+                "get_current_pose_success": "机器人反馈:get_current_pose()成功",
+                "arm_up_done": "机器人反馈:执行arm_up()完成",
+                "arm_down_done": "机器人反馈:执行arm_down()完成",
+                "drift_done": "机器人反馈:执行drift()完成",
+                "wait_done": "机器人反馈:执行wait({duration})完成",
+                "arm_shake_done": "机器人反馈:执行arm_shake()完成",
+                "arm_nod_done": "机器人反馈:执行arm_nod()完成",
+                "arm_applaud_done": "机器人反馈:执行arm_applaud()完成",
+                "grasp_obj_done": "机器人反馈:执行grasp_obj({x1},{y1},{x2},{y2})完成",
+                "grasp_obj_failed": "机器人反馈:执行grasp_obj({x1},{y1},{x2},{y2})失败",
+                "putdown_done": "机器人反馈:执行putdown()完成",
+                "set_cmdvel_done": "机器人反馈:执行set_cmdvel({linear_x},{linear_y},{angular_z},{duration})完成",
+                "move_left_done": "机器人反馈:执行move_left({angle},{angular_speed})完成",
+                "move_right_done": "机器人反馈:执行move_right({angle},{angular_speed})完成",
+                "turn_left_done": "机器人反馈:执行turn_left()完成",
+                "turn_right_done": "机器人反馈:执行turn_right()完成",
+                "dance_done": "机器人反馈:执行dance()完成",
+                "apriltag_sort_done": "机器人反馈:执行apriltag_sort({target_id})完成",
+                "apriltag_sort_failed": "机器人反馈:执行apriltag_sort({target_id})失败",
+                "apriltag_follow_2D_done": "机器人反馈:执行apriltag_follow_2D({target_id})完成",
+                "apriltag_follow_2D_failed": "机器人反馈:执行apriltag_follow_2D({target_id})失败",
+                "apriltag_remove_higher_done": "机器人反馈:执行apriltag_remove_higher({target_high})完成",
+                "apriltag_remove_higher_failed": "机器人反馈:执行apriltag_remove_higher({target_high})失败",
+                "color_follow_2D_done": "机器人反馈:执行color_follow_2D({color})完成",
+                "color_follow_2D_failed": "机器人反馈:执行color_follow_2D({color})失败",
+                "color_remove_higher_done": "机器人反馈:执行color_remove_higher({color},{target_high})完成",
+                "color_remove_higher_failed": "机器人反馈:执行color_remove_higher({color},{target_high})失败",
+                "follw_line_clear_done": "机器人反馈:执行follw_line_clear()完成",
+                "response_done": "机器人反馈：回复用户完成",
+                "failure_execute_action_function_not_exists": "机器人反馈:动作函数不存在，无法执行",
+                "finish": "finish",
+                "multiple_done": "机器人反馈：执行{actions}完成",
+                "putdown_failed": "机器人反馈:执行putdown()失败,输入参数错误",
+            },
+            "en": {  # 英文 / English
+                "navigation_1": "Robot feedback: Navigation target {point_name} rejected",
+                "navigation_2": "Robot feedback: Execute navigation({point_name}) completed",
+                "navigation_3": "Robot feedback: Execute navigation({point_name}) failed, target does not exist",
+                "navigation_4": "Robot feedback: Execute navigation({point_name}) failed",
+                "get_current_pose_success": "Robot feedback: get_current_pose() succeeded",
+                "arm_up_done": "Robot feedback: Execute arm_up() completed",
+                "arm_down_done": "Robot feedback: Execute arm_down() completed",
+                "drift_done": "Robot feedback: Execute drift() completed",
+                "wait_done": "Robot feedback: Execute wait({duration}) completed",
+                "arm_shake_done": "Robot feedback: Execute arm_shake() completed",
+                "arm_nod_done": "Robot feedback: Execute arm_nod() completed",
+                "arm_applaud_done": "Robot feedback: Execute arm_applaud() completed",
+                "grasp_obj_done": "Robot feedback: Execute grasp_obj({x1},{y1},{x2},{y2}) completed",
+                "grasp_obj_failed": "Robot feedback: Execute grasp_obj({x1},{y1},{x2},{y2}) failed",
+                "putdown_done": "Robot feedback: Execute putdown() completed",
+                "set_cmdvel_done": "Robot feedback: Execute set_cmdvel({linear_x},{linear_y},{angular_z},{duration}) completed",
+                "move_left_done": "Robot feedback: Execute move_left({angle},{angular_speed}) completed",
+                "move_right_done": "Robot feedback: Execute move_right({angle},{angular_speed}) completed",
+                "turn_left_done": "Robot feedback: Execute turn_left() completed",
+                "turn_right_done": "Robot feedback: Execute turn_right() completed",
+                "dance_done": "Robot feedback: Execute dance() completed",
+                "apriltag_sort_done": "Robot feedback: Execute apriltag_sort({target_id}) completed",
+                "apriltag_sort_failed": "Robot feedback: Execute apriltag_sort({target_id}) failed",
+                "apriltag_follow_2D_done": "Robot feedback: Execute apriltag_follow_2D({target_id}) completed",
+                "apriltag_follow_2D_failed": "Robot feedback: Execute apriltag_follow_2D({target_id}) failed",
+                "apriltag_remove_higher_done": "Robot feedback: Execute apriltag_remove_higher({target_high}) completed",
+                "apriltag_remove_higher_failed": "Robot feedback: Execute apriltag_remove_higher({target_high}) failed",
+                "color_follow_2D_done": "Robot feedback: Execute color_follow_2D({color}) completed",
+                "color_follow_2D_failed": "Robot feedback: Execute color_follow_2D({color}) failed",
+                "color_remove_higher_done": "Robot feedback: Execute color_remove_higher({color},{target_high}) completed",
+                "color_remove_higher_failed": "Robot feedback: Execute color_remove_higher({color},{target_high}) failed",
+                "follw_line_clear_done": "Robot feedback: Execute follw_line_clear() completed",
+                "response_done": "Robot feedback: Reply to user completed",
+                "failure_execute_action_function_not_exists": "Robot feedback: Execute action function not exists",
+                "finish": "finish",
+                "multiple_done": "Robot feedback: Execution {actions} completed",
+            },
+        }
+
+    def load_target_points(self):
+        """
+        加载地图映射文件 /Load map mapping file
+        """
+        with open(self.map_mapping_config, "r") as file:
+            target_points = yaml.safe_load(file)
+        self.navpose_dict = {}
+        for name, data in target_points.items():
+            pose = PoseStamped()
+            pose.header.frame_id = "map"
+            pose.pose.position.x = data["position"]["x"]
+            pose.pose.position.y = data["position"]["y"]
+            pose.pose.position.z = data["position"]["z"]
+            pose.pose.orientation.x = data["orientation"]["x"]
+            pose.pose.orientation.y = data["orientation"]["y"]
+            pose.pose.orientation.z = data["orientation"]["z"]
+            pose.pose.orientation.w = data["orientation"]["w"]
+            self.navpose_dict[name] = pose
+
+    # def arm_grasp_init(self):
+    #     """
+    #     初始化机械臂抓取功能 /initialize the grasping function of the robotic arm
+    #     """
+    #     # 机械臂状态变量/Robotic arm status variable
+    #     self.up_joints = [90, 90, 90, 90, 90, 90]
+    #     self.down_joints = [90, 0, 90, 90, 90, 90]
+    #     self.detect_joints = [90, 120, 0, 0, 90, 90]
+    #     self.init_joints = [
+    #         90,
+    #         130,
+    #         0,
+    #         5,
+    #         90,
+    #         0,
+    #     ]
+    #     # 机械臂初始姿态/robot arm initial pose
+    #     self.putsown_joints = [
+    #         90,
+    #         10,
+    #         50,
+    #         50,
+    #         90,
+    #         135,
+    #     ]  # 机械臂放下姿态/robot arm putdown pose
+    #     while not self.TargetAngle_pub.get_subscription_count():
+    #         self.pubSix_Arm(self.init_joints)
+    #         time.sleep(0.1)
+    #     self.pubSix_Arm(self.init_joints)
+    #     self.apriltag_sort_future = Future()
+    #     self.apriltag_follow_2D_future = Future()
+    #     self.apriltag_remove_higher_future = Future()
+    #     self.color_follow_2D_future = Future()
+    #     self.color_sort_future = Future()
+    #     self.color_remove_higher_future = Future()
+    #     self.grasp_obj_future = Future()
+    #     self.follw_line_clear_future = Future()
+
+    def record_status_callback(self, msg):
+        # self.get_logger().info(f"record_status_callback:{msg.data}")
+        if msg.data:
+            self.is_recording = True
+        else:
+            self.is_recording = False
+
+    def largemodel_arm_done_callback(self, msg):
+        """
+        机械臂抓取完成话题回调函数/robot arm done callback function
+        用于接受机械臂抓取完成话题，并设置Future对象完成 /used to receive the topic of the robotic arm grasping completion, and set the Future object to complete
+        """
+        if msg.data in ["apriltag_sort_done", "apriltag_sort_failed"]:
+            if not self.apriltag_sort_future.done():
+                self.apriltag_sort_future.set_result(msg)
+        elif msg.data == "apriltag_follow_2D_done":
+            if not self.apriltag_follow_2D_future.done():
+                self.apriltag_follow_2D_future.set_result(msg)
+        elif msg.data in [
+            "apriltag_remove_higher_done",
+            "apriltag_remove_higher_failed",
+        ]:
+            self.get_logger().info(f"msg.data:{msg.data}")
+            if not self.apriltag_remove_higher_future.done():
+                self.apriltag_remove_higher_future.set_result(msg)
+        elif msg.data == "color_follow_2D_done":
+            if not self.color_follow_2D_future.done():
+                self.color_follow_2D_future.set_result(msg)
+        elif msg.data == "color_sort_done":
+            if not self.color_sort_future.done():
+                self.color_sort_future.set_result(msg)
+        elif msg.data == "grasp_obj_done":
+            if not self.grasp_obj_future.done():
+                self.grasp_obj_future.set_result(msg)
+        elif msg.data == "color_remove_higher_done":
+            if not self.color_remove_higher_future.done():
+                self.color_remove_higher_future.set_result(msg)
+        elif msg.data == "follw_line_clear_future_done":
+            if not self.follw_line_clear_future.done():
+                self.follw_line_clear_future.set_result(msg)
+
+    def wakeup_callback(self, msg):
+        """
+        唤醒打断回调函数/Wake-up interrupt callback function
+        用于接受唤醒信号，判断是否需要打断当前的动作、语音 /used to receive the wake-up signal, determine whether to interrupt the current action, voice
+        """
+
+        if msg.data:
+            if (
+                pygame.mixer.music.get_busy()  # 如果音乐正在播放/If the music is playing
+            ):
+                self.stop_event.set()  # 停止正在播放的音乐/Stop the music currently playing
+            if (
+                self.action_runing  # 如果当前有动作正在执行/If there is an action currently being
+            ):
+                self.interrupt_flag = True  # 置位中断标志位/Set the interruption flag
+                self.stop()
+        # self.check_all_process()
+
+    def get_current_pose(self):
+        """
+        获取当前在全局地图坐标系下的位置 /Get the current position in the global map coordinate system
+        """
+        # 获取当前目标点坐标
+        transform = self.tf_buffer.lookup_transform(
+            "map", "base_footprint", rclpy.time.Time()
+        )
+        # 提取位置和姿态
+        pose = PoseStamped()
+        pose.header.frame_id = "map"
+        pose.pose.position.x = transform.transform.translation.x
+        pose.pose.position.y = transform.transform.translation.y
+        pose.pose.position.z = 0.0
+        pose.pose.orientation = transform.transform.rotation
+        self.navpose_dict["zero"] = pose
+        # 打印记录的坐标
+        position = pose.pose.position
+        orientation = pose.pose.orientation
+        self.get_logger().info(
+            f"Recorded Pose - Position: x={position.x}, y={position.y},\
+                                z={position.z},Orientation: x={orientation.x}, y={orientation.y}, z={orientation.z}, w={orientation.w}"
+        )
+        if not self.interrupt_flag:
+            self.action_status_pub("get_current_pose_success")
+
+    def action_status_pub(self, key, **kwargs):
+        """
+        多语言版本的动作结果发布方法
+        :param key: 文本标识
+        :param**kwargs: 占位符参数
+        """
+        text_template = self.feedback_largemoel_dict[self.language].get(key)
+
+        try:
+            message = text_template.format(**kwargs)
+        except KeyError as e:
+            self.get_logger().error(f"Translation placeholder error: {e} (key: {key})")
+            message = f"[Translation failed: {key}]"
+
+        # 发布消息
+        self.actionstatus_pub.publish(String(data=message))
+        self.get_logger().info(f"Published message: {message}")
+
+    def navigation(self, point_name):
+        """
+        从navpose_dict字典中获取目标点坐标.并导航到目标点
+        """
+        self.navigation_finish_flag = False
+        self.goal_handle = None
+        self.result = None
+        point_name = point_name.strip("'\"")
+        if point_name not in self.navpose_dict:
+            self.get_logger().error(
+                f"Target point '{point_name}' does not exist in the navigation dictionary."
+            )
+            self.action_status_pub(
+                "navigation_3", point_name=point_name
+            )  # 目标点地图映射中不存在
+            return
+
+        if self.first_record:
+            # 出发前记录当前在全局地图中的坐标(只有在每个任务周期的第一次执行时才会记录)/ before starting a new task, record the current pose in the global map
+            transform = self.tf_buffer.lookup_transform(
+                "map", "base_footprint", rclpy.time.Time()
+            )
+            pose = PoseStamped()
+            pose.header.frame_id = "map"
+            pose.pose.position.x = transform.transform.translation.x
+            pose.pose.position.y = transform.transform.translation.y
+            pose.pose.position.z = 0.0
+            pose.pose.orientation = transform.transform.rotation
+            self.navpose_dict["zero"] = pose
+            self.first_record = False
+
+        # 获取目标点坐标 /get_target_pose
+        target_pose = self.navpose_dict.get(point_name)
+        goal_msg = NavigateToPose.Goal()
+        goal_msg.pose = target_pose
+        send_goal_future = self.navclient.send_goal_async(goal_msg)
+
+        def goal_response_callback(future):
+            self.goal_handle = future.result()
+            if not self.goal_handle or not self.goal_handle.accepted:
+                self.get_logger().error("Goal was rejected!")
+                self.action_status_pub("navigation_1", point_name=point_name)
+                return
+
+            get_result_future = self.goal_handle.get_result_async()
+
+            def result_callback(future_result):
+                self.result = future_result.result()
+                self.navigation_finish_flag = True
+                if self.result.status == 4:
+                    self.action_status_pub(
+                        "navigation_2", point_name=point_name
+                    )  # 执行导航成功 /execute navigation success
+
+                elif self.result.status == 5:
+                    self.get_logger().info("Cancel navigation")
+                else:
+                    self.get_logger().info(
+                        f"Navigation failed with status: {self.result.status}"
+                    )
+                    self.action_status_pub(
+                        "navigation_4", point_name=point_name
+                    )  # 执行导航失败 /execute_navigation_failed
+
+            get_result_future.add_done_callback(result_callback)
+
+        send_goal_future.add_done_callback(goal_response_callback)
+
+        while not self.navigation_finish_flag:
+            if self.interrupt_flag and self.goal_handle is not None:
+                self.navclient._cancel_goal(self.goal_handle)
+                break
+            time.sleep(0.1)
+        self.stop()
+
+    # def pubSix_Arm(self, joints, id=6, angle=180.0, runtime=2000):
+    #     while not self.TargetAngle_pub.get_subscription_count():
+    #         self.get_logger().info("Waiting for arm_subscriber...")
+    #         time.sleep(0.1)
+    #
+    #     arm_joint = ArmJoints()
+    #     arm_joint.joint1 = joints[0]
+    #     arm_joint.joint2 = joints[1]
+    #     arm_joint.joint3 = joints[2]
+    #     arm_joint.joint4 = joints[3]
+    #     arm_joint.joint5 = joints[4]
+    #     arm_joint.joint6 = joints[5]
+    #     arm_joint.time = runtime
+    #     self.TargetAngle_pub.publish(arm_joint)
+
+    # def pubSingle_Arm(self, joint_id=6, joint_angle=180.0, runtime=800):
+    #     arm_joint = ArmJoint()
+    #     arm_joint.joint = int(joint_angle)
+    #     arm_joint.id = int(joint_id)
+    #     arm_joint.time = runtime
+    #     self.SingleJoint_pub.publish(arm_joint)
+
+    # def pubCurrentJoints(self):
+    #     cur_joints = CurJoints()
+    #     cur_joints.joints = self.init_joints
+    #     self.pub_cur_joints.publish(cur_joints)
+
+    # def arm_up(self):  # 机械臂向上
+    #     self.done = False
+    #     self.pubSix_Arm(self.up_joints)
+    #     time.sleep(1.0)
+    #     self.done = True
+    #     if not self.combination_mode and not self.interrupt_flag:
+    #         self.action_status_pub("arm_up_done")
+
+    # def arm_down(self):  # 机械臂向下
+    #     self.done = False
+    #     self.pubSix_Arm(self.down_joints)
+    #     time.sleep(1.0)
+    #     self.done = True
+    #     if not self.combination_mode and not self.interrupt_flag:
+    #         self.action_status_pub("arm_down_done")
+
+    # def arm_dance(self):  # 机械臂跳舞
+    #     dance_moves = [
+    #         [90, 90, 90, 90, 90, 90],
+    #         [90, 60, 120, 60, 90, 90],
+    #         [90, 45, 135, 45, 90, 90],
+    #         [90, 60, 120, 60, 90, 90],
+    #         [90, 90, 90, 90, 90, 90],
+    #         [90, 100, 80, 80, 90, 90],
+    #         [90, 120, 60, 60, 90, 90],
+    #         [90, 135, 45, 45, 90, 90],
+    #         [90, 90, 90, 90, 90, 90],
+    #         [90, 90, 90, 20, 90, 150],
+    #         [90, 90, 90, 90, 90, 90],
+    #         [90, 90, 90, 20, 90, 150],
+    #     ]
+    #     for joints in dance_moves:
+    #         if self.interrupt_flag:
+    #             break
+    #         self.pubSix_Arm(joints)
+    #         time.sleep(1.0)
+    #     self.pubSix_Arm(self.init_joints)
+
+    def drift(self):
+        """
+        漂移动作
+        """
+        twist = Twist()
+        twist.linear.x = 0.0
+        twist.linear.y = 0.5
+        twist.angular.z = 1.0
+        self._execute_action(twist, durationtime=4.0)
+        if not self.combination_mode and not self.interrupt_flag:
+            self.action_status_pub("drift_done")
+
+    def wait(self, duration):
+        duration = float(duration)
+        time.sleep(duration)
+        if not self.combination_mode and not self.interrupt_flag:
+            self.action_status_pub("wait_done", duration=duration)
+
+    # def arm_shake(self):  # 机械臂摇头
+    #     for i in range(3):
+    #         if self.interrupt_flag:
+    #             break
+    #         tar_arm_joint = [140, 130, 0, 5, 90, 0]
+    #         self.pubSix_Arm(tar_arm_joint)
+    #         time.sleep(1.0)
+    #         tar_arm_joint = [40, 130, 0, 5, 90, 0]
+    #         self.pubSix_Arm(tar_arm_joint)
+    #         time.sleep(1.0)
+    #
+    #     self.pubSix_Arm(self.init_joints)
+    #     if not self.combination_mode and not self.interrupt_flag:
+    #         self.action_status_pub("arm_shake_done")
+
+    # def arm_nod(self):  # 机械臂点头
+    #     for i in range(3):
+    #         if self.interrupt_flag:
+    #             break
+    #         tar_arm_joint = [90, 130, 0, 95, 90, 0]
+    #         self.pubSix_Arm(tar_arm_joint)
+    #         time.sleep(1.0)
+    #         self.pubSix_Arm(self.init_joints)
+    #         time.sleep(1.0)
+    #     self.pubSix_Arm(self.init_joints)
+    #     if not self.combination_mode and not self.interrupt_flag:
+    #         self.action_status_pub("arm_nod_done")
+
+    # def arm_applaud(self):  # 机械臂鼓掌
+    #     for i in range(3):
+    #         if self.interrupt_flag:
+    #             break
+    #         tar_arm_joint = [90, 145, 0, 71, 90, 31]
+    #         self.pubSix_Arm(tar_arm_joint)
+    #         time.sleep(1.0)
+    #         tar_arm_joint = [90, 145, 0, 71, 90, 168]
+    #         self.pubSix_Arm(tar_arm_joint)
+    #         time.sleep(1.0)
+    #     self.pubSix_Arm(self.init_joints)
+    #     if not self.combination_mode and not self.interrupt_flag:
+    #         self.action_status_pub("arm_applaud_done")
+
+
+    # def check_track(self):
+    #     """
+    #     检查相关进程是否存活
+    #     """
+    #     pass
+
+    # def track(self, x1, y1, x2, y2):
+    #     """
+    #     追踪物体
+    #     x1,y1,x2,y2: 物体外边框坐标
+    #     """
+    #     self.check_track()
+    #     cmd1 = "ros2 run largemodel_arm KCF_track"
+    #     cmd2 = "ros2 run M3Pro_KCF ALM_KCF_Tracker_Node"
+    #
+    #     subprocess.Popen(
+    #         [
+    #             "gnome-terminal",
+    #             "--title=KCF_track",
+    #             "--",
+    #             "bash",
+    #             "-c",
+    #             f"{cmd2}; exec bash",
+    #         ]
+    #     )
+    #     subprocess.Popen(
+    #         [
+    #             "gnome-terminal",
+    #             "--title=ALM_KCF_Tracker_Node",
+    #             "--",
+    #             "bash",
+    #             "-c",
+    #             f"{cmd1}; exec bash",
+    #         ]
+    #     )
+    #     time.sleep(5.0) #等待ALM_KCF_Tracker_Node启动完成
+    #
+    #     x1 = int(x1)
+    #     y1 = int(y1)
+    #     x2 = int(x2)
+    #     y2 = int(y2)
+    #     self.object_position_pub.publish(Int16MultiArray(data=[x1, y1, x2, y2]))
+    #     while True:
+    #         if self.interrupt_flag:
+    #             self.check_track()
+    #             self.pubSix_Arm(self.init_joints)
+    #             return
+    #         time.sleep(0.1)
+    #     self.pubSix_Arm(self.init_joints)
+
+    # def check_close_grasp_obj(self):
+    #     """
+    #     检查相关进程是否存活
+    #     """
+    #     pass
+
+
+    # def grasp_obj(self, x1, y1, x2, y2):
+    #     """
+    #     抓取物体
+    #     x1,y1,x2,y2: 物体外边框坐标
+    #     """
+    #     self.check_close_grasp_obj()
+    #     cmd1 = "ros2 run largemodel_arm grasp_desktop"
+    #     cmd2 = "ros2 run largemodel_arm KCF_follow"
+    #     cmd3 = "ros2 run M3Pro_KCF ALM_KCF_Tracker_Node"
+    #     # cmd3 = "ros2 run --prefix 'gdb -ex run --args' M3Pro_KCF ALM_KCF_Tracker_Node"
+    #     subprocess.Popen(
+    #         [
+    #             "gnome-terminal",
+    #             "--title=ALM_KCF_Tracker",
+    #             "--",
+    #             "bash",
+    #             "-c",
+    #             f"{cmd3}; exec bash",
+    #         ]
+    #     )
+    #     time.sleep(5.0) #等待ALM_KCF_Tracker_Node启动完成
+    #     subprocess.Popen(
+    #         [
+    #             "gnome-terminal",
+    #             "--title=grasp_desktop",
+    #             "--",
+    #             "bash",
+    #             "-c",
+    #             f"{cmd1}; exec bash",
+    #         ]
+    #     )
+    #     subprocess.Popen(
+    #         [
+    #             "gnome-terminal",
+    #             "--title=KCF_follow",
+    #             "--",
+    #             "bash",
+    #             "-c",
+    #             f"{cmd2}; exec bash",
+    #         ]
+    #     )
+    #     time.sleep(1.0)
+    #     x1 = int(x1)
+    #     y1 = int(y1)
+    #     x2 = int(x2)
+    #     y2 = int(y2)
+    #     self.object_position_pub.publish(Int16MultiArray(data=[x1, y1, x2, y2]))
+    #
+    #     while not self.grasp_obj_future.done():
+    #         if self.interrupt_flag:
+    #             self.check_close_grasp_obj()
+    #             self.pubSix_Arm(self.init_joints)  # 机械臂收回
+    #             self.stop()
+    #             return
+    #         time.sleep(0.1)
+    #
+    #     result = self.grasp_obj_future.result()
+    #     if not self.interrupt_flag:
+    #         if result.data == "grasp_obj_done":
+    #             self.action_status_pub("grasp_obj_done", x1=x1, y1=y1, x2=x2, y2=y2)
+    #         else:
+    #             self.action_status_pub("grasp_obj_failed", x1=x1, y1=y1, x2=x2, y2=y2)
+    #
+    #     self.check_close_grasp_obj()
+    #     self.grasp_obj_future = Future()  # 复位Future对象
+    #     if self.interrupt_flag:
+    #         time.sleep(0.5)
+    #         self.pubSix_Arm(self.init_joints)  # 机械臂收回
+
+    # def putdown(self):
+    #     self.pubSix_Arm(self.putsown_joints)  # 机械臂下放
+    #     time.sleep(4)
+    #     self.pubSingle_Arm(6, 30, 1000)  # 机械臂打开夹抓，放下物品
+    #     time.sleep(3)
+    #     self.pubSix_Arm(self.init_joints)  # 机械臂收回
+    #     if not self.interrupt_flag:
+    #         self.action_status_pub("putdown_done")
+
+
+    def seewhat(self):
+        self.save_single_image()
+        msg = String(data="seewhat")
+        self.seewhat_handle_pub.publish(
+            msg
+        )  # 归一化，发布seewhat话题，由model_service调用大模型
+
+    def set_cmdvel(self, linear_x, linear_y, angular_z, duration):  # 发布cmd_vel
+        # 将参数从字符串转换为浮点数
+        linear_x = float(linear_x)
+        linear_y = float(linear_y)
+        angular_z = float(angular_z)
+        duration = float(duration)
+        twist = Twist()
+        twist.linear.x = linear_x
+        twist.linear.y = linear_y
+        twist.angular.z = angular_z
+        self._execute_action(twist, durationtime=duration)
+        if not self.combination_mode and not self.interrupt_flag:
+            self.action_status_pub(
+                "set_cmdvel_done",
+                linear_x=linear_x,
+                linear_y=linear_y,
+                angular_z=angular_z,
+                duration=duration,
+            )
+
+    def move_left(self, angle, angular_speed):  # 左转x度
+        angle = float(angle)
+        angular_speed = float(angular_speed)
+        angle_rad = math.radians(angle)  # 将角度转换为弧度
+        duration = abs(angle_rad / angular_speed)
+        angular_speed = abs(angular_speed)
+        twist = Twist()
+        twist.linear.x = 0.0
+        twist.angular.z = angular_speed
+        self._execute_action(twist, 1, duration)
+        self.stop()
+        if not self.combination_mode and not self.interrupt_flag:
+            self.action_status_pub(
+                "move_left_done",
+                angle=angle,
+                angular_speed=angular_speed,
+            )
+
+    def move_right(self, angle, angular_speed):  # 右转x度
+        angle = float(angle)
+        angular_speed = float(angular_speed)
+        angle_rad = math.radians(angle)  # 将角度转换为弧度
+        duration = abs(angle_rad / angular_speed)
+        angular_speed = -abs(angular_speed)
+        twist = Twist()
+        twist.linear.x = 0.0
+        twist.angular.z = angular_speed
+        self._execute_action(twist, 1, duration)
+        self.stop()
+        if not self.combination_mode and not self.interrupt_flag:
+            self.action_status_pub(
+                "move_right_done",
+                angle=angle,
+                angular_speed=angular_speed,
+            )
+
+    def turn_left(self):  # 左转弯
+        twist = Twist()
+        twist.linear.x = 0.4
+        twist.angular.z = 1.0
+        self._execute_action(twist)
+        self.stop()
+        if not self.combination_mode and not self.interrupt_flag:
+            self.action_status_pub("turn_left_done")
+
+    def turn_right(self):  # 右转弯
+        twist = Twist()
+        twist.linear.x = 0.4
+        twist.angular.z = -1.0
+        self._execute_action(twist)
+        self.stop()
+        if not self.combination_mode and not self.interrupt_flag:
+            self.action_status_pub("turn_right_done")
+
+    def dance(self):  # 跳舞
+        # thread = Thread(target=self.arm_dance)
+        # thread.start()
+        actions = [
+            {"linear_x": 0.6, "linear_y": 0.0, "angular_z": 0.0, "durationtime": 1.5},
+            {"linear_x": -0.4, "linear_y": 0.0, "angular_z": 0.0, "durationtime": 1.0},
+            {"linear_x": 0.0, "linear_y": 0.3, "angular_z": 0.0, "durationtime": 1.0},
+            {"linear_x": 0.0, "linear_y": -0.3, "angular_z": 0.0, "durationtime": 1.0},
+            {"linear_x": 0.0, "linear_y": 0.0, "angular_z": 0.6, "durationtime": 3.0},
+            {"linear_x": 0.0, "linear_y": 0.0, "angular_z": -0.6, "durationtime": 3.0},
+        ]
+
+        for action in actions:
+            if self.interrupt_flag:
+                break
+            twist = Twist()
+            twist.linear.x = action["linear_x"]
+            twist.linear.y = action["linear_y"]
+            twist.angular.z = action["angular_z"]
+            self._execute_action(twist, durationtime=action["durationtime"])
+
+        # thread.join(timeout=5.0)
+        self.stop()
+        # self.pubSix_Arm(self.init_joints)  # 机械臂收回
+        if not self.combination_mode and not self.interrupt_flag:
+            self.action_status_pub("dance_done")
+
+    def stop(self):  # 停止
+        twist = Twist()
+        twist.linear.x = 0.0
+        twist.linear.y = 0.0
+        twist.angular.z = 0.0
+        self.publisher.publish(twist)
+
+    def _execute_action(self, twist, num=1, durationtime=3.0):
+        for _ in range(num):
+            start_time = time.time()
+            while (time.time() - start_time) < durationtime:
+                if self.interrupt_flag:
+                    self.stop()
+                    return
+                self.publisher.publish(twist)
+                time.sleep(0.1)
+
+    # def check_apriltag_sort(self):
+    #     """
+    #     检查相关进程是否存活
+    #     """
+    #     pass
+
+    # def apriltag_sort(self, target_id):  # 夹取机器码
+    #     self.check_apriltag_sort()
+    #     target_idf = float(target_id)
+    #     cmd1 = "ros2 run largemodel_arm grasp_desktop_apritag"
+    #     cmd2 = f"ros2 run largemodel_arm apriltag_sort --ros-args -p target_id:={target_idf:.1f}"
+    #     subprocess.Popen(
+    #         [
+    #             "gnome-terminal",
+    #             "--title=grasp_desktop_apritag",
+    #             "--",
+    #             "bash",
+    #             "-c",
+    #             f"{cmd1}; exec bash",
+    #         ]
+    #     )
+    #     subprocess.Popen(
+    #         [
+    #             "gnome-terminal",
+    #             "--title=apriltag_sort",
+    #             "--",
+    #             "bash",
+    #             "-c",
+    #             f"{cmd2}; exec bash",
+    #         ]
+    #     )
+    #
+    #     while not self.apriltag_sort_future.done():
+    #         if self.interrupt_flag:
+    #             self.check_apriltag_sort()
+    #             self.stop()
+    #             self.pubSix_Arm(self.init_joints)
+    #             return
+    #         time.sleep(0.1)
+    #
+    #     result = self.apriltag_sort_future.result()
+    #     if not self.interrupt_flag:
+    #         if result.data == "apriltag_sort_done":
+    #             self.action_status_pub("apriltag_sort_done", target_id=target_id)
+    #         elif result.data == "apriltag_sort_failed":
+    #             self.action_status_pub("apriltag_sort_failed", target_id=target_id)
+    #
+    #     self.check_apriltag_sort()
+    #     self.apriltag_sort_future = Future()  # 复位Future对象
+
+    # def check_apriltag_remove_higher(self):
+    #     """
+    #     检查相关进程是否存活
+    #     """
+    #     pass
+
+    # def apriltag_remove_higher(self, target_high):  # 移除指定高度的机器码
+    #     self.check_apriltag_remove_higher()
+    #     target_highf = float(target_high) / 100
+    #     cmd1 = "ros2 run largemodel_arm grasp_desktop_remove"
+    #     cmd2 = f"ros2 run largemodel_arm apriltag_remove_higher --ros-args -p target_high:={target_highf:.2f}"
+    #     subprocess.Popen(
+    #         [
+    #             "gnome-terminal",
+    #             "--title=grasp_desktop_remove",
+    #             "--",
+    #             "bash",
+    #             "-c",
+    #             f"{cmd1}; exec bash",
+    #         ]
+    #     )
+    #     subprocess.Popen(
+    #         [
+    #             "gnome-terminal",
+    #             "--title=apriltag_remove_higher",
+    #             "--",
+    #             "bash",
+    #             "-c",
+    #             f"{cmd2}; exec bash",
+    #         ]
+    #     )
+    #
+    #     while not self.apriltag_remove_higher_future.done():
+    #         if self.interrupt_flag:
+    #             self.check_apriltag_remove_higher()
+    #             self.stop()
+    #             self.pubSix_Arm(self.init_joints)
+    #             return
+    #         time.sleep(0.1)
+    #     result = self.apriltag_remove_higher_future.result()
+    #
+    #     if not self.interrupt_flag:
+    #         if result.data == "apriltag_remove_higher_done":
+    #             self.action_status_pub(
+    #                 "apriltag_remove_higher_done", target_high=target_high
+    #             )
+    #         elif result.data == "apriltag_remove_higher_failed":
+    #             self.action_status_pub(
+    #                 "apriltag_remove_higher_failed", target_high=target_high
+    #             )
+    #
+    #     self.check_apriltag_remove_higher()
+    #     self.apriltag_remove_higher_future = Future()  # 复位Future对象
+    #     self.pubSix_Arm(self.init_joints)
+
+    # def check_color_remove_higher(self):
+    #     """
+    #     检查相关进程是否存活
+    #     """
+    #     pass
+
+    # def color_remove_higher(self, color, target_high):
+    #     self.check_color_remove_higher()
+    #     arm_joints = [90, 110, 0, 0, 90, 0]
+    #     self.pubSix_Arm(arm_joints)
+    #     color = color.strip("'\"")  # 去掉单引号和双引号
+    #     target_highf = float(target_high) / 100
+    #     if color == "red":
+    #         target_color = float(1)
+    #     elif color == "green":
+    #         target_color = float(2)
+    #     elif color == "blue":
+    #         target_color = float(3)
+    #     elif color == "yellow":
+    #         target_color = float(4)
+    #     else:
+    #         self.get_logger().info(
+    #             "Fatal ERROR:Incorrect color input,Does the AI output not meet expectations?"
+    #         )
+    #         self.action_status_pub(
+    #             "color_remove_higher_failed", color=color, target_high=target_high
+    #         )
+    #         return
+    #
+    #     cmd1 = "ros2 run largemodel_arm grasp_desktop_remove_color"
+    #     cmd2 = f"ros2 run largemodel_arm color_remove_higher --ros-args -p target_high:={target_highf:.2f} -p target_color:={target_color:.1f}"
+    #     subprocess.Popen(
+    #         [
+    #             "gnome-terminal",
+    #             "--title=grasp_desktop_remove_color",
+    #             "--",
+    #             "bash",
+    #             "-c",
+    #             f"{cmd1}; exec bash",
+    #         ]
+    #     )
+    #     subprocess.Popen(
+    #         [
+    #             "gnome-terminal",
+    #             "--title=color_remove_higher",
+    #             "--",
+    #             "bash",
+    #             "-c",
+    #             f"{cmd2}; exec bash",
+    #         ]
+    #     )
+    #
+    #     while not self.color_remove_higher_future.done():
+    #         if self.interrupt_flag:
+    #             self.check_color_remove_higher()
+    #             self.stop()
+    #             self.pubSix_Arm(self.init_joints)
+    #             return
+    #         time.sleep(0.1)
+    #
+    #     result = self.color_remove_higher_future.result()
+    #     if not self.interrupt_flag:
+    #         if result.data == "color_remove_higher_done":
+    #             self.action_status_pub(
+    #                 "color_remove_higher_done", color=color, target_high=target_high
+    #             )
+    #         else:
+    #             self.action_status_pub(
+    #                 "color_remove_higher_failed", color=color, target_high=target_high
+    #             )
+    #
+    #     self.check_color_remove_higher()
+    #     self.color_remove_higher_future = Future()  # 复位Future对象
+    #     self.pubSix_Arm(self.init_joints)
+
+    # def check_follw_line_clear(self):
+    #     """
+    #     检查相关进程是否存活
+    #     """
+    #     pass
+
+    # def follw_line_clear(self) -> None:
+    #     self.check_follw_line_clear()
+    #     cmd1 = "ros2 run largemodel_arm grasp_desktop_remove"
+    #     cmd2 = "ros2 run largemodel_arm follow_line --ros-args -p start_follow:=True"
+    #     subprocess.Popen(
+    #         [
+    #             "gnome-terminal",
+    #             "--title=grasp_desktop_remove",
+    #             "--",
+    #             "bash",
+    #             "-c",
+    #             f"{cmd1}; exec bash",
+    #         ]
+    #     )
+    #     subprocess.Popen(
+    #         [
+    #             "gnome-terminal",
+    #             "--title=follow_line",
+    #             "--",
+    #             "bash",
+    #             "-c",
+    #             f"{cmd2}; exec bash",
+    #         ]
+    #     )
+    #
+    #     while not self.follw_line_clear_future.done():
+    #         if self.interrupt_flag:
+    #             self.check_follw_line_clear()
+    #             self.stop()
+    #             self.pubSix_Arm(self.init_joints)
+    #             return
+    #         time.sleep(0.1)
+    #
+    #     if not self.interrupt_flag:
+    #         if self.follw_line_clear_future.result() is not None:
+    #             self.action_status_pub("follw_line_clear_done")
+    #
+    #     self.check_follw_line_clear()
+    #     self.follw_line_clear_future = Future()  # 复位Future对象
+    #     self.pubSix_Arm(self.init_joints)
+
+
+    # def check_all_process(self):
+    #     """
+    #     检查所有相关进程是否存活
+    #     """
+    #     pass
+
+
+
+
+    # 核心程序，解析动作列表并执行  # Core program, parse and execute action list
+    def execute_callback(self, goal_handle):
+        """
+        动作执行回调函数：分3种情况：  # Action execution callback function: divided into 3 cases:
+        1. 动作列表为空  # 1. Empty action list
+        2. 动作列表长度为1  # 2. Action list length is 1
+        3. 动作列表长度大于1  # 3. Action list length is greater than 1
+        文字交互模式下，不进行语音合成和播放  # In text interaction mode, no voice synthesis or playback is performed
+        """
+
+        if self.is_recording:
+            goal_handle.succeed()
+            result = Rot.Result()
+            result.success = True
+            return result
+
+        feedback_msg = Rot.Feedback()
+        actions = goal_handle.request.actions
+        self.action_runing = True
+        if not actions:  # 动作列表为空  # If the action list is empty
+            if not self.text_chat_mode and (
+                goal_handle.request.llm_response is not None
+                or goal_handle.request.text_response != ""
+            ):  # 语音模式，播放对话  # Voice mode, play dialogue
+                self.model_client.voice_synthesis(
+                    goal_handle.request.llm_response, self.tts_out_path
+                )
+                self.play_audio(self.tts_out_path, feedback=True)
+            else:
+                self.action_status_pub("response_done")
+
+        elif len(actions) == 1:  # 动作列表长度为1  # If the action list length is 1
+
+            action = actions[0]
+            if not self.text_chat_mode and (
+                goal_handle.request.llm_response is not None
+                or goal_handle.request.text_response != ""
+            ):  # 语音模式，播放对话  # Voice mode, play dialogue
+                self.model_client.voice_synthesis(
+                    goal_handle.request.llm_response, self.tts_out_path
+                )
+                self.play_audio(self.tts_out_path)
+
+            match = re.match(r"(\w+)\((.*)\)", action)
+            action_name, args_str = match.groups()
+            if not hasattr(self, action_name):
+                self.get_logger().warning(
+                    f"action_service: {action} is invalid action,skip execution"
+                )
+                self.action_status_pub(
+                    "failure_execute_action_function_not_exists"
+                )  # Robot feedback: action function does not exist, cannot execute
+
+            else:
+                action_name, args_str = match.groups()
+                args = [arg.strip() for arg in args_str.split(",")] if args_str else []
+                method = getattr(self, action_name)
+                method(*args)
+
+            if self.interrupt_flag:
+                self.interrupt_flag = False
+        else:  # 动作列表长度大于1,使能组合模式  # If the action list length is greater than 1, enable combination mode
+
+            self.combination_mode = True
+            if not self.text_chat_mode and (
+                goal_handle.request.llm_response is not None
+                or goal_handle.request.text_response != ""
+            ):  # 语音模式，播放对话  # Voice mode, play dialogue
+                self.model_client.voice_synthesis(
+                    goal_handle.request.llm_response, self.tts_out_path
+                )
+                self.play_audio_async(self.tts_out_path)
+
+            for action in actions:
+                if self.interrupt_flag:
+                    break
+                match = re.match(r"(\w+)\((.*)\)", action)
+                action_name, args_str = match.groups()
+                args = [arg.strip() for arg in args_str.split(",")] if args_str else []
+
+                if not hasattr(self, action_name):
+                    self.get_logger().warning(
+                        f"action_service: {action} is invalid action，skip execution"  # action_service: {action} is an invalid action, skip execution
+                    )
+                    self.action_status_pub(
+                        "failure_execute_action_function_not_exists"
+                    )  # Robot feedback: action function does not exist, cannot execute
+                else:
+                    method = getattr(self, action_name)
+                    method(*args)
+                    feedback_msg.status = f"action service execute  {action}  successed"
+
+            if not self.interrupt_flag:
+                self.action_status_pub(
+                    "multiple_done", actions=actions
+                )  # Robot feedback: execution of {actions} completed
+            self.combination_mode = (
+                False  # 重置组合模式标志位  # Reset combination mode flag
+            )
+        self.stop()  # 执行完全部动作停止机器人  # Stop the robot after executing all actions
+        self.action_runing = False  # 重置运行标志位  # Reset running flag
+        self.interrupt_flag = False
+        goal_handle.succeed()
+        result = Rot.Result()
+        result.success = True
+        return result
+
+    def finish_dialogue(self):  # 发布AI模型结束当前流程标志
+        self.first_record = True  # 重置导航记录标志位 # Reset navigation record flag
+        self.is_recording = False  # 重置录音标志位  # Reset recording flag
+        # self.pubSix_Arm(self.init_joints)  # 机械臂收回
+        self.action_status_pub("finish")  # 结束当前任务
+
+    def finishtask(self):
+        """
+        空操作,不反馈消息，用于结束反馈
+        """
+        return
+
+    def kill_process_tree(pid):
+        try:
+            parent = psutil.Process(pid)
+            children = parent.children(recursive=True)
+
+            # 先终止所有子进程
+            for child in children:
+                try:
+                    child.terminate()
+                except psutil.NoSuchProcess:
+                    pass
+
+            # 等待子进程终止
+            gone, alive = psutil.wait_procs(children, timeout=3)
+
+            # 强制杀死仍然存活的进程
+            for p in alive:
+                try:
+                    p.kill()
+                except psutil.NoSuchProcess:
+                    pass
+
+            # 最后终止父进程
+            try:
+                parent.terminate()
+                parent.wait(timeout=3)
+            except psutil.TimeoutExpired:
+                parent.kill()
+            except psutil.NoSuchProcess:
+                pass
+
+        except psutil.NoSuchProcess:
+            pass
+
+    def play_audio(self, file_path: str, feedback: Bool = False) -> None:
+        """
+        同步方式播放音频函数The function for playing audio in synchronous mode
+        """
+        if self.is_recording:
+            return
+        pygame.mixer.music.load(file_path)
+        pygame.mixer.music.play()
+        while pygame.mixer.music.get_busy():
+            if self.stop_event.is_set() or self.is_recording:
+                pygame.mixer.music.stop()
+                self.stop_event.clear()  # 清除事件
+                return
+            pygame.time.Clock().tick(10)
+        if feedback:
+            self.action_status_pub("response_done")
+
+    def play_audio_async(self, file_path: str, feedback: Bool = False) -> None:
+        """
+        异步方式播放音频函数The function for playing audio in asynchronous mode
+        """
+        if self.is_recording:
+            return
+
+        def target():
+            pygame.mixer.music.load(file_path)
+            pygame.mixer.music.play()
+            while pygame.mixer.music.get_busy():
+                if self.stop_event.is_set() or self.is_recording:
+                    pygame.mixer.music.stop()
+                    self.stop_event.clear()  # 清除事件
+                    return
+                pygame.time.Clock().tick(5)
+            if feedback:
+                self.action_status_pub("response_done")
+
+        thread = threading.Thread(target=target)
+        thread.daemon = True
+        thread.start()
+
+    def save_single_image(self):
+        """
+        保存一张图片 / Save a single image
+        """
+        if self.image_msg is None:
+            self.get_logger().warning("No image received yet.")  # 尚未接收到图像...
+            return
+        try:
+            # 将ROS图像消息转换为OpenCV图像 / Convert ROS image message to OpenCV image
+            cv_image = self.bridge.imgmsg_to_cv2(self.image_msg, "bgr8")
+            # 保存图片 / Save the image
+            cv2.imwrite(self.image_save_path, cv_image)
+            self.get_logger().info(f"Image saved to {self.image_save_path}")
+        except Exception as e:
+            self.get_logger().error(f"Error saving image: {e}")  # 保存图像时出错...
+
+    def display_saved_image(self):
+        """
+        显示已保存的图片4秒后关闭窗口 / Display the saved image for 4 seconds before closing the window
+        Note: 在无头环境或服务器环境中，图像显示功能被禁用，仅保留图片保存功能
+        """
+        # Image display is disabled in headless/server environments
+        # The image has been saved in save_single_image() for downstream processing
+        pass
+
+    def image_callback(self, msg):  # 图像回调函数 / Image callback function
+        self.image_msg = msg
+
+
+def main(args=None):
+    rclpy.init(args=args)
+    custom_action_server = CustomActionServer()
+    executor = MultiThreadedExecutor(num_threads=6)
+    executor.add_node(custom_action_server)
+
+    try:
+        executor.spin()
+    except KeyboardInterrupt:
+        custom_action_server.stop()
+        pass
+    finally:
+        custom_action_server.stop()
+        custom_action_server.destroy_node()
+        executor.shutdown()
+        rclpy.shutdown()
+
+
+if __name__ == "__main__":
+    main()
+
+
+

brain/PlannerNode2/largemodel/largemodel/asr.py

@@ -0,0 +1,368 @@
+import rclpy
+import os
+import time
+from rclpy.node import Node
+import pyaudio
+from playsound import playsound
+import wave
+import threading
+import webrtcvad
+import queue
+from std_msgs.msg import String, UInt16, Bool
+from utils.mic_serial import kws_mic
+from utils import large_model_interface
+from utils.large_model_interface import rec_wav_music_en
+from ament_index_python.packages import get_package_share_directory
+import functools
+def measure_execution_time(func):
+    """
+    装饰器：测量函数执行时间并使用 ROS 日志打印结果
+    """
+    @functools.wraps(func)
+    def wrapper(self, *args, **kwargs):
+        start_time = time.time()
+        result = func(self, *args, **kwargs)
+        end_time = time.time()
+        execution_time = end_time - start_time
+        
+        # 使用 ROS 日志系统记录执行时间
+        if hasattr(self, 'get_logger'):
+            self.get_logger().info(f"[性能统计] {func.__name__} 函数执行时间: {execution_time:.4f} 秒")
+        else:
+            print(f"[性能统计] {func.__name__} 函数执行时间: {execution_time:.4f} 秒")
+        return result
+    return wrapper
+class ASRNode(Node):
+    def __init__(self):
+        super().__init__("asr_node")
+        # 初始化参数、变量 / Initialize parameters and variables
+        self.init_param_config()
+        # 初始化语音唤醒 / Initialize keyword spotting (KWS)
+        self.kws_init()
+        # 初始化ASR模型 / Initialize ASR model
+        self.asr_mdoel_init()
+        # 初始化语言设置 / Initialize language settings
+        self.language_init()
+        # 初始化系统声音 / Initialize system sound functionality
+        self.system_sound_init()
+        # 初始化ROS通信 / Initialize ROS communication
+        self.init_ros_comunication()
+        # 打印初始化信息 / Log initialization completion
+        self.get_logger().info("asr_node Initialization completed")
+
+    def init_ros_comunication(self):
+        # 创建蜂鸣器发布者 / Create a publisher for the buzzer
+        self.pub_beep = self.create_publisher(UInt16, "beep", 10)
+        # 创建ASR发布者，发布转换完成的消息 / Create an ASR publisher to publish conversion results
+        self.asr_pub = self.create_publisher(String, "asr", 5)
+        # 创建唤醒信息发布者 / Create a publisher for wake-up signals
+        self.wakeup_pub = self.create_publisher(Bool, "wakeup", 5)
+        #创建发布录音状态发布者 / Create a publisher for recording status
+        self.record_status_pub=self.create_publisher(Bool, "record_status", 5)
+
+    def init_param_config(self):
+        self.user_speechdir = os.path.join(
+            get_package_share_directory("largemodel"),
+            "resources_file",
+            "user_speech.wav",
+        )
+        # 参数声明 / Declare parameters
+        self.declare_parameter("VAD_MODE", 1)
+        self.declare_parameter("sample_rate", 16000)
+        self.declare_parameter("frame_duration_ms", 30)
+        self.declare_parameter("language", "en")
+        self.declare_parameter("use_oline_asr", False)
+        self.declare_parameter("mic_serial_port", "/dev/mic")
+        self.declare_parameter("mic_index", 0)
+        self.declare_parameter("regional_setting", "China")
+
+
+        # 获取服务器参数 / Get server parameters
+        self.VAD_MODE = (
+            self.get_parameter("VAD_MODE").get_parameter_value().integer_value
+        )
+        self.sample_rate = (
+            self.get_parameter("sample_rate").get_parameter_value().integer_value
+        )
+        self.frame_duration_ms = (
+            self.get_parameter("frame_duration_ms").get_parameter_value().integer_value
+        )
+        self.language = (
+            self.get_parameter("language").get_parameter_value().string_value
+        )
+        self.use_oline_asr = (
+            self.get_parameter("use_oline_asr").get_parameter_value().bool_value
+        )
+        self.mic_serial_port = (
+            self.get_parameter("mic_serial_port").get_parameter_value().string_value
+        )
+        self.mic_index = (
+            self.get_parameter("mic_index").get_parameter_value().integer_value
+        )
+        self.regional_setting = (
+            self.get_parameter("regional_setting").get_parameter_value().string_value
+        )
+        self.frame_bytes = int(
+            self.sample_rate * self.frame_duration_ms / 1000
+        )  # 音频帧大小 / Audio frame size
+    
+        # 大模型接口实例端 / Instance of the large model interface
+        # 传入 logger 用于调试日志
+        self.modelinterface = large_model_interface.model_interface(logger=self.get_logger())
+        # 初始化 WebRTC VAD / Initialize WebRTC VAD
+        self.vad = webrtcvad.Vad()
+        self.vad.set_mode(self.VAD_MODE)
+        self.current_thread = None  # 唤醒处理线程 / Thread for handling wake-up events
+        self.stop_event = threading.Event()
+
+    def main_loop(self):
+        while rclpy.ok():
+            while (
+                self.audio_request_queue.qsize() > 1
+            ):  # 只处理最近的一次唤醒请求，防止重复唤醒 / Process only the most recent wake-up request to prevent duplicates
+                self.audio_request_queue.get()
+
+            if not self.audio_request_queue.empty():
+                self.audio_request_queue.get()
+                self.wakeup_pub.publish(
+                    Bool(data=True)
+                )  # 发布唤醒信号 / Publish wake-up signal
+                self.get_logger().info("I'm here")
+                playsound(
+                    self.audio_dict[self.first_response]
+                )  # 应答用户 / Respond to the user
+
+                if (
+                    self.current_thread and self.current_thread.is_alive()
+                ):  # 打断上次的唤醒处理线程 / Interrupt the previous wake-up handling thread
+                    self.stop_event.set()
+                    self.current_thread.join()  # 等待当前线程结束 / Wait for the current thread to finish
+                    self.stop_event.clear()  # 清除事件 / Clear the event
+                self.current_thread = threading.Thread(target=self.kws_handler)
+                self.current_thread.daemon = True
+                self.current_thread.start()
+            rclpy.spin_once(self, timeout_sec=0.1)
+            time.sleep(0.1)
+
+    def kws_handler(self) -> None:
+        if self.stop_event.is_set():
+            return
+
+        if self.listen_for_speech(self.mic_index):
+            asr_text = self.ASR_conversion(
+                self.user_speechdir
+            )  # 进行 ASR 转换 / Perform ASR conversion
+            if (
+                asr_text == "error"
+            ):  # 检查 ASR 结果长度是否小于4个字符 / Check if ASR result length is less than 4 characters
+                self.get_logger().warn(
+                    "I still don't understand what you mean. Please try again"
+                )
+                playsound(
+                    self.audio_dict[self.error_response]
+                )  # 错误响应 / Error response
+            else:
+                self.get_logger().info(asr_text)
+                self.get_logger().info("😀okay, let me think for a moment...")
+                self.asr_pub_result(asr_text)  # 发布 ASR结果 / Publish ASR result
+        else:
+            return
+
+    def system_sound_init(
+        self,
+    ):  # 初始化系统声音相关的功能 / Initialize system sound functionality
+        pkg_path = get_package_share_directory("largemodel")
+        self.audio_dict = {}  # 系统声音字典 / Dictionary of system sounds
+        self.audio_dict["longwan-women-1"] = os.path.join(
+            pkg_path, "resources_file", "longwan-women-1.mp3"
+        )
+        self.audio_dict["longwan-women-2"] = os.path.join(
+            pkg_path, "resources_file", "longwan-women-2.mp3"
+        )
+        self.audio_dict["longxiaochun-women-1"] = os.path.join(
+            pkg_path, "resources_file", "longxiaochun-women-1.mp3"
+        )
+        self.audio_dict["longxiaochun-women-2"] = os.path.join(
+            pkg_path, "resources_file", "longxiaochun-women-2.mp3"
+        )
+
+    def asr_mdoel_init(self):  # 初始化asr模型 / Initialize ASR model
+        if self.regional_setting == "international":  
+
+            self.get_logger().info(
+                f"The online asr model :XUN-FEI ASR is loaded"
+            )            
+        elif self.regional_setting == "China":
+            if self.use_oline_asr:
+                
+                self.get_logger().info(
+                    f"The online asr model :{self.modelinterface.init_oline_asr(self.language)} is loaded"
+                )
+            else:
+                # -------- SenseVoiceSmall 语音识别  --模型加载----- / Load SenseVoiceSmall online ASR model
+                self.modelinterface.init_local_asr_model()
+                self.get_logger().info("The asr model :SenseVoiceSmall is loaded")        
+
+        else:
+            while True:
+                self.get_logger().info('Please check the regional_setting parameter in yahboom.yaml file, it should be either "China" or "international".')
+                    
+                time.sleep(1)
+
+    def language_init(self):
+        if self.language == "zh":
+            self.first_response = "longwan-women-1"
+            self.error_response = "longwan-women-2"
+        elif self.language == "en":
+            self.first_response = "longxiaochun-women-1"
+            self.error_response = "longxiaochun-women-2"
+        else:
+            while True:
+                self.get_logger().error(
+                    "language setting error,please check your language setting"
+                )  # 语言设置错误，请检查语言设置 / Language setting error, please check your language setting
+                time.sleep(3)
+
+    def kws_init(
+        self,
+    ):  # 初始化关键词唤醒相关的内容 / Initialize keyword spotting (KWS) related content
+        self.port_name = self.mic_serial_port
+        self.audio_request_queue = (
+            queue.Queue()
+        )  # 用于传递音频请求 / Queue for passing audio requests
+        self.serial_port = kws_mic(
+            port=self.port_name, kwsquence=self.audio_request_queue, baudrate=115200
+        )
+        self.serial_port.open()
+        if not self.serial_port.ser or not self.serial_port.ser.is_open:
+            while True:
+                time.sleep(1)
+                self.get_logger().error(
+                    "Failed to open kws serial port.Please check whether the hardware wiring or the voice module is normal?"
+                )  # 未能打开kws串口 / Failed to open KWS serial port
+        receive_thread = threading.Thread(target=self.serial_port.receive_data)
+        receive_thread.daemon = True
+        receive_thread.start()
+
+    def asr_pub_result(self, asr_result: str) -> None:
+        msg = String(data=asr_result)
+        self.asr_pub.publish(msg)
+    # @measure_execution_time
+    def ASR_conversion(self, input_file: str) -> str:
+        if self.regional_setting == "international":  
+            res=rec_wav_music_en()
+            if res is not None:
+                return res
+            else:
+                return "error"
+        else:
+  
+            if self.use_oline_asr:
+                result = self.modelinterface.oline_asr(input_file)
+                if result[0] == "ok" and len(result[1]) > 4:
+                    return result[1]
+                else:
+                    self.get_logger().error(f"ASR Error:{result[1]}")  # ASR错误 / ASR error
+                    return "error"
+            else:
+                result = self.modelinterface.SenseVoiceSmall_ASR(input_file)
+                if result[0] == "ok" and len(result[1]) > 4:
+                    return result[1]
+                else:
+                    self.get_logger().error(f"ASR Error:{result[1]}")  # ASR错误 / ASR error
+                    return "error"
+
+    def listen_for_speech(self, mic_index=0):
+        self.record_status_pub.publish(Bool(data=True))
+        p = pyaudio.PyAudio()
+        audio_buffer = []
+        silence_counter = 0
+        MAX_SILENCE_FRAMES = 90  # 30帧*30ms=900ms静音后停止 / Stop after 900ms of silence (30 frames * 30ms)
+        speaking = False  # 语音活动标志 / Flag indicating speech activity
+        frame_counter = 0  # 计数器 / Frame counter
+        stream_kwargs = {
+            "format": pyaudio.paInt16,
+            "channels": 1,
+            "rate": self.sample_rate,
+            "input": True,
+            "frames_per_buffer": self.frame_bytes,
+        }
+        if mic_index != 0:
+            stream_kwargs["input_device_index"] = mic_index
+
+        # 通过蜂鸣器提示用户讲话 / Prompt the user to speak via the buzzer
+        self.pub_beep.publish(UInt16(data=1))
+        time.sleep(0.5)
+        self.pub_beep.publish(UInt16(data=0))
+
+        try:
+            # 打开音频流 / Open audio stream
+            stream = p.open(**stream_kwargs)
+            while True:
+                if self.stop_event.is_set():
+                    return False
+
+                frame = stream.read(
+                    self.frame_bytes, exception_on_overflow=False
+                )  # 读取音频数据 / Read audio data
+                is_speech = self.vad.is_speech(
+                    frame, self.sample_rate
+                )  # VAD检测 / VAD detection
+
+                if is_speech:
+                    # 检测到语音活动 / Detected speech activity
+                    speaking = True
+                    audio_buffer.append(frame)
+                    silence_counter = 0
+                else:
+                    if speaking:
+                        # 在语音活动后检测静音 / Detect silence after speech activity
+                        silence_counter += 1
+                        audio_buffer.append(
+                            frame
+                        )  # 持续记录缓冲 / Continue recording buffer
+
+                        # 静音持续时间达标时结束录音 / End recording when silence duration meets the threshold
+                        if silence_counter >= MAX_SILENCE_FRAMES:
+                            break
+                frame_counter += 1
+                if frame_counter % 2 == 0:
+                    self.get_logger().info("1" if is_speech else "-")
+                    
+        finally:
+            stream.stop_stream()
+            stream.close()
+            p.terminate()
+            self.record_status_pub.publish(Bool(data=False))
+
+        # 保存有效录音（去除尾部静音） / Save valid recording (remove trailing silence)
+        if speaking and len(audio_buffer) > 0:
+            # 裁剪最后静音部分 / Trim the last silent part
+            clean_buffer = (
+                audio_buffer[:-MAX_SILENCE_FRAMES]
+                if len(audio_buffer) > MAX_SILENCE_FRAMES
+                else audio_buffer
+            )
+
+            with wave.open(self.user_speechdir, "wb") as wf:
+                wf.setnchannels(1)
+                wf.setsampwidth(p.get_sample_size(pyaudio.paInt16))
+                wf.setframerate(self.sample_rate)
+                wf.writeframes(b"".join(clean_buffer))
+                return True
+            
+        
+def main(args=None):
+    rclpy.init(args=args)
+    sense_voice_node = ASRNode()
+    try:
+        sense_voice_node.main_loop()
+    except KeyboardInterrupt:
+        pass
+    finally:
+        sense_voice_node.destroy_node()
+        rclpy.shutdown()
+
+
+if __name__ == "__main__":
+    main()

brain/PlannerNode2/largemodel/largemodel/model_service.py

@@ -0,0 +1,483 @@
+import os
+import json
+import rclpy
+from rclpy.node import Node
+from interfaces.action import Rot
+from std_msgs.msg import String
+from utils import large_model_interface
+from rclpy.action import ActionClient
+from ament_index_python.packages import get_package_share_directory
+from utils.promot import get_prompt, get_map_mapping, set_map_mapping, set_large_model_config, set_model_paths, set_system_config
+import time
+import re
+import functools
+
+
+def measure_execution_time(func):
+    """
+    装饰器：测量函数执行时间并使用 ROS 日志打印结果
+    """
+    @functools.wraps(func)
+    def wrapper(self, *args, **kwargs):
+        start_time = time.time()
+        result = func(self, *args, **kwargs)
+        end_time = time.time()
+        execution_time = end_time - start_time
+        
+        # 使用 ROS 日志系统记录执行时间
+        if hasattr(self, 'get_logger'):
+            self.get_logger().info(f"[性能统计] {func.__name__} 函数执行时间: {execution_time:.4f} 秒")
+        else:
+            print(f"[性能统计] {func.__name__} 函数执行时间: {execution_time:.4f} 秒")
+        return result
+    return wrapper
+
+class LargeModelService(Node):
+    def __init__(self):
+        super().__init__("LargeModelService")
+
+        self.init_param_config()  # 初始化参数配置 / Initialize parameter configuration
+        self.init_largemodel()  # 初始化大模型 / Initialize large model
+        self.init_ros_comunication()  # 初始化ROS通信 / Initialize ROS communication
+        self.init_language()  # 初始化语言/Initialize language
+
+        self.get_logger().info(
+            "LargeModelService node Initialization completed..."
+        )  # 打印日志 / Print log
+
+    def init_largemodel(self):
+        # 创建模型接口客户端 / Create model interface client
+        # 传入 logger 用于调试日志
+        self.model_client = large_model_interface.model_interface(logger=self.get_logger())
+        self.new_order_cycle = True  # 新指令周期标志 / New order cycle flag
+        if self.regional_setting == "China":  # 如果是中国地区
+            self.model_client.init_Multimodal()  # 初始化执行层模型，决策层模型无需初始化 / Initialize execution layer model, decision layer model does not need initialization
+        elif self.regional_setting == "international":  # 如果是国际地区
+            self.model_client.init_dify_client()
+        else:
+            while True:
+                self.get_logger().info()(
+                    'Please check the regional_setting parameter in yahboom.yaml file, it should be either "China" or "international".'
+                )
+                time.sleep(1)
+
+    def init_param_config(self):
+        self.pkg_path = get_package_share_directory("largemodel")
+        self.image_save_path = os.path.join(
+            self.pkg_path, "resources_file", "image.png"
+        )
+        # 参数声明 / Parameter declaration
+
+        self.declare_parameter("language", "zh")
+        self.declare_parameter("regional_setting", "China")
+        self.declare_parameter("text_chat_mode", False)
+        # 获取参数服务器参数 / Get parameters from the parameter server
+        self.language = (
+            self.get_parameter("language").get_parameter_value().string_value
+        )
+        self.regional_setting = (
+            self.get_parameter("regional_setting").get_parameter_value().string_value
+        )
+        self.text_chat_mode = (
+            self.get_parameter("text_chat_mode").get_parameter_value().bool_value
+        )
+
+        self.conversation_id = None  # 会话id
+        self.map_mapping = ""  # 地图映射（从订阅获取）
+        self.config_data = {}  # 配置数据（从订阅获取）
+        self.map_points = []  # 地图导航点列表（从订阅获取）
+        self.environment_data = {}  # 环境数据（从订阅获取）
+
+    def init_language(self):
+        self.language_dict = {
+            "zh": "中文",
+            "en": "English",
+        }
+        language_list = ["zh", "en"]
+        if self.language not in language_list:
+            while True:
+                self.get_logger().info(
+                    "The language setting is incorrect. Please check the action_service'' language setting in the yahboom.yaml file"
+                )
+                self.get_logger().info(self.language)
+                time.sleep(1)
+        self.prompt_dict = {  #
+            "zh": {  # 中文 / Chinese
+                "prompt_1": "用户：{prompt},决策层AI规划:{execute_instructions}",
+                "prompt_2": "机器人反馈:执行seewhat()完成",
+                "prompt_3": "决策层AI规划:{execute_instructions}",
+            },
+            "en": {  # 英文 / English
+                "prompt_1": "user:{prompt},Decision making AI planning:{execute_instructions}",
+                "prompt_2": "Robot feedback: Execute seewhat() completed",
+                "prompt_3": "Decision making AI planning:{execute_instructions}",
+            },
+        }
+
+    def init_ros_comunication(self):
+        # 创建执行动作状态订阅者 / Create action status subscriber
+        self.actionstatus_sub = self.create_subscription(
+            String, "actionstatus", self.actionstatus_callback, 1
+        )
+        # 创建动作客户端，连接到 'action_service' / Create action client, connect to 'action_service'
+        self._action_client = ActionClient(self, Rot, "action_service")
+        # asr话题订阅者 / ASR topic subscriber
+        self.asrsub = self.create_subscription(String, "asr", self.asr_callback, 1)
+        # 创建seehat订阅者 / Create seewhat subscriber
+        self.seewhat_sub = self.create_subscription(
+            String, "seewhat_handle", self.seewhat_callback, 1
+        )
+        # 创建执行动作状态发布者 / Create action status publisher
+        self.actionstatus_pub = self.create_publisher(String, "actionstatus", 1)
+        # 创建文字交互发布者 / Create text interaction publisher
+        self.text_pub = self.create_publisher(String, "text_response", 1)
+
+        # 订阅配置节点数据 / Subscribe config node data
+        self.config_sub = self.create_subscription(
+            String, "/ai/config", self.config_callback, 10
+        )
+        self.environment_sub = None  # 环境数据订阅者（需要动态更新）
+        self.environment_topic = "/ai/env"  # 默认值（与 environment_node publish_topic 一致）
+
+        # 初始化时就创建环境数据订阅（使用默认 topic）
+        self.update_environment_subscription()
+
+    def seewhat_callback(self, msg):
+        if msg.data == "seewhat":
+            if (
+                self.regional_setting == "China"
+            ):  # 在线模型推理方式：决策层推理+执行层监督 / Online model inference method: Decision layer reasoning + Execution layer supervision
+                self.dual_large_model_mode(type="image")
+            else:
+                self.dual_large_model_international_model(type="image")
+
+    def asr_callback(self, msg):
+        if (
+            self.regional_setting == "China"
+        ):  # 在线模型推理方式：决策层推理+执行层监督 / Online model inference method: Decision layer reasoning + Execution layer supervision
+            self.dual_large_model_mode(type="text", prompt=msg.data)
+        else:
+            self.dual_large_model_international_model(type="text", prompt=msg.data)
+
+    def actionstatus_callback(self, msg):
+        if (
+            msg.data == "finish"
+        ):  # 如果收到的是finish则表示当前指令执行完成，开启新的指令执行周期 / If "finish" is received, it means the current instruction has been executed and a new instruction cycle begins
+            self.new_order_cycle = True
+            self.get_logger().info(
+                "The current instruction cycle has ended"
+            )  # 当前指令周期已结束...
+        else:  # 向指令执行层大模型反馈动作执行结果 / Feedback action execution results to the large model in the command execution layer
+            if self.regional_setting == "China":
+                self.dual_large_model_mode(type="text", prompt=msg.data)
+            else:
+                self.dual_large_model_international_model(type="text", prompt=msg.data)
+
+    def config_callback(self, msg):
+        """
+        订阅配置数据回调函数
+        从 /ai/config topic 接收配置数据
+        """
+        try:
+            config_json = json.loads(msg.data)
+            self.config_data = config_json.get('config', {})
+
+            # 更新大模型配置
+            large_model_config = self.config_data.get('large_model', {})
+            if large_model_config:
+                set_large_model_config(large_model_config)
+                # 如果模型接口支持动态更新，则调用更新接口
+                if hasattr(self.model_client, 'update_config'):
+                    self.model_client.update_config(large_model_config)
+
+            # 更新模型路径
+            model_paths = self.config_data.get('model_paths', {})
+            if model_paths:
+                set_model_paths(model_paths)
+
+            # 更新系统配置
+            system_config = self.config_data.get('system', {})
+            if system_config:
+                set_system_config(system_config)
+
+            # 更新 topics 配置
+            topics_config = self.config_data.get('topics', {})
+            if topics_config:
+                environment_node_config = topics_config.get('environment_node', {})
+                if environment_node_config:
+                    new_topic = environment_node_config.get('environment_topic', '/ai/env')
+                    if new_topic != self.environment_topic or self.environment_sub is None:
+                        self.environment_topic = new_topic
+                        self.update_environment_subscription()
+                        self.get_logger().info(f'[配置] 环境数据订阅 Topic 已更新: {self.environment_topic}')
+
+        except Exception as e:
+            self.get_logger().warn(f'解析配置数据失败: {e}')
+
+    def update_environment_subscription(self):
+        """动态更新环境数据订阅"""
+        try:
+            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 environment_callback(self, msg):
+        """
+        订阅环境数据回调函数
+        从 /ai/environment topic 接收环境数据
+        """
+        try:
+            env_json = json.loads(msg.data)
+            self.environment_data = env_json
+
+            # 更新地图映射数据
+            map_data = env_json.get('map', {})
+            if map_data:
+                points = map_data.get('points', [])
+                if points:
+                    # 将 points 字典转换为地图映射格式
+                    # 使用更清晰的格式，让大模型知道用 id 调用
+                    map_str = "#地图映射\n\n"
+                    for point in points:
+                        point_id = point.get('id', '')
+                        name = point.get('name', '')
+                        if point_id and name:
+                            map_str += f"{point_id} -> {name}\n"
+                    self.map_mapping = map_str
+                    set_map_mapping(map_str)
+                    self.map_points = points
+                else:
+                    self.get_logger().warn("[环境回调] points 为空")
+            else:
+                self.get_logger().warn("[环境回调] map_data 为空")
+
+        except Exception as e:
+            self.get_logger().warn(f'解析环境数据失败: {e}')
+
+    # @measure_execution_time
+    def dual_large_model_mode(self, type, prompt=""):
+        """
+        此函数实现了双模型推理模式，即先由文本生成模型进行任务规划，然后由多模态大模型生成动作列表
+        This function implements the dual model inference mode, where the text generation model first plans the task, and then the multimodal large model generates the action list.
+        """
+        if (
+            self.new_order_cycle
+        ):  # 判断是否是新任务周期 / Determine if it is a new task cycle
+            # 获取完整的 prompt 并打印
+            full_prompt = get_prompt()
+            self.get_logger().info("=" * 80)
+            self.get_logger().info("[调试] 发送给决策层大模型的完整 Prompt:")
+            self.get_logger().info("=" * 80)
+            self.get_logger().info(full_prompt)
+            self.get_logger().info("=" * 80)
+            self.get_logger().info(f"[调试] 用户输入: {prompt}")
+            self.get_logger().info("=" * 80)
+            
+            # 判断上一轮对话指令是否完成如果完成就清空历史上下文，开启新的上下文 / Determine if the previous round of dialogue instructions are completed. If completed, clear the historical context and start a new context
+            self.model_client.init_Multimodal_history(full_prompt)  # 初始化执行层上下文历史 / Initialize execution layer context history
+            execute_instructions = self.model_client.TaskDecision(
+                prompt
+            )  # 调用决策层大模型进行任务规划 / Call the decision layer large model for task planning
+
+            if not execute_instructions == "error":
+
+                prompt_desidon = (
+                    self.prompt_dict[self.language]
+                    .get("prompt_3")
+                    .format(execute_instructions=execute_instructions[1])
+                )  # 翻译成对应语言的prompt /translate into the corresponding language prompt
+                if self.text_chat_mode:
+                    msg = String(data=prompt_desidon)
+                    self.text_pub.publish(msg)
+                else:
+                    self.get_logger().info(prompt_desidon)  # 即将执行的任务：...
+
+                prompt_desidon = (
+                    self.prompt_dict[self.language]
+                    .get("prompt_1")
+                    .format(prompt=prompt, execute_instructions=execute_instructions[1])
+                )  # 翻译成对应语言的prompt /translate into the corresponding language prompt
+
+                self.instruction_process(
+                    type="text",
+                    prompt=prompt_desidon,
+                )  # 传递决策层模型规划好的执行步骤给执行层模型 / Pass the planned execution steps from the decision layer model to the execution layer model
+
+                self.new_order_cycle = (
+                    False  # 重置指令周期标志位 / Reset the instruction cycle flag
+                )
+            else:
+                self.get_logger().info(
+                    "The model service is abnormal. Check the large model account or configuration options"
+                )  # 模型推理失败，请检查模型配额和账户是否正常！！！
+        else:
+            self.instruction_process(
+                prompt, type
+            )  # 调用执行层大模型生成成动作列表并执行 / Call the execution layer large model to generate an action list and execute
+
+    def instruction_process(self, prompt, type, conversation_id=None):
+        """
+        根据输入信息的类型（文字/图片），构建不同的请求体进行推理，并返回结果）
+        Based on the type of input information (text/image), construct different request bodies for inference and return the result.
+        """
+        prompt_seewhat = self.prompt_dict[self.language].get("prompt_2")
+        if self.regional_setting == "China":  # 国内版
+            if type == "text":
+                raw_content = self.model_client.multimodalinfer(prompt)
+            elif type == "image":
+                raw_content = self.model_client.multimodalinfer(
+                    prompt_seewhat, image_path=self.image_save_path
+                )
+            json_str = self.extract_json_content(raw_content)
+
+        elif self.regional_setting == "international":  # 国际版
+            if type == "text":
+                result = self.model_client.TaskExecution(
+                    input=prompt,
+                    map_mapping=self.map_mapping,
+                    language=self.language_dict[self.language],
+                    conversation_id=conversation_id,
+                )
+                if result[0]:
+                    json_str = self.extract_json_content(result[1])
+                    self.conversation_id = result[2]
+                else:
+                    self.get_logger().info(f"ERROR:{result[1]}")
+            elif type == "image":
+                result = self.model_client.TaskExecution(
+                    input=prompt_seewhat,
+                    map_mapping=self.map_mapping,
+                    language=self.language_dict[self.language],
+                    image_path=self.image_save_path,
+                    conversation_id=conversation_id,
+                )
+                if result[0]:
+                    json_str = self.extract_json_content(result[1])
+                    self.conversation_id = result[2]
+                else:
+                    self.get_logger().info(f"ERROR:{result[1]}")
+
+        if json_str is not None:
+            # 解析JSON字符串,分离"action"、"response"字段 / Parse JSON string, separate "action" and "response" fields
+            action_plan_json = json.loads(json_str)
+            action_list = action_plan_json.get("action", [])
+            llm_response = action_plan_json.get("response", "")
+        else:
+            self.get_logger().info(
+                f"LargeScaleModel return: {json_str},The format was unexpected. The output format of the AI model at the execution layer did not meet the requirements"
+            )
+            return
+
+        if self.text_chat_mode:
+            msg = String(data=f'"action": {action_list}, "response": {llm_response}')
+            self.text_pub.publish(msg)
+        else:
+            self.get_logger().info(
+                f'"action": {action_list}, "response": {llm_response}'
+            )
+
+        self.send_action_service(
+            action_list, llm_response
+        )  # 异步发送动作列表、回复内容给ActionServer / Asynchronously send action list and response content to ActionServer
+
+    def dual_large_model_international_model(self, type, prompt=""):
+        """
+        此函数适用于国际版双模型推理模式,使用dify作为中间件
+        /this function is suitable for international model inference mode, using dify as the middleware
+        """
+        if (
+            self.new_order_cycle
+        ):  # 判断是否是新任务周期 / Determine if it is a new task cycle
+            self.conversation_id = None
+            result = self.model_client.TaskDecision(prompt)
+
+            if result[0]:
+                prompt_desidon = (
+                    self.prompt_dict[self.language]
+                    .get("prompt_3")
+                    .format(execute_instructions=result[1])
+                )  # 翻译成对应语言的prompt /translate into the corresponding language prompt
+                if self.text_chat_mode:  # 文字交互模式 / Text interaction mode
+                    msg = String(data=prompt_desidon)
+                    self.text_pub.publish(msg)
+                else:  # 语音交互模式 / Voice interaction mode
+                    self.get_logger().info(prompt_desidon)
+                prompt_desion = (
+                    self.prompt_dict[self.language]
+                    .get("prompt_1")
+                    .format(prompt=prompt, execute_instructions=result[1])
+                )
+                self.instruction_process(type="text", prompt=prompt_desion)
+
+                self.new_order_cycle = (
+                    False  # 重置指令周期标志位 / Reset the instruction cycle flag
+                )
+            else:
+                self.get_logger().info(
+                    "The model service is abnormal. Check the large model account or configuration options"
+                )  # 模型推理失败，请检查模型配额和账户是否正常！！！
+
+        else:
+            self.instruction_process(
+                prompt, type, conversation_id=self.conversation_id
+            )  # 调用执行层大模型生成成动作列表并执行 / Call the execution layer large model to generate an action list and execute
+
+    def send_action_service(self, actions, text):
+        goal_msg = Rot.Goal()  # 创建目标消息对象 / Create goal message object
+        goal_msg.actions = actions  # 设置目标消息中的动作列表 / Set the action list in the goal message
+        goal_msg.llm_response = text
+        self._send_goal_future = self._action_client.send_goal_async(goal_msg)
+        # 添加目标发送后的响应回调函数 / Add response callback function after sending the goal
+        self._send_goal_future.add_done_callback(self.goal_response_callback)
+
+    def goal_response_callback(self, future):
+        goal_handle = future.result()  # 获取目标句柄 / Get goal handle
+        if not goal_handle.accepted:
+            self.get_logger().info(
+                "action_client message: action service rejected action list"
+            )  # 目标被拒绝...
+
+    @staticmethod
+    def extract_json_content(
+        raw_content,
+    ):  # 解析变量提取json / Extract JSON by parsing variables
+        try:
+            # 方法一：分割代码块 / Method 1: Split code blocks
+            if "```json" in raw_content:
+                # 分割代码块并取中间部分 / Split code blocks and take the middle part
+                json_str = raw_content.split("```json")[1].split("```")[0].strip()
+            elif "```" in raw_content:
+                # 处理没有指定类型的代码块 / Handle code blocks without specified types
+                json_str = raw_content.split("```")[1].strip()
+            else:
+                # 直接尝试解析 / Try parsing directly
+                json_str = raw_content
+
+            # 方法二：正则表达式提取（备用方案） / Method 2: Regular expression extraction (backup plan)
+            if not json_str:
+
+                match = re.search(r"\{.*\}", raw_content, re.DOTALL)
+                if match:
+                    json_str = match.group()
+
+            return json_str
+
+        except Exception as e:
+            return None
+
+
+
+def main(args=None):
+    rclpy.init(args=args)
+    model_service = LargeModelService()
+    rclpy.spin(model_service)
+    rclpy.shutdown()
+
+
+if __name__ == "__main__":
+    main()

brain/PlannerNode2/largemodel/launch/largemodel_control.launch.py

@@ -0,0 +1,64 @@
+import os
+from ament_index_python.packages import get_package_share_directory
+from launch import LaunchDescription
+from launch_ros.actions import Node
+from launch.launch_description_sources import PythonLaunchDescriptionSource
+from launch.actions import IncludeLaunchDescription
+from launch.actions import DeclareLaunchArgument
+from launch.substitutions import LaunchConfiguration
+from launch.conditions import UnlessCondition
+def generate_launch_description():
+    # 获取包的共享目录
+    #M3Pro_demopkg_share = get_package_share_directory('M3Pro_demo')
+    params_file=os.path.join(get_package_share_directory('largemodel'), "config", "yahboom.yaml") 
+
+    #启动参数
+    text_chat_mode = LaunchConfiguration('text_chat_mode', default=False)
+    text_chat_mode_arg= DeclareLaunchArgument('text_chat_mode', default_value=text_chat_mode)
+
+    # 定义节点
+    model_server = Node(
+        package='largemodel',
+        executable='model_service',
+        name='model_service',
+        parameters=[
+            params_file,
+            {'text_chat_mode': text_chat_mode}  # 动态参数，命令行参数覆盖yahboom.yaml同名参数再传给节点
+        ],
+        output='screen'
+    )
+
+    asr_server = Node(
+        package='largemodel',
+        executable='asr',
+        name='asr',
+        parameters=[params_file],
+        output='screen',
+        condition=UnlessCondition(text_chat_mode)
+    )
+
+    action_server = Node(
+        package='largemodel',
+        executable='action_service',
+        name='action_service',
+        parameters=[
+            params_file,
+            {'text_chat_mode': text_chat_mode}  # 动态参数，命令行参数覆盖yahboom.yaml同名参数再传给节点
+        ],
+        output='screen'
+    )
+
+    #camrea_kin_node = IncludeLaunchDescription(PythonLaunchDescriptionSource(os.path.join(M3Pro_demopkg_share, 'launch', 'camera_arm_kin.launch.py')))
+
+    return LaunchDescription([
+        text_chat_mode_arg,    #声明启动参数
+        #camrea_kin_node,       #启动相机和运动学结算节点
+        model_server,          #启动模型服务节点
+        action_server,         #启动动作服务器节
+        asr_server,            #启动asr用户交互节点
+    ])
+
+
+
+
+

brain/PlannerNode2/largemodel/package.xml

@@ -0,0 +1,18 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>largemodel</name>
+  <version>0.0.0</version>
+  <description>TODO: Package description</description>
+  <maintainer email="jetson@todo.todo">jetson</maintainer>
+  <license>TODO: License declaration</license>
+
+  <test_depend>ament_copyright</test_depend>
+  <test_depend>ament_flake8</test_depend>
+  <test_depend>ament_pep257</test_depend>
+  <test_depend>python3-pytest</test_depend>
+
+  <export>
+    <build_type>ament_python</build_type>
+  </export>
+</package>

brain/PlannerNode2/largemodel/setup.cfg

@@ -0,0 +1,4 @@
+[develop]
+script_dir=$base/lib/largemodel
+[install]
+install_scripts=$base/lib/largemodel

brain/PlannerNode2/largemodel/setup.py

@@ -0,0 +1,32 @@
+from setuptools import find_packages, setup
+import os
+from glob import glob
+package_name = 'largemodel'
+
+setup(
+    name=package_name,
+    version='0.0.0',
+    packages=find_packages(exclude=['test']),
+    data_files=[
+        ('share/ament_index/resource_index/packages',
+            ['resource/' + package_name]),
+        ('share/' + package_name, ['package.xml']),
+        (os.path.join('share', package_name, 'launch'), glob('launch/*.launch.py')),
+        (os.path.join('share', package_name, 'config'), glob('config/*.yaml')),
+        (os.path.join('share', package_name, 'resources_file'), [os.path.join(root, f) for root, dirs, files in os.walk('resources_file') for f in files]),
+    ],
+    install_requires=['setuptools'],
+    zip_safe=True,
+    maintainer='jetson',
+    maintainer_email='jetson@todo.todo',
+    description='TODO: Package description',
+    license='TODO: License declaration',
+    tests_require=['pytest'],
+    entry_points={
+        'console_scripts': [
+            'asr = largemodel.asr:main',
+            'action_service = largemodel.action_service:main',
+            'model_service = largemodel.model_service:main',   
+        ],
+    },
+)

brain/PlannerNode2/largemodel/test/test_copyright.py

@@ -0,0 +1,25 @@
+# Copyright 2015 Open Source Robotics Foundation, Inc.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+#     http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+from ament_copyright.main import main
+import pytest
+
+
+# Remove the `skip` decorator once the source file(s) have a copyright header
+@pytest.mark.skip(reason='No copyright header has been placed in the generated source file.')
+@pytest.mark.copyright
+@pytest.mark.linter
+def test_copyright():
+    rc = main(argv=['.', 'test'])
+    assert rc == 0, 'Found errors'

brain/PlannerNode2/largemodel/test/test_flake8.py

@@ -0,0 +1,25 @@
+# Copyright 2017 Open Source Robotics Foundation, Inc.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+#     http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+from ament_flake8.main import main_with_errors
+import pytest
+
+
+@pytest.mark.flake8
+@pytest.mark.linter
+def test_flake8():
+    rc, errors = main_with_errors(argv=[])
+    assert rc == 0, \
+        'Found %d code style errors / warnings:\n' % len(errors) + \
+        '\n'.join(errors)

brain/PlannerNode2/largemodel/test/test_pep257.py

@@ -0,0 +1,23 @@
+# Copyright 2015 Open Source Robotics Foundation, Inc.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+#     http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+from ament_pep257.main import main
+import pytest
+
+
+@pytest.mark.linter
+@pytest.mark.pep257
+def test_pep257():
+    rc = main(argv=['.', 'test'])
+    assert rc == 0, 'Found code style errors / warnings'

brain/PlannerNode2/largemodel/utils/dify_client2.py

@@ -0,0 +1,459 @@
+import json
+
+import requests
+
+
+class DifyClient:
+    def __init__(self, api_key, base_url: str = "https://api.dify.ai/v1"):
+        self.api_key = api_key
+        self.base_url = base_url
+
+    def _send_request(self, method, endpoint, json=None, params=None, stream=False):
+        headers = {
+            "Authorization": f"Bearer {self.api_key}",
+            "Content-Type": "application/json",
+        }
+
+        url = f"{self.base_url}{endpoint}"
+        response = requests.request(
+            method, url, json=json, params=params, headers=headers, stream=stream
+        )
+
+        return response
+
+    def _send_request_with_files(self, method, endpoint, data, files):
+        headers = {"Authorization": f"Bearer {self.api_key}"}
+
+        url = f"{self.base_url}{endpoint}"
+        response = requests.request(
+            method, url, data=data, headers=headers, files=files
+        )
+
+        return response
+
+    def message_feedback(self, message_id, rating, user):
+        data = {"rating": rating, "user": user}
+        return self._send_request("POST", f"/messages/{message_id}/feedbacks", data)
+
+    def get_application_parameters(self, user):
+        params = {"user": user}
+        return self._send_request("GET", "/parameters", params=params)
+
+    def file_upload(self, user, files):
+        data = {"user": user}
+        return self._send_request_with_files(
+            "POST", "/files/upload", data=data, files=files
+        )
+
+    def text_to_audio(self, text: str, user: str, streaming: bool = False):
+        data = {"text": text, "user": user, "streaming": streaming}
+        return self._send_request("POST", "/text-to-audio", json=data)
+
+    def get_meta(self, user):
+        params = {"user": user}
+        return self._send_request("GET", "/meta", params=params)
+
+
+class CompletionClient(DifyClient):
+    def create_completion_message(self, inputs, response_mode, user, files=None):
+        data = {
+            "inputs": inputs,
+            "response_mode": response_mode,
+            "user": user,
+            "files": files,
+        }
+        return self._send_request(
+            "POST",
+            "/completion-messages",
+            data,
+            stream=True if response_mode == "streaming" else False,
+        )
+
+
+class ChatClient(DifyClient):
+    def create_chat_message(
+        self,
+        inputs,
+        query,
+        user,
+        response_mode="blocking",
+        conversation_id=None,
+        files=None,
+    ):
+        data = {
+            "inputs": inputs,
+            "query": query,
+            "user": user,
+            "response_mode": response_mode,
+            "files": files,
+        }
+        if conversation_id:
+            data["conversation_id"] = conversation_id
+    
+        return self._send_request(
+            "POST",
+            "/chat-messages",
+            data,
+            stream=True if response_mode == "streaming" else False,
+        )
+    
+    def get_suggested(self, message_id, user: str):
+        params = {"user": user}
+        return self._send_request(
+            "GET", f"/messages/{message_id}/suggested", params=params
+        )
+
+    def stop_message(self, task_id, user):
+        data = {"user": user}
+        return self._send_request("POST", f"/chat-messages/{task_id}/stop", data)
+
+    def get_conversations(self, user, last_id=None, limit=None, pinned=None):
+        params = {"user": user, "last_id": last_id, "limit": limit, "pinned": pinned}
+        return self._send_request("GET", "/conversations", params=params)
+
+    def get_conversation_messages(
+        self, user, conversation_id=None, first_id=None, limit=None
+    ):
+        params = {"user": user}
+
+        if conversation_id:
+            params["conversation_id"] = conversation_id
+        if first_id:
+            params["first_id"] = first_id
+        if limit:
+            params["limit"] = limit
+
+        return self._send_request("GET", "/messages", params=params)
+
+    def rename_conversation(
+        self, conversation_id, name, auto_generate: bool, user: str
+    ):
+        data = {"name": name, "auto_generate": auto_generate, "user": user}
+        return self._send_request(
+            "POST", f"/conversations/{conversation_id}/name", data
+        )
+
+    def delete_conversation(self, conversation_id, user):
+        data = {"user": user}
+        return self._send_request("DELETE", f"/conversations/{conversation_id}", data)
+
+    def audio_to_text(self, audio_file, user):
+        data = {"user": user}
+        files = {"audio_file": audio_file}
+        return self._send_request_with_files("POST", "/audio-to-text", data, files)
+
+
+class WorkflowClient(DifyClient):
+    def run(
+        self, inputs: dict, response_mode: str = "streaming", user: str = "abc-123"
+    ):
+        data = {"inputs": inputs, "response_mode": response_mode, "user": user}
+        return self._send_request("POST", "/workflows/run", data)
+
+    def stop(self, task_id, user):
+        data = {"user": user}
+        return self._send_request("POST", f"/workflows/tasks/{task_id}/stop", data)
+
+    def get_result(self, workflow_run_id):
+        return self._send_request("GET", f"/workflows/run/{workflow_run_id}")
+
+
+class KnowledgeBaseClient(DifyClient):
+    def __init__(
+        self,
+        api_key,
+        base_url: str = "https://api.dify.ai/v1",
+        dataset_id: str | None = None,
+    ):
+        """
+        Construct a KnowledgeBaseClient object.
+
+        Args:
+            api_key (str): API key of Dify.
+            base_url (str, optional): Base URL of Dify API. Defaults to 'https://api.dify.ai/v1'.
+            dataset_id (str, optional): ID of the dataset. Defaults to None. You don't need this if you just want to
+                create a new dataset. or list datasets. otherwise you need to set this.
+        """
+        super().__init__(api_key=api_key, base_url=base_url)
+        self.dataset_id = dataset_id
+
+    def _get_dataset_id(self):
+        if self.dataset_id is None:
+            raise ValueError("dataset_id is not set")
+        return self.dataset_id
+
+    def create_dataset(self, name: str, **kwargs):
+        return self._send_request("POST", "/datasets", {"name": name}, **kwargs)
+
+    def list_datasets(self, page: int = 1, page_size: int = 20, **kwargs):
+        return self._send_request(
+            "GET", f"/datasets?page={page}&limit={page_size}", **kwargs
+        )
+
+    def create_document_by_text(
+        self, name, text, extra_params: dict | None = None, **kwargs
+    ):
+        """
+        Create a document by text.
+
+        :param name: Name of the document
+        :param text: Text content of the document
+        :param extra_params: extra parameters pass to the API, such as indexing_technique, process_rule. (optional)
+            e.g.
+            {
+            'indexing_technique': 'high_quality',
+            'process_rule': {
+                'rules': {
+                    'pre_processing_rules': [
+                        {'id': 'remove_extra_spaces', 'enabled': True},
+                        {'id': 'remove_urls_emails', 'enabled': True}
+                    ],
+                    'segmentation': {
+                        'separator': '\n',
+                        'max_tokens': 500
+                    }
+                },
+                'mode': 'custom'
+            }
+        }
+        :return: Response from the API
+        """
+        data = {
+            "indexing_technique": "high_quality",
+            "process_rule": {"mode": "automatic"},
+            "name": name,
+            "text": text,
+        }
+        if extra_params is not None and isinstance(extra_params, dict):
+            data.update(extra_params)
+        url = f"/datasets/{self._get_dataset_id()}/document/create_by_text"
+        return self._send_request("POST", url, json=data, **kwargs)
+
+    def update_document_by_text(
+        self, document_id, name, text, extra_params: dict | None = None, **kwargs
+    ):
+        """
+        Update a document by text.
+
+        :param document_id: ID of the document
+        :param name: Name of the document
+        :param text: Text content of the document
+        :param extra_params: extra parameters pass to the API, such as indexing_technique, process_rule. (optional)
+            e.g.
+            {
+            'indexing_technique': 'high_quality',
+            'process_rule': {
+                'rules': {
+                    'pre_processing_rules': [
+                        {'id': 'remove_extra_spaces', 'enabled': True},
+                        {'id': 'remove_urls_emails', 'enabled': True}
+                    ],
+                    'segmentation': {
+                        'separator': '\n',
+                        'max_tokens': 500
+                    }
+                },
+                'mode': 'custom'
+            }
+        }
+        :return: Response from the API
+        """
+        data = {"name": name, "text": text}
+        if extra_params is not None and isinstance(extra_params, dict):
+            data.update(extra_params)
+        url = (
+            f"/datasets/{self._get_dataset_id()}/documents/{document_id}/update_by_text"
+        )
+        return self._send_request("POST", url, json=data, **kwargs)
+
+    def create_document_by_file(
+        self, file_path, original_document_id=None, extra_params: dict | None = None
+    ):
+        """
+        Create a document by file.
+
+        :param file_path: Path to the file
+        :param original_document_id: pass this ID if you want to replace the original document (optional)
+        :param extra_params: extra parameters pass to the API, such as indexing_technique, process_rule. (optional)
+            e.g.
+            {
+            'indexing_technique': 'high_quality',
+            'process_rule': {
+                'rules': {
+                    'pre_processing_rules': [
+                        {'id': 'remove_extra_spaces', 'enabled': True},
+                        {'id': 'remove_urls_emails', 'enabled': True}
+                    ],
+                    'segmentation': {
+                        'separator': '\n',
+                        'max_tokens': 500
+                    }
+                },
+                'mode': 'custom'
+            }
+        }
+        :return: Response from the API
+        """
+        files = {"file": open(file_path, "rb")}
+        data = {
+            "process_rule": {"mode": "automatic"},
+            "indexing_technique": "high_quality",
+        }
+        if extra_params is not None and isinstance(extra_params, dict):
+            data.update(extra_params)
+        if original_document_id is not None:
+            data["original_document_id"] = original_document_id
+        url = f"/datasets/{self._get_dataset_id()}/document/create_by_file"
+        return self._send_request_with_files(
+            "POST", url, {"data": json.dumps(data)}, files
+        )
+
+    def update_document_by_file(
+        self, document_id, file_path, extra_params: dict | None = None
+    ):
+        """
+        Update a document by file.
+
+        :param document_id: ID of the document
+        :param file_path: Path to the file
+        :param extra_params: extra parameters pass to the API, such as indexing_technique, process_rule. (optional)
+            e.g.
+            {
+            'indexing_technique': 'high_quality',
+            'process_rule': {
+                'rules': {
+                    'pre_processing_rules': [
+                        {'id': 'remove_extra_spaces', 'enabled': True},
+                        {'id': 'remove_urls_emails', 'enabled': True}
+                    ],
+                    'segmentation': {
+                        'separator': '\n',
+                        'max_tokens': 500
+                    }
+                },
+                'mode': 'custom'
+            }
+        }
+        :return:
+        """
+        files = {"file": open(file_path, "rb")}
+        data = {}
+        if extra_params is not None and isinstance(extra_params, dict):
+            data.update(extra_params)
+        url = (
+            f"/datasets/{self._get_dataset_id()}/documents/{document_id}/update_by_file"
+        )
+        return self._send_request_with_files(
+            "POST", url, {"data": json.dumps(data)}, files
+        )
+
+    def batch_indexing_status(self, batch_id: str, **kwargs):
+        """
+        Get the status of the batch indexing.
+
+        :param batch_id: ID of the batch uploading
+        :return: Response from the API
+        """
+        url = f"/datasets/{self._get_dataset_id()}/documents/{batch_id}/indexing-status"
+        return self._send_request("GET", url, **kwargs)
+
+    def delete_dataset(self):
+        """
+        Delete this dataset.
+
+        :return: Response from the API
+        """
+        url = f"/datasets/{self._get_dataset_id()}"
+        return self._send_request("DELETE", url)
+
+    def delete_document(self, document_id):
+        """
+        Delete a document.
+
+        :param document_id: ID of the document
+        :return: Response from the API
+        """
+        url = f"/datasets/{self._get_dataset_id()}/documents/{document_id}"
+        return self._send_request("DELETE", url)
+
+    def list_documents(
+        self,
+        page: int | None = None,
+        page_size: int | None = None,
+        keyword: str | None = None,
+        **kwargs,
+    ):
+        """
+        Get a list of documents in this dataset.
+
+        :return: Response from the API
+        """
+        params = {}
+        if page is not None:
+            params["page"] = page
+        if page_size is not None:
+            params["limit"] = page_size
+        if keyword is not None:
+            params["keyword"] = keyword
+        url = f"/datasets/{self._get_dataset_id()}/documents"
+        return self._send_request("GET", url, params=params, **kwargs)
+
+    def add_segments(self, document_id, segments, **kwargs):
+        """
+        Add segments to a document.
+
+        :param document_id: ID of the document
+        :param segments: List of segments to add, example: [{"content": "1", "answer": "1", "keyword": ["a"]}]
+        :return: Response from the API
+        """
+        data = {"segments": segments}
+        url = f"/datasets/{self._get_dataset_id()}/documents/{document_id}/segments"
+        return self._send_request("POST", url, json=data, **kwargs)
+
+    def query_segments(
+        self,
+        document_id,
+        keyword: str | None = None,
+        status: str | None = None,
+        **kwargs,
+    ):
+        """
+        Query segments in this document.
+
+        :param document_id: ID of the document
+        :param keyword: query keyword, optional
+        :param status: status of the segment, optional, e.g. completed
+        """
+        url = f"/datasets/{self._get_dataset_id()}/documents/{document_id}/segments"
+        params = {}
+        if keyword is not None:
+            params["keyword"] = keyword
+        if status is not None:
+            params["status"] = status
+        if "params" in kwargs:
+            params.update(kwargs["params"])
+        return self._send_request("GET", url, params=params, **kwargs)
+
+    def delete_document_segment(self, document_id, segment_id):
+        """
+        Delete a segment from a document.
+
+        :param document_id: ID of the document
+        :param segment_id: ID of the segment
+        :return: Response from the API
+        """
+        url = f"/datasets/{self._get_dataset_id()}/documents/{document_id}/segments/{segment_id}"
+        return self._send_request("DELETE", url)
+
+    def update_document_segment(self, document_id, segment_id, segment_data, **kwargs):
+        """
+        Update a segment in a document.
+
+        :param document_id: ID of the document
+        :param segment_id: ID of the segment
+        :param segment_data: Data of the segment, example: {"content": "1", "answer": "1", "keyword": ["a"], "enabled": True}
+        :return: Response from the API
+        """
+        data = {"segment": segment_data}
+        url = f"/datasets/{self._get_dataset_id()}/documents/{document_id}/segments/{segment_id}"
+        return self._send_request("POST", url, json=data, **kwargs)

brain/PlannerNode2/largemodel/utils/large_model_interface.py

@@ -0,0 +1,1012 @@
+from dashscope import Application
+import dashscope
+from openai import OpenAI
+import os
+import piper
+import wave
+from http import HTTPStatus
+from dashscope.audio.asr import Recognition
+from funasr import AutoModel
+from dashscope.audio.tts_v2 import *
+from dashscope.audio.asr import *
+from ament_index_python.packages import get_package_share_directory
+from dify_client2 import CompletionClient, ChatClient
+from promot import get_prompt, get_large_model_config, get_model_paths, get_system_config
+import yaml
+import base64
+import requests
+import json
+import netifaces
+from urllib.request import urlopen
+from urllib.request import Request
+from urllib.error import URLError
+from urllib.parse import urlencode
+from urllib.parse import quote_plus
+import websocket
+import datetime
+import hashlib
+import base64
+import hmac
+from urllib.parse import urlencode
+import time
+import ssl
+from wsgiref.handlers import format_date_time
+from datetime import datetime
+from time import mktime
+import _thread as thread
+from subprocess import Popen
+import functools
+def measure_execution_time(func):
+    """
+    装饰器：测量函数执行时间并使用 ROS 日志打印结果
+    """
+    @functools.wraps(func)
+    def wrapper(self, *args, **kwargs):
+        start_time = time.time()
+        result = func(self, *args, **kwargs)
+        end_time = time.time()
+        execution_time = end_time - start_time
+        
+        # 使用 ROS 日志系统记录执行时间
+        if hasattr(self, 'get_logger'):
+            self.get_logger().info(f"[性能统计] {func.__name__} 函数执行时间: {execution_time:.4f} 秒")
+        else:
+            print(f"[性能统计] {func.__name__} 函数执行时间: {execution_time:.4f} 秒")
+        return result
+    return wrapper
+
+xufei = ""
+Ws_Param = ""
+
+STATUS_FIRST_FRAME = 0  # 第一帧的标识
+STATUS_CONTINUE_FRAME = 1  # 中间帧标识
+STATUS_LAST_FRAME = 2  # 最后一帧的标识
+record_speech_file = os.path.join(
+    get_package_share_directory("largemodel"), "resources_file", "user_speech.wav"
+)
+
+
+class Ws_Param(object):
+    # 初始化
+    def __init__(self, APPID, APIKey, APISecret, AudioFile):
+
+        self.APPID = APPID
+        self.APIKey = APIKey
+        self.APISecret = APISecret
+        self.AudioFile = AudioFile
+
+        # 公共参数(common)
+        self.CommonArgs = {"app_id": self.APPID}
+        # 业务参数(business)，更多个性化参数可在官网查看
+        self.BusinessArgs = {
+            "domain": "iat",
+            "language": "en_us",
+            "accent": "mandarin",
+            "vinfo": 1,
+            "vad_eos": 10000,
+        }
+
+    # 生成url
+    def create_url(self):
+        url = "wss://ws-api.xfyun.cn/v2/iat"
+        # 生成RFC1123格式的时间戳
+        now = datetime.now()
+        date = format_date_time(mktime(now.timetuple()))
+
+        # 拼接字符串
+        signature_origin = "host: " + "ws-api.xfyun.cn" + "\n"
+        signature_origin += "date: " + date + "\n"
+        signature_origin += "GET " + "/v2/iat " + "HTTP/1.1"
+        # 进行hmac-sha256进行加密
+        signature_sha = hmac.new(
+            self.APISecret.encode("utf-8"),
+            signature_origin.encode("utf-8"),
+            digestmod=hashlib.sha256,
+        ).digest()
+        signature_sha = base64.b64encode(signature_sha).decode(encoding="utf-8")
+
+        authorization_origin = (
+            'api_key="%s", algorithm="%s", headers="%s", signature="%s"'
+            % (self.APIKey, "hmac-sha256", "host date request-line", signature_sha)
+        )
+        authorization = base64.b64encode(authorization_origin.encode("utf-8")).decode(
+            encoding="utf-8"
+        )
+        # 将请求的鉴权参数组合为字典
+        v = {"authorization": authorization, "date": date, "host": "ws-api.xfyun.cn"}
+        # 拼接鉴权参数，生成url
+        url = url + "?" + urlencode(v)
+        return url
+
+
+# 收到websocket消息的处理
+def on_message(ws, message):
+
+    try:
+        code = json.loads(message)["code"]
+        sid = json.loads(message)["sid"]
+        if code != 0:
+            errMsg = json.loads(message)["message"]
+            # print("sid:%s call error:%s code is:%s" % (sid, errMsg, code))
+        else:
+            data = json.loads(message)["data"]["result"]["ws"]
+
+            result = ""
+            for i in data:
+                for w in i["cw"]:
+                    result += w["w"]
+
+            global xufei
+            xufei += result
+
+    except Exception as e:
+        print("receive msg,but parse exception:", e)
+
+
+# 收到websocket错误的处理
+def on_error(ws, error):
+    print("### error:", error)
+
+
+# 收到websocket关闭的处理
+def on_close(ws, a, b):
+    # print("###speak iat closed ###")
+    return
+
+
+# 收到websocket连接建立的处理
+def on_open(ws):
+    def run(*args):
+        frameSize = 8000  # 每一帧的音频大小
+        intervel = 0.04  # 发送音频间隔(单位:s)
+        status = (
+            STATUS_FIRST_FRAME  # 音频的状态信息，标识音频是第一帧，还是中间帧、最后一帧
+        )
+
+        with open(wsParam.AudioFile, "rb") as fp:
+            while True:
+                buf = fp.read(frameSize)
+                # 文件结束
+                if not buf:
+                    status = STATUS_LAST_FRAME
+                # 第一帧处理
+                # 发送第一帧音频，带business 参数
+                # appid 必须带上，只需第一帧发送
+                if status == STATUS_FIRST_FRAME:
+
+                    d = {
+                        "common": wsParam.CommonArgs,
+                        "business": wsParam.BusinessArgs,
+                        "data": {
+                            "status": 0,
+                            "format": "audio/L16;rate=16000",
+                            "audio": str(base64.b64encode(buf), "utf-8"),
+                            "encoding": "raw",
+                        },
+                    }
+                    d = json.dumps(d)
+                    ws.send(d)
+                    status = STATUS_CONTINUE_FRAME
+                # 中间帧处理
+                elif status == STATUS_CONTINUE_FRAME:
+                    d = {
+                        "data": {
+                            "status": 1,
+                            "format": "audio/L16;rate=16000",
+                            "audio": str(base64.b64encode(buf), "utf-8"),
+                            "encoding": "raw",
+                        }
+                    }
+                    ws.send(json.dumps(d))
+                # 最后一帧处理
+                elif status == STATUS_LAST_FRAME:
+                    d = {
+                        "data": {
+                            "status": 2,
+                            "format": "audio/L16;rate=16000",
+                            "audio": str(base64.b64encode(buf), "utf-8"),
+                            "encoding": "raw",
+                        }
+                    }
+                    ws.send(json.dumps(d))
+                    time.sleep(1)
+                    break
+                # 模拟音频采样间隔
+                time.sleep(intervel)
+        ws.close()
+
+    thread.start_new_thread(run, ())
+
+
+wsParam = ""
+XUNFEI_TTS_FILE = os.path.join(
+    get_package_share_directory("largemodel"), "resources_file", "XUNFEI_TTS.mp3"
+)
+
+
+class Ws_Param_1(object):
+    # 初始化 initialization
+    def __init__(self, APPID, APIKey, APISecret, Text):
+        self.APPID = APPID
+        self.APIKey = APIKey
+        self.APISecret = APISecret
+        self.Text = Text
+
+        # 公共参数(common)
+        self.CommonArgs = {"app_id": self.APPID}
+        # 业务参数(business)，更多个性化参数可在官网查看
+        self.BusinessArgs = {
+            "aue": "lame",
+            "sfl": 1,
+            "auf": "audio/L16;rate=16000",
+            "vcn": "x4_xiaoyan",
+            "tte": "utf8",
+            "speed": 50,
+            "pitch": 50,
+        }
+        self.Data = {
+            "status": 2,
+            "text": str(base64.b64encode(self.Text.encode("utf-8")), "UTF8"),
+        }
+        # 使用小语种须使用以下方式，此处的unicode指的是 utf16小端的编码方式，即"UTF-16LE"”
+        # self.Data = {"status": 2, "text": str(base64.b64encode(self.Text.encode('utf-16')), "UTF8")}
+
+    # 生成url Generate URL
+    def create_url_1(self):
+        url = "wss://tts-api.xfyun.cn/v2/tts"
+        # 生成RFC1123格式的时间戳 Generate timestamp in RFC1123 format
+        now = datetime.now()
+        date = format_date_time(mktime(now.timetuple()))
+
+        # 拼接字符串 Splicing strings
+        signature_origin = "host: " + "ws-api.xfyun.cn" + "\n"
+        signature_origin += "date: " + date + "\n"
+        signature_origin += "GET " + "/v2/tts " + "HTTP/1.1"
+        # 进行hmac-sha256进行加密 Encrypt hmac-sha256
+        signature_sha = hmac.new(
+            self.APISecret.encode("utf-8"),
+            signature_origin.encode("utf-8"),
+            digestmod=hashlib.sha256,
+        ).digest()
+        signature_sha = base64.b64encode(signature_sha).decode(encoding="utf-8")
+
+        authorization_origin = (
+            'api_key="%s", algorithm="%s", headers="%s", signature="%s"'
+            % (self.APIKey, "hmac-sha256", "host date request-line", signature_sha)
+        )
+        authorization = base64.b64encode(authorization_origin.encode("utf-8")).decode(
+            encoding="utf-8"
+        )
+        # 将请求的鉴权参数组合为字典 Combine the requested authentication parameters into a dictionary
+        v = {"authorization": authorization, "date": date, "host": "ws-api.xfyun.cn"}
+        # 拼接鉴权参数，生成url Splicing authentication parameters and generating URLs
+        url = url + "?" + urlencode(v)
+        return url
+
+
+def on_message_1(ws, message):
+    try:
+        message = json.loads(message)
+        code = message["code"]
+        sid = message["sid"]
+        audio = message["data"]["audio"]
+        audio = base64.b64decode(audio)
+        status = message["data"]["status"]
+        # print(message)
+        if status == 2:
+            # print("ws is closed")
+            ws.close()
+        if code != 0:
+            errMsg = message["message"]
+            print("sid:%s call error:%s code is:%s" % (sid, errMsg, code))
+        else:
+            with open(XUNFEI_TTS_FILE, "ab") as f:
+                f.write(audio)
+    except Exception as e:
+        print("receive msg,but parse exception:", e)
+
+
+# 收到websocket错误的处理 Handling of websocket errors received
+def on_error_1(ws, error):
+    print("### error:", error)
+
+
+def on_close_1(ws, close_status_code, close_msg):
+    return
+
+
+# 收到websocket连接建立的处理 Received processing for establishing websocket connection
+def on_open_1(ws):
+    def run(*args):
+        d = {
+            "common": wsParam.CommonArgs,
+            "business": wsParam.BusinessArgs,
+            "data": wsParam.Data,
+        }
+        d = json.dumps(d)
+        # print("------>开始发送文本数据")
+        ws.send(d)
+        if os.path.exists(XUNFEI_TTS_FILE):
+            os.remove(XUNFEI_TTS_FILE)
+
+    thread.start_new_thread(run, ())
+
+
+class model_interface:
+    def __init__(self, logger=None):
+        self.logger = logger  # 可选的 logger 用于打印调试信息
+        self.init_config_param()
+        dashscope.api_key = self.tongyi_api_key
+
+    def init_config_param(self):
+        self.pkg_path = get_package_share_directory("largemodel")
+        config_param_file = os.path.join(
+            self.pkg_path, "config", "large_model_interface.yaml"
+        )
+        with open(config_param_file, "r") as file:
+            config_param = yaml.safe_load(file)
+        self.tongyi_api_key = config_param.get("tongyi_api_key")
+        self.tongyi_base_url = config_param.get("tongyi_base_url")
+        self.tongyi_app_id = config_param.get("tongyi_app_id")
+        self.oline_asr_model = config_param.get("oline_asr_model")
+        self.zh_tts_model = config_param.get("zh_tts_model")
+        self.zh_tts_json = config_param.get("zh_tts_json")
+        self.en_tts_model = config_param.get("en_tts_model")
+        self.en_tts_json = config_param.get("en_tts_json")
+        self.multimodel = config_param.get("multimodel")
+        self.ANYTHINGLLM_BASE_URL = config_param.get("ANYTHINGLLM_BASE_URL")
+        self.API_KEY = config_param.get("API_KEY")
+        self.WORKSPACE_SLUG = config_param.get("WORKSPACE_SLUG")
+        self.oline_asr_sample_rate = config_param.get("oline_asr_sample_rate")
+        self.oline_tts_model = config_param.get("oline_tts_model")
+        self.voice_tone = config_param.get("voice_tone")
+        self.local_asr_model = config_param.get("local_asr_model")
+        self.tts_supplier = config_param.get("tts_supplier")
+        self.baidu_API_KEY = config_param.get("baidu_API_KEY")
+        self.baidu_SECRET_KEY = config_param.get("baidu_SECRET_KEY")
+        self.CUID = config_param.get("CUID")
+        self.PER = config_param.get("PER")
+        self.SPD = config_param.get("SPD")
+        self.PIT = config_param.get("PIT")
+        self.VOL = config_param.get("VOL")
+        self.decision_AI_api_key = config_param.get("decision_AI_api_key")
+        self.execution_AI_api_key = config_param.get("execution_AI_api_key")
+        self.network_adapter = config_param.get("network_adapter")
+
+        self.decision_id = None  # dify决策层id
+        self.execution_id = None  # dify执行层id
+        self.international_mode = False  # 是否启用国际模式，默认为国内模式
+
+        # 从缓存更新配置（如果缓存中有配置的话）
+        self.update_config_from_cache()
+
+    def update_config_from_cache(self):
+        """从缓存更新配置（从 config_node 订阅获取）"""
+        # 获取大模型配置缓存
+        config = get_large_model_config()
+        if config:
+            if config.get('tongyi_api_key'):
+                self.tongyi_api_key = config.get('tongyi_api_key')
+                dashscope.api_key = self.tongyi_api_key
+            if config.get('tongyi_base_url'):
+                self.tongyi_base_url = config.get('tongyi_base_url')
+            if config.get('tongyi_app_id'):
+                self.tongyi_app_id = config.get('tongyi_app_id')
+            if config.get('multimodel'):
+                self.multimodel = config.get('multimodel')
+            if config.get('oline_asr_model'):
+                self.oline_asr_model = config.get('oline_asr_model')
+            if config.get('oline_asr_sample_rate'):
+                self.oline_asr_sample_rate = config.get('oline_asr_sample_rate')
+            if config.get('oline_tts_model'):
+                self.oline_tts_model = config.get('oline_tts_model')
+            if config.get('voice_tone'):
+                self.voice_tone = config.get('voice_tone')
+            if config.get('tts_supplier'):
+                self.tts_supplier = config.get('tts_supplier')
+            if config.get('baidu_API_KEY'):
+                self.baidu_API_KEY = config.get('baidu_API_KEY')
+            if config.get('baidu_SECRET_KEY'):
+                self.baidu_SECRET_KEY = config.get('baidu_SECRET_KEY')
+            if config.get('CUID'):
+                self.CUID = config.get('CUID')
+            if config.get('PER'):
+                self.PER = config.get('PER')
+            if config.get('SPD'):
+                self.SPD = config.get('SPD')
+            if config.get('PIT'):
+                self.PIT = config.get('PIT')
+            if config.get('VOL'):
+                self.VOL = config.get('VOL')
+            if config.get('decision_AI_api_key'):
+                self.decision_AI_api_key = config.get('decision_AI_api_key')
+            if config.get('execution_AI_api_key'):
+                self.execution_AI_api_key = config.get('execution_AI_api_key')
+            if config.get('network_adapter'):
+                self.network_adapter = config.get('network_adapter')
+
+        # 获取模型路径缓存
+        paths = get_model_paths()
+        if paths:
+            if paths.get('zh_tts_model'):
+                self.zh_tts_model = paths.get('zh_tts_model')
+            if paths.get('zh_tts_json'):
+                self.zh_tts_json = paths.get('zh_tts_json')
+            if paths.get('en_tts_model'):
+                self.en_tts_model = paths.get('en_tts_model')
+            if paths.get('en_tts_json'):
+                self.en_tts_json = paths.get('en_tts_json')
+            if paths.get('local_asr_model'):
+                self.local_asr_model = paths.get('local_asr_model')
+
+        # 获取系统配置缓存
+        system = get_system_config()
+        if system:
+            if system.get('tongyi_base_url'):
+                self.tongyi_base_url = system.get('tongyi_base_url')
+
+    def update_config(self, config):
+        """
+        动态更新配置（供外部调用）
+        当 config_node 发布新配置时会调用此方法
+        """
+        if config.get('tongyi_api_key'):
+            self.tongyi_api_key = config.get('tongyi_api_key')
+            dashscope.api_key = self.tongyi_api_key
+        if config.get('tongyi_base_url'):
+            self.tongyi_base_url = config.get('tongyi_base_url')
+        if config.get('tongyi_app_id'):
+            self.tongyi_app_id = config.get('tongyi_app_id')
+        if config.get('multimodel'):
+            self.multimodel = config.get('multimodel')
+        if config.get('oline_asr_model'):
+            self.oline_asr_model = config.get('oline_asr_model')
+        if config.get('oline_tts_model'):
+            self.oline_tts_model = config.get('oline_tts_model')
+        if config.get('voice_tone'):
+            self.voice_tone = config.get('voice_tone')
+        if config.get('tts_supplier'):
+            self.tts_supplier = config.get('tts_supplier')
+        if config.get('baidu_API_KEY'):
+            self.baidu_API_KEY = config.get('baidu_API_KEY')
+        if config.get('baidu_SECRET_KEY'):
+            self.baidu_SECRET_KEY = config.get('baidu_SECRET_KEY')
+        if config.get('CUID'):
+            self.CUID = config.get('CUID')
+        if config.get('PER'):
+            self.PER = config.get('PER')
+        if config.get('SPD'):
+            self.SPD = config.get('SPD')
+        if config.get('PIT'):
+            self.PIT = config.get('PIT')
+        if config.get('VOL'):
+            self.VOL = config.get('VOL')
+        if config.get('decision_AI_api_key'):
+            self.decision_AI_api_key = config.get('decision_AI_api_key')
+        if config.get('execution_AI_api_key'):
+            self.execution_AI_api_key = config.get('execution_AI_api_key')
+        if config.get('network_adapter'):
+            self.network_adapter = config.get('network_adapter')
+
+    def init_dify_client(self):
+        self.international_mode = True
+        self.user = "yahboom"
+        self.decision_client = ChatClient(
+            self.decision_AI_api_key, base_url="http://localhost/v1"
+        )
+        self.execution_client = ChatClient(
+            self.execution_AI_api_key, base_url="http://localhost/v1"
+        )
+        if self.decision_client is not None:
+            return True
+        else:
+            return False
+
+    def init_Multimodal(self):
+        self.multimodal_client = OpenAI(
+            api_key=self.tongyi_api_key, base_url=self.tongyi_base_url
+        )
+        self.init_Multimodal_history(get_prompt())
+
+    def init_Multimodal_history(self, system_prompt):
+        self.Multimodalmessages = []
+        self.Multimodalmessages.append(
+            {"role": "user", "content": [{"type": "text", "text": system_prompt}]}
+        )
+        self.Multimodalmessages.append(
+            {
+                "role": "assistant",
+                "content": [
+                    {
+                        "type": "text",
+                        "text": "我已经记住所有规则、动作函数和案例了，请开始您的指令吧",
+                    }
+                ],
+            }
+        )
+
+    def init_oline_asr(self, language):
+        self.language = language
+        return self.oline_asr_model
+
+    def multimodalinfer(self, prompt, image_path=None):
+        """version: 2.0
+        通用多模态接口，适用于通义千问平台的多模态模型
+        """
+        if image_path:
+            image_data = self.encode_image(image_path)
+            conversation_entry = {
+                "role": "user",
+                "content": [
+                    {
+                        "type": "image_url",
+                        "image_url": {"url": f"data:image/png;base64,{image_data}"},
+                    },
+                    {"type": "text", "text": "机器人反馈:执行seewhat()完成"},
+                ],
+            }
+        else:
+            conversation_entry = {
+                "role": "user",
+                "content": [{"type": "text", "text": prompt}],
+            }
+
+        self.Multimodalmessages.append(conversation_entry)
+
+        completion = self.multimodal_client.chat.completions.create(
+            model=self.multimodel, messages=self.Multimodalmessages
+        )
+
+        self.Multimodalmessages.append(
+            {
+                "role": "assistant",
+                "content": [
+                    {"type": "text", "text": completion.choices[0].message.content}
+                ],
+            }
+        )
+
+        return completion.choices[0].message.content
+
+    def TaskDecision(self, input: str) -> list:  # 任务决策规划
+        """
+        决策层模型接口
+        input: 用户输入
+        """
+        if self.international_mode:  # 国际版，调用本地dify应用API
+            try:
+                # 打印发送给 Dify 决策层的请求信息
+                if self.logger:
+                    self.logger.info(f"[决策层-Dify] 发送请求: query={input}")
+                chat_response = self.decision_client.create_chat_message(
+                    inputs={},
+                    query=input,
+                    user=self.user,
+                    response_mode="blocking",
+                )
+                chat_response.raise_for_status()
+                result = chat_response.json()
+                # 打印 Dify 返回结果
+                if self.logger:
+                    self.logger.info(f"[决策层-Dify] 返回结果: {result}")
+                if result.get("answer") is not None:
+                    output = [True, result.get("answer"), result.get("conversation_id")]
+                else:
+                    output = [
+                        False,
+                        "The model service is abnormal. Check the large model account or configuration options",
+                        None,
+                    ]
+            except Exception as e:
+                if self.logger:
+                    self.logger.error(f"[决策层-Dify] 调用异常: {e}")
+                output = [
+                    False,
+                    "The model service is abnormal. Check the large model account or configuration options",
+                    None,
+                ]
+
+        else:  # 国内版,调用百炼大模型平台应用API
+            try:
+                # 打印发送给百炼的请求信息
+                if self.logger:
+                    self.logger.info(f"[决策层-百炼] 发送请求:")
+                    self.logger.info(f"  - api_key: {self.tongyi_api_key[:10]}...")
+                    self.logger.info(f"  - app_id: {self.tongyi_app_id}")
+                    self.logger.info(f"  - prompt: {input}")
+
+                response = Application.call(
+                    api_key=self.tongyi_api_key, app_id=self.tongyi_app_id, prompt=input
+                )
+                
+                # 打印百炼返回结果
+                if self.logger:
+                    self.logger.info(f"[决策层-百炼] 返回结果: {response}")
+                    if hasattr(response, 'output') and response.output:
+                        self.logger.info(f"[决策层-百炼] output.text: {response.output.text}")
+                    if hasattr(response, 'usage'):
+                        self.logger.info(f"[决策层-百炼] usage: {response.usage}")
+                    if hasattr(response, 'request_id'):
+                        self.logger.info(f"[决策层-百炼] request_id: {response.request_id}")
+
+                if response.output.text is not None:
+                    output = [True, response.output.text, None]
+                else:
+                    output = [
+                        False,
+                        "The model service is abnormal. Check the large model account or configuration options",
+                        None,
+                    ]
+            except Exception as e:
+                if self.logger:
+                    self.logger.error(f"[决策层-百炼] 调用异常: {e}")
+                output = [
+                    False,
+                    "The model service is abnormal. Check the large model account or configuration options",
+                    None,
+                ]
+
+        return output
+
+    def TaskExecution(
+        self,
+        input: str,
+        map_mapping: str,
+        language: str,
+        image_path=None,
+        conversation_id=None,
+    ) -> list:  # 执行层模型接口
+        """
+        执行层模型接口,适用于dify
+        input: 用户输入
+        map_mapping: 地图映射
+        language: 回复语言
+        image_path: 图片路径
+        conversation_id: 会话id
+
+        return:list
+        """
+        if image_path is not None:
+
+            with open(image_path, "rb") as file:  # 上传图片
+                files = {"file": ("robot-perspective-picture", file, "image/png")}
+                response = self.execution_client.file_upload("yahboom", files)
+                file_id = response.json().get("id")
+
+            image = [
+                {
+                    "type": "image",
+                    "transfer_method": "local_file",
+                    "upload_file_id": file_id,
+                }
+            ]
+            try:
+                chat_response = self.execution_client.create_chat_message(
+                    inputs={"map_mapping": map_mapping, "language": language},
+                    query=input,
+                    user=self.user,
+                    response_mode="blocking",
+                    conversation_id=conversation_id,
+                    files=image,
+                )
+                chat_response.raise_for_status()
+                result = chat_response.json()
+                if result.get("answer") is not None:
+                    output = [True, result.get("answer"), result.get("conversation_id")]
+                else:
+                    output = [
+                        False,
+                        "The model service is abnormal. Check the large model account or configuration options",
+                        None,
+                    ]
+            except Exception as e:
+                output = [
+                    False,
+                    "The model service is abnormal. Check the large model account or configuration options",
+                    None,
+                ]
+        else:
+            try:
+                chat_response = self.execution_client.create_chat_message(
+                    inputs={"map_mapping": map_mapping, "language": language},
+                    query=input,
+                    user=self.user,
+                    response_mode="blocking",
+                    conversation_id=conversation_id,
+                )
+                chat_response.raise_for_status()
+
+                result = chat_response.json()
+                if result.get("answer") is not None:
+                    output = [True, result.get("answer"), result.get("conversation_id")]
+                else:
+                    output = [
+                        False,
+                        "The model service is abnormal. Check the large model account or configuration options",
+                        None,
+                    ]
+            except Exception as e:
+                output = [
+                    False,
+                    "The model service is abnormal. Check the large model account or configuration options",
+                    None,
+                ]
+
+        return output
+
+    def oline_asr(self, input_file):
+        """
+        语音识别接口,兼容通义千问平台paraformer、gummy系列模型
+        """
+        if self.oline_asr_model in [
+            "paraformer-realtime-v2",
+            "paraformer-realtime-v1",
+            "paraformer-realtime-8k-v2",
+            "paraformer-realtime-8k-v1",
+        ]:
+            output = self.paraformer_asr_inferce(input_file)
+            return output
+        elif self.oline_asr_model in ["gummy-realtime-v1", "gummy-chat-v1"]:
+            output = self.gummy_asr_inferce(input_file)
+            return output
+
+    def paraformer_asr_inferce(self, input_file):
+        """
+        通义千问平台paraformer模型接口
+        """
+        recognition = Recognition(
+            model=self.oline_asr_model,
+            format="wav",
+            sample_rate=self.oline_asr_sample_rate,
+            callback=None,
+        )
+        result = recognition.call(input_file)
+        if result.status_code == HTTPStatus.OK:
+            sentences = result.get_sentence()
+            if sentences and isinstance(sentences, list):
+                return ["ok", sentences[0].get("text", "")]
+            else:
+                return [
+                    "error",
+                    "ASR Error: The large model returns an empty result. Please check the account balance or parameter configuration",
+                ]
+        else:
+            return ["error", "ASR Error:" + result.message]
+
+    def gummy_asr_inferce(self, input_file):
+        """
+        通义千问平台gummy模型接口
+        """
+        translator = TranslationRecognizerRealtime(
+            model=self.oline_asr_model,
+            format="wav",
+            sample_rate=self.oline_asr_sample_rate,
+            translation_target_languages=[self.language],
+            translation_enabled=True,
+            callback=None,
+        )
+
+        result = translator.call(input_file)
+        if not result.error_message:
+            output = ""
+            for transcription_result in result.transcription_result_list:
+                output += transcription_result.text
+            return ["ok", output]
+        else:
+            return ["error", result.error_message]
+
+    def init_local_asr_model(self):
+        self.model_senceVoice = AutoModel(
+            model=self.local_asr_model, trust_remote_code=False, disable_update=True
+        )
+
+    def tts_model_init(self, model_type="oline", language="zh"):
+        if model_type == "oline":
+            if self.tts_supplier == "baidu":
+                self.token = self.fetch_token()
+            self.model_type = "oline"
+        elif model_type == "local":
+            self.model_type = "local"
+            # 初始化Piper语音合成模型
+            if language == "zh":
+                tts_model = self.zh_tts_model
+                tts_json = self.zh_tts_json
+            elif language == "en":
+
+                tts_model = self.en_tts_model
+                tts_json = self.en_tts_json
+            self.synthesizer = piper.PiperVoice.load(
+                tts_model, config_path=tts_json, use_cuda=False
+            )
+        elif model_type == "XUNFEI_FOR_INTERNATIONAL":
+            self.model_type = "XUNFEI_FOR_INTERNATIONAL"
+
+
+    def SenseVoiceSmall_ASR(self, input_file, language="zn"):
+        res = self.model_senceVoice.generate(
+            input=input_file,
+            cache={},
+            language=language,  # "zn", "en", "yue", "ja", "ko", "nospeech"
+            use_itn=False,
+        )
+        prompt = res[0]["text"].split(">")[-1]
+        return ["ok", prompt]
+    @measure_execution_time
+    def voice_synthesis(self, text, path):
+        """
+        语音合成
+        text:合成的文本
+        path:保存路径
+        返回1:失败 返回0:成功
+        """
+        if self.model_type == "oline":
+            if self.tts_supplier == "baidu":
+                """
+                百度智能云平台语音合成模型接口
+                """
+                # print('baiduhecheng')
+                TTS_URL = "http://tsn.baidu.com/text2audio"
+                tex = quote_plus(text)
+                params = {
+                    "tok": self.token,
+                    "tex": tex,
+                    "per": self.PER,
+                    "spd": self.SPD,
+                    "pit": self.PIT,
+                    "vol": self.VOL,
+                    "aue": 3,
+                    "cuid": self.CUID,
+                    "lan": "zh",
+                    "ctp": 1,
+                }  # lan ctp 固定参数
+
+                data = urlencode(params)
+                req = Request(TTS_URL, data.encode("utf-8"))
+                # has_error = False
+                try:
+                    f = urlopen(req)
+                    result_str = f.read()
+
+                    # headers = dict((name.lower(), value) for name, value in f.headers.items())
+
+                except URLError as err:
+                    print("asr http response http code : " + str(err.code))
+                    result_str = err.read()
+                    # has_error = True
+                    return 1
+                with open(path, "wb") as of:
+                    of.write(result_str)
+                    return 0
+
+            elif self.tts_supplier == "aliyun":
+                """
+                阿里通义语音合成接口
+                """
+                self.synthesizer = SpeechSynthesizer(
+                    model=self.oline_tts_model, voice=self.voice_tone, volume=100
+                )
+                audio = self.synthesizer.call(text)
+                if audio is None:
+                    return 1
+                else:
+                    with open(path, "wb") as f:
+                        f.write(audio)
+                    return 0
+        elif self.model_type == "local":
+            with wave.open(path, "wb") as wav_file:
+                wav_file.setnchannels(1)  # 单声道
+                wav_file.setsampwidth(2)  # 16位采样
+                wav_file.setframerate(self.synthesizer.config.sample_rate)  # 设置采样率
+                # 进行文本转语音
+                self.synthesizer.synthesize(text, wav_file)
+        elif self.model_type == "XUNFEI_FOR_INTERNATIONAL":
+            Xinghou_speaktts(text)
+
+
+    def openrouter_model_infer(self, prompt, image_path=None):
+        """
+        使用anythingllm连接openrouter平台大模型:已弃用
+        Connect the large model of the openrouter platform using anythingllm
+        """
+        if image_path:
+            image_data = self.encode_image(image_path)
+            data = {
+                "message": self.system_text["text1"],
+                "mode": "chat",
+                "attachments": [
+                    {
+                        "name": "image.png",
+                        "mime": "image/png",
+                        "contentString": f"data:image/png;base64,{image_data}",
+                    }
+                ],
+                "reset": False,
+            }
+        else:
+            data = {"message": prompt, "mode": "chat"}
+        # --- 发送 POST 请求 ---
+        response = requests.post(self.chat_endpoint, headers=self.headers, json=data)
+        response.raise_for_status()  # 如果请求失败 (状态码 >= 400)，则抛出异常
+        # --- 处理响应 ---
+        result = response.json()
+
+        return result["textResponse"]
+
+    def fetch_token(self):
+        """
+        专用于百度语音合成的token生成方法,百度平台有专有的token生成工具
+        """
+        TOKEN_URL = "http://aip.baidubce.com/oauth/2.0/token"
+        SCOPE = "audio_tts_post"  # 有此scope表示有tts能力，没有请在网页里勾选
+        params = {
+            "grant_type": "client_credentials",
+            "client_id": self.baidu_API_KEY,
+            "client_secret": self.baidu_SECRET_KEY,
+        }
+        post_data = urlencode(params)
+        post_data = post_data.encode("utf-8")
+        req = Request(TOKEN_URL, post_data)
+        try:
+            f = urlopen(req, timeout=5)
+            result_str = f.read()
+        except URLError as err:
+            print("token http response http code : " + str(err.code))
+            result_str = err.read()
+
+        result_str = result_str.decode()
+        result = json.loads(result_str)
+        if "access_token" in result.keys() and "scope" in result.keys():
+            return result["access_token"]
+
+    @staticmethod
+    def encode_image(image_path):
+        with open(image_path, "rb") as image_file:
+            return base64.b64encode(image_file.read()).decode("utf-8")
+
+    @staticmethod
+    def get_ip(network_interface):
+        addresses = netifaces.ifaddresses(network_interface)
+        if netifaces.AF_INET in addresses:
+            for info in addresses[netifaces.AF_INET]:
+                if "addr" in info:
+                    return info["addr"]
+
+
+# 录完音，可以直接调用去识别 After recording the audio, it can be directly called for recognition
+def rec_wav_music_en():
+    global xufei, wsParam
+    xufei = ""
+    # time1 = datetime.now()
+    wsParam = Ws_Param(
+        APPID="f12672f1",
+        APISecret="NmUyYTRmNTM2MjE3OWJkMDczYzlhZDgz",
+        APIKey="8c7b9858dc5e11e8490ce0d09879ad1e",
+        AudioFile=record_speech_file,
+    )
+    websocket.enableTrace(False)
+    wsUrl = wsParam.create_url()
+    ws = websocket.WebSocketApp(
+        wsUrl, on_message=on_message, on_error=on_error, on_close=on_close
+    )
+    ws.on_open = on_open
+    ws.run_forever(sslopt={"cert_reqs": ssl.CERT_NONE})
+
+    return xufei
+
+
+def Xinghou_speaktts(context):
+    global wsParam
+    # 测试时候在此处正确填写相关信息即可运行 Fill in the relevant information correctly here during testing to run
+    wsParam = Ws_Param_1(
+        APPID="f12672f1",
+        APISecret="NmUyYTRmNTM2MjE3OWJkMDczYzlhZDgz",
+        APIKey="8c7b9858dc5e11e8490ce0d09879ad1e",
+        Text=context,
+    )
+    websocket.enableTrace(False)
+    wsUrl = wsParam.create_url_1()
+    ws = websocket.WebSocketApp(
+        wsUrl, on_message=on_message_1, on_error=on_error_1, on_close=on_close_1
+    )
+    ws.on_open = on_open_1
+    ws.run_forever(sslopt={"cert_reqs": ssl.CERT_NONE})

brain/PlannerNode2/largemodel/utils/mic_serial.py

@@ -0,0 +1,64 @@
+import os
+import serial
+import time
+class kws_mic:
+    def __init__(self, port,kwsquence, baudrate=115200, timeout=1):
+
+        self.ser = None
+        self.port = port
+        self.baudrate = baudrate
+        self.timeout = timeout
+        self.running = False
+        self.kws_queue = kwsquence
+    def open(self):
+        try:
+            self.ser = serial.Serial(
+                port=self.port,
+                baudrate=self.baudrate,
+                timeout=self.timeout
+            )
+            if self.ser.is_open:
+                print(f"serial {self.port} open")
+                self.running = True
+        except Exception as e:
+            print(f"open serial fail: {e}")
+
+    def close(self):
+        if self.ser and self.ser.is_open:
+            self.running = False
+            self.ser.close()
+            print(f"serial {self.port} close")
+
+    def send_data(self, data):
+        if self.ser and self.ser.is_open:
+            try:
+                self.ser.write(data.encode('utf-8')) 
+            except Exception as e:
+                print(f"send fial: {e}")
+
+    def receive_data(self):
+        step = 1
+        while self.running:
+            if self.ser and self.ser.is_open:
+                try:
+                    data = self.ser.read() 
+                    if data:
+                        dealdata = bytearray(data)[0]
+                        if dealdata == 0xAA and step ==1:
+                            step = 2
+                        elif dealdata == 0x55 and step ==2:
+                            step = 3
+                        elif (dealdata == 0x01 or dealdata == 0x02 or dealdata == 0x03 or dealdata == 0x04 or dealdata == 0x05 or dealdata == 0x06) and step ==3:
+                            step = 4
+                        elif dealdata == 0x00 and step ==4:
+                            step = 5
+                        elif dealdata == 0xFB and step ==5:
+                            # print("kws detected")
+                            self.kws_queue.put("resonse_1")
+                            step = 1
+
+                except Exception as e:
+                    print(f"recvice fail: {e}")
+            time.sleep(0.1)
+
+

brain/PlannerNode2/largemodel/utils/promot.py

@@ -0,0 +1,192 @@
+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 = {}  # 系统配置缓存
+
+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 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提示语
+  '''
+  # 从缓存获取地图映射
+  map_mapping = _map_mapping_cache if _map_mapping_cache else "#地图映射\n\n(暂无地图数据)\n"
+  return default_prompt+action_function_library+map_mapping+sample_library
+
+def get_map_mapping():
+  '''
+  获取地图映射关系
+  '''
+  # 从缓存获取地图映射
+  return _map_mapping_cache if _map_mapping_cache else "#地图映射\n\n"
+
+
+
+
+

brain/PlannerNode2/nav_simulator/launch/nav_simulator.launch.py

@@ -0,0 +1,30 @@
+"""导航模拟节点启动文件"""
+
+from launch import LaunchDescription
+from launch_ros.actions import Node
+
+
+def generate_launch_description():
+    """生成 launch 描述"""
+
+    nav_simulator_node = Node(
+        package='nav_simulator',
+        executable='nav_simulator_node',
+        name='nav_simulator_node',
+        output='screen',
+        parameters=[{
+            'map_mapping_file': '/home/sunrise/opt/dev/project/aiagent/brain/PlannerNode2/largemodel/config/map_mapping.yaml',
+            'initial_x': 0.0,
+            'initial_y': 0.0,
+            'initial_z': 0.0,
+            'initial_yaw': 0.0,
+            'move_speed': 0.5,  # m/s
+            'tf_publish_rate': 10.0,  # Hz
+            'simulate_move': True,
+        }],
+        remappings=[],
+    )
+
+    return LaunchDescription([
+        nav_simulator_node,
+    ])

brain/PlannerNode2/nav_simulator/nav_simulator/nav_simulator_node.py

@@ -0,0 +1,314 @@
+#!/usr/bin/env python3
+"""
+导航模拟节点 (Nav Simulator Node)
+用于模拟 TF 变换和 Nav2 Action Server，以便在没有真实机器人/定位系统时测试导航功能。
+
+功能:
+1. 发布 TF 变换 (map → base_footprint)
+2. 提供 navigate_to_pose Action Server
+3. 模拟机器人移动过程
+4. 加载 map_mapping.yaml 中的导航点配置
+"""
+
+import rclpy
+from rclpy.node import Node
+from rclpy.action import ActionServer
+from rclpy.action.server import ServerGoalHandle
+from rclpy.callback_groups import ReentrantCallbackGroup
+
+import yaml
+import math
+import time
+import os
+
+from geometry_msgs.msg import PoseStamped, TransformStamped, Quaternion
+from nav2_msgs.action import NavigateToPose
+from tf2_ros import TransformBroadcaster
+
+
+def quaternion_from_euler(roll, pitch, yaw):
+    """从欧拉角 (roll, pitch, yaw) 计算四元数"""
+    cy = math.cos(yaw * 0.5)
+    sy = math.sin(yaw * 0.5)
+    cp = math.cos(pitch * 0.5)
+    sp = math.sin(pitch * 0.5)
+    cr = math.cos(roll * 0.5)
+    sr = math.sin(roll * 0.5)
+
+    w = cr * cp * cy + sr * sp * sy
+    x = sr * cp * cy - cr * sp * sy
+    y = cr * sp * cy + sr * cp * sy
+    z = cr * cp * sy - sr * sp * cy
+
+    return (x, y, z, w)
+
+
+class NavSimulatorNode(Node):
+    """
+    导航模拟节点
+
+    模拟:
+    - TF 变换发布 (map → base_footprint)
+    - Nav2 NavigateToPose Action Server
+    - 机器人移动过程
+    """
+
+    def __init__(self):
+        super().__init__('nav_simulator_node')
+
+        # ========== 参数声明 ==========
+        self.declare_parameter('map_mapping_file', '/home/sunrise/opt/dev/project/aiagent/brain/PlannerNode2/largemodel/config/map_mapping.yaml')
+        self.declare_parameter('initial_x', 0.0)
+        self.declare_parameter('initial_y', 0.0)
+        self.declare_parameter('initial_z', 0.0)
+        self.declare_parameter('initial_yaw', 0.0)
+        self.declare_parameter('move_speed', 0.5)  # m/s
+        self.declare_parameter('tf_publish_rate', 10.0)  # Hz
+        self.declare_parameter('simulate_move', True)  # 是否模拟移动过程
+
+        self.map_mapping_file = self.get_parameter('map_mapping_file').value
+        self.move_speed = self.get_parameter('move_speed').value
+        self.tf_publish_rate = self.get_parameter('tf_publish_rate').value
+        self.simulate_move = self.get_parameter('simulate_move').value
+
+        # ========== 状态变量 ==========
+        self.navpose_dict = {}  # 导航点字典
+        self.current_pose = {
+            'x': self.get_parameter('initial_x').value,
+            'y': self.get_parameter('initial_y').value,
+            'z': self.get_parameter('initial_z').value,
+            'yaw': self.get_parameter('initial_yaw').value,
+        }
+        self.navigation_in_progress = False
+        self.cancel_requested = False
+
+        # ========== 加载地图配置 ==========
+        self.load_map_mapping()
+
+        # ========== TF 发布者 ==========
+        self.tf_broadcaster = TransformBroadcaster(self)
+        self.tf_timer = self.create_timer(
+            1.0 / self.tf_publish_rate,
+            self.publish_tf
+        )
+
+        # ========== Nav2 Action Server ==========
+        self.action_server = ActionServer(
+            self,
+            NavigateToPose,
+            'navigate_to_pose',
+            self.execute_callback,
+            callback_group=ReentrantCallbackGroup()
+        )
+
+        self.get_logger().info('导航模拟节点已启动')
+        self.get_logger().info(f'初始位置: x={self.current_pose["x"]:.3f}, y={self.current_pose["y"]:.3f}, yaw={self.current_pose["yaw"]:.3f}')
+        self.get_logger().info(f'移动速度: {self.move_speed} m/s')
+        self.get_logger().info(f'已加载 {len(self.navpose_dict)} 个导航点')
+
+    def load_map_mapping(self):
+        """从 YAML 文件加载地图导航点配置"""
+        if not os.path.exists(self.map_mapping_file):
+            self.get_logger().warn(f'地图配置文件不存在: {self.map_mapping_file}')
+            return
+
+        try:
+            with open(self.map_mapping_file, 'r') as f:
+                target_points = yaml.safe_load(f)
+
+            for name, data in target_points.items():
+                pose = PoseStamped()
+                pose.header.frame_id = 'map'
+                pose.pose.position.x = data['position']['x']
+                pose.pose.position.y = data['position']['y']
+                pose.pose.position.z = data['position'].get('z', 0.0)
+
+                q = quaternion_from_euler(
+                    0.0,  # roll
+                    0.0,  # pitch
+                    data['orientation'].get('yaw', 0.0)
+                )
+                pose.pose.orientation.x = q[0]
+                pose.pose.orientation.y = q[1]
+                pose.pose.orientation.z = q[2]
+                pose.pose.orientation.w = q[3]
+
+                self.navpose_dict[name] = pose
+
+            self.get_logger().info(f'成功加载地图配置: {self.map_mapping_file}')
+
+        except Exception as e:
+            self.get_logger().error(f'加载地图配置失败: {e}')
+
+    def quaternion_to_yaw(self, q):
+        """从四元数提取 yaw 角"""
+        return math.atan2(
+            2.0 * (q.w * q.z + q.x * q.y),
+            1.0 - 2.0 * (q.y * q.y + q.z * q.z)
+        )
+
+    def yaw_to_quaternion(self, yaw):
+        """从 yaw 角生成四元数 (绕 Z 轴旋转)"""
+        q = quaternion_from_euler(0.0, 0.0, yaw)
+        q_msg = Quaternion()
+        q_msg.x = q[0]
+        q_msg.y = q[1]
+        q_msg.z = q[2]
+        q_msg.w = q[3]
+        return q_msg
+
+    def publish_tf(self):
+        """发布 TF 变换 (map → base_footprint)"""
+        t = TransformStamped()
+        t.header.stamp = self.get_clock().now().to_msg()
+        t.header.frame_id = 'map'
+        t.child_frame_id = 'base_footprint'
+
+        t.transform.translation.x = self.current_pose['x']
+        t.transform.translation.y = self.current_pose['y']
+        t.transform.translation.z = self.current_pose['z']
+
+        q = self.yaw_to_quaternion(self.current_pose['yaw'])
+        t.transform.rotation.x = q.x
+        t.transform.rotation.y = q.y
+        t.transform.rotation.z = q.z
+        t.transform.rotation.w = q.w
+
+        self.tf_broadcaster.sendTransform(t)
+
+    def calculate_distance(self, x1, y1, x2, y2):
+        """计算两点之间的距离"""
+        return math.sqrt((x2 - x1) ** 2 + (y2 - y1) ** 2)
+
+    def simulate_movement(self, start_x, start_y, start_yaw, target_x, target_y, target_yaw):
+        """
+        模拟机器人移动到目标位置
+
+        使用线性插值模拟移动过程:
+        1. 计算距离和角度
+        2. 按固定速度逐步更新位置
+        3. 支持取消导航
+        """
+        distance = self.calculate_distance(start_x, start_y, target_x, target_y)
+        move_time = distance / self.move_speed if self.move_speed > 0 else 0.0
+
+        self.get_logger().info(f'开始模拟移动: 距离={distance:.3f}m, 预计时间={move_time:.2f}s')
+
+        if not self.simulate_move or move_time == 0.0:
+            # 立即到达目标
+            self.current_pose['x'] = target_x
+            self.current_pose['y'] = target_y
+            self.current_pose['yaw'] = target_yaw
+            return True
+
+        # 分步移动，频率为 tf_publish_rate
+        steps = max(10, int(move_time * self.tf_publish_rate))
+        dt = move_time / steps if steps > 0 else 0.0
+
+        for i in range(steps):
+            if self.cancel_requested:
+                self.get_logger().info('导航被取消')
+                self.cancel_requested = False
+                return False
+
+            alpha = (i + 1) / steps
+            self.current_pose['x'] = start_x + (target_x - start_x) * alpha
+            self.current_pose['y'] = start_y + (target_y - start_y) * alpha
+
+            # 角度插值（考虑角度跨越问题）
+            yaw_diff = target_yaw - start_yaw
+            while yaw_diff > math.pi:
+                yaw_diff -= 2 * math.pi
+            while yaw_diff < -math.pi:
+                yaw_diff += 2 * math.pi
+            self.current_pose['yaw'] = start_yaw + yaw_diff * alpha
+
+            time.sleep(dt)
+
+        # 确保到达精确目标位置
+        self.current_pose['x'] = target_x
+        self.current_pose['y'] = target_y
+        self.current_pose['yaw'] = target_yaw
+
+        return True
+
+    async def execute_callback(self, goal_handle: ServerGoalHandle):
+        """
+        NavigateToPose Action Server 的执行回调
+
+        处理导航请求:
+        1. 接收目标点
+        2. 模拟移动过程
+        3. 返回结果
+        """
+        self.navigation_in_progress = True
+        self.cancel_requested = False
+
+        goal_pose = goal_handle.request.pose
+        target_x = goal_pose.pose.position.x
+        target_y = goal_pose.pose.position.y
+        target_z = goal_pose.pose.position.z
+        target_yaw = self.quaternion_to_yaw(goal_pose.pose.orientation)
+
+        start_x = self.current_pose['x']
+        start_y = self.current_pose['y']
+        start_yaw = self.current_pose['yaw']
+
+        self.get_logger().info(f'收到导航目标: x={target_x:.3f}, y={target_y:.3f}, yaw={target_yaw:.3f}')
+        self.get_logger().info(f'当前位置: x={start_x:.3f}, y={start_y:.3f}, yaw={start_yaw:.3f}')
+
+        # 发布反馈
+        feedback_msg = NavigateToPose.Feedback()
+        feedback_msg.current_pose = goal_pose
+        goal_handle.publish_feedback(feedback_msg)
+
+        # 模拟移动
+        success = self.simulate_movement(start_x, start_y, start_yaw, target_x, target_y, target_yaw)
+
+        self.navigation_in_progress = False
+
+        if success:
+            self.get_logger().info('导航成功完成')
+            goal_handle.succeed()
+            return NavigateToPose.Result()
+        else:
+            self.get_logger().info('导航被取消')
+            goal_handle.aborted()
+            return NavigateToPose.Result()
+
+    def set_cancel(self):
+        """请求取消当前导航"""
+        if self.navigation_in_progress:
+            self.get_logger().info('收到取消请求')
+            self.cancel_requested = True
+
+    def set_position(self, x, y, yaw):
+        """
+        手动设置机器人位置（用于测试）
+
+        Args:
+            x, y: 位置坐标
+            yaw: 朝向角 (弧度)
+        """
+        self.current_pose['x'] = x
+        self.current_pose['y'] = y
+        self.current_pose['yaw'] = yaw
+        self.get_logger().info(f'位置已更新: x={x:.3f}, y={y:.3f}, yaw={yaw:.3f}')
+
+
+def main(args=None):
+    rclpy.init(args=args)
+
+    node = NavSimulatorNode()
+
+    try:
+        rclpy.spin(node)
+    except KeyboardInterrupt:
+        pass
+    finally:
+        node.destroy_node()
+        rclpy.shutdown()
+
+
+if __name__ == '__main__':
+    main()

brain/PlannerNode2/nav_simulator/package.xml

@@ -0,0 +1,22 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>nav_simulator</name>
+  <version>0.0.0</version>
+  <description>导航模拟节点 - 模拟 TF 变换和 Nav2 Action Server 用于测试</description>
+  <maintainer email="jetson@todo.todo">jetson</maintainer>
+  <license>TODO: License declaration</license>
+
+  <build_depend>ament_python</build_depend>
+
+  <exec_depend>python3-yaml</exec_depend>
+
+  <test_depend>ament_copyright</test_depend>
+  <test_depend>ament_flake8</test_depend>
+  <test_depend>ament_pep257</test_depend>
+  <test_depend>python3-pytest</test_depend>
+
+  <export>
+    <build_type>ament_python</build_type>
+  </export>
+</package>

brain/PlannerNode2/nav_simulator/setup.cfg

@@ -0,0 +1,5 @@
+[develop]
+script_dir=$base/lib/nav_simulator
+
+[install]
+install_scripts=$base/lib/nav_simulator

brain/PlannerNode2/nav_simulator/setup.py

@@ -0,0 +1,27 @@
+from setuptools import setup
+
+package_name = 'nav_simulator'
+
+setup(
+    name=package_name,
+    version='0.0.0',
+    packages=[package_name],
+    data_files=[
+        ('share/ament_index/resource_index/packages',
+            ['resource/' + package_name]),
+        ('share/' + package_name, ['package.xml']),
+        ('share/' + package_name + '/launch', ['launch/nav_simulator.launch.py']),
+    ],
+    install_requires=['setuptools'],
+    zip_safe=True,
+    maintainer='jetson',
+    maintainer_email='jetson@todo.todo',
+    description='导航模拟节点 - 模拟 TF 变换和 Nav2 Action Server',
+    license='TODO: License declaration',
+    tests_require=['pytest'],
+    entry_points={
+        'console_scripts': [
+            'nav_simulator_node = nav_simulator.nav_simulator_node:main',
+        ],
+    },
+)

brain/llm_client.py

@@ -0,0 +1,578 @@
+"""
+llm_client.py - LLM Planner 客户端适配层
+
+该模块负责封装 LLM 调用，为 Planner 提供统一的 LLM 接口。
+
+职责边界：
+    - 接收 prompt，调用 LLM，返回 Plan JSON dict
+    - 不执行 capability
+    - 不依赖 ROS2
+    - 不做 Prompt 构造（由 prompt_manager 负责）
+
+架构说明：
+    本模块是纯 Python 接口层，不直接连接 OmniNode ROS2 节点。
+    真实的 OmniNode 通信应由 planner_node.py 完成：
+    - planner_node.py: 负责与 OmniNode ROS2 节点通信，获取 world_snapshot
+    - prompt_manager.py: 负责构造 prompt
+    - llm_client.py: 仅负责 LLM 结果处理和抽象接口层（占位）
+
+设计原则：
+    - 可替换接口：当前为占位实现，未来可替换为 Omni HTTP/SDK
+    - 异常安全：LLM 失败时返回安全的 fallback plan
+    - JSON 输出：输出必须是能被 Plan.from_dict() 解析的 dict
+
+使用方式：
+    from llm_client import LLMPlannerClient
+
+    client = LLMPlannerClient()
+    plan_json = client.generate_plan_json(
+        user_intent="降温",
+        world_snapshot={"temperature": 32},
+        available_tools=["adjust_fan", "speak"],
+        domain_rules={"high_risk_actions": ["turn_off"]},
+    )
+"""
+
+from __future__ import annotations
+
+import json
+import re
+import time
+import uuid
+from typing import Any
+
+try:
+    import yaml
+    YAML_AVAILABLE = True
+except ImportError:
+    YAML_AVAILABLE = False
+
+
+# =============================================================================
+# 异常定义
+# =============================================================================
+
+class LLMClientError(Exception):
+    """LLM 客户端基础异常"""
+    pass
+
+
+class LLMConnectionError(LLMClientError):
+    """LLM 连接异常"""
+    pass
+
+
+class LLMResponseParseError(LLMClientError):
+    """LLM 响应解析异常"""
+    pass
+
+
+class LLMTimeoutError(LLMClientError):
+    """LLM 调用超时异常"""
+    pass
+
+
+# =============================================================================
+# LLM Planner 客户端
+# =============================================================================
+
+class LLMPlannerClient:
+    """LLM Planner 客户端
+    
+    封装 LLM 调用，为 Planner 提供统一的 LLM 接口。
+    当前为占位实现，未来可替换为 Omni HTTP/SDK。
+    
+    属性：
+        api_endpoint: LLM API 端点
+        model_name: 模型名称
+        timeout: 超时时间（秒）
+        max_retries: 最大重试次数
+    
+    示例：
+        >>> client = LLMPlannerClient()
+        >>> plan_json = client.generate_plan_json(
+        ...     user_intent="降温",
+        ...     world_snapshot={"temperature": 32},
+        ...     available_tools=["adjust_fan"],
+        ... )
+    """
+    
+    # LLM 端点配置（占位，未来由配置注入）
+    DEFAULT_API_ENDPOINT = "http://localhost:8000/omni/generate"
+    
+    def __init__(
+        self,
+        api_endpoint: str | None = None,
+        model_name: str = "omni-planner-v1",
+        timeout: float = 30.0,
+        max_retries: int = 3,
+    ):
+        """初始化 LLM Planner 客户端
+        
+        Args:
+            api_endpoint: API 端点，None 则使用默认端点
+            model_name: 模型名称
+            timeout: 超时时间（秒）
+            max_retries: 最大重试次数
+        """
+        self.api_endpoint = api_endpoint or self.DEFAULT_API_ENDPOINT
+        self.model_name = model_name
+        self.timeout = timeout
+        self.max_retries = max_retries
+    
+    def generate_plan_json(
+        self,
+        user_intent: str,
+        world_snapshot: dict[str, Any],
+        available_tools: list[str],
+        domain_rules: dict[str, Any] | None = None,
+        planner_config: dict[str, Any] | None = None,
+    ) -> dict[str, Any]:
+        """生成 Plan JSON
+        
+        主要入口方法。根据用户意图和世界状态生成标准 Plan JSON。
+        
+        Args:
+            user_intent: 用户意图描述
+            world_snapshot: 世界状态快照
+            available_tools: 可用能力列表
+            domain_rules: 领域规则，None 则使用默认值
+            planner_config: Planner 配置，None 则忽略
+        
+        Returns:
+            Plan JSON dict，可被 Plan.from_dict() 解析
+        
+        Raises:
+            LLMClientError: LLM 调用或解析失败
+        """
+        domain_rules = domain_rules or {}
+        planner_config = planner_config or {}
+        
+        try:
+            # 调用模型
+            response_text = self._call_model(user_intent, world_snapshot, available_tools, domain_rules)
+            
+            # 提取 JSON
+            plan_json = self._extract_json_from_response(response_text)
+            
+            # 基本校验
+            self._validate_plan_json(plan_json)
+            
+            return plan_json
+            
+        except (LLMConnectionError, LLMResponseParseError, LLMTimeoutError):
+            # LLM 相关异常，返回 fallback
+            return self._build_safe_fallback_plan_json(
+                user_intent=user_intent,
+                reason="llm_error",
+            )
+        except Exception as e:
+            # 其他异常，记录并返回 fallback
+            return self._build_safe_fallback_plan_json(
+                user_intent=user_intent,
+                reason=f"unexpected_error: {e}",
+            )
+    
+    def _call_model(
+        self,
+        user_intent: str,
+        world_snapshot: dict[str, Any],
+        available_tools: list[str],
+        domain_rules: dict[str, Any],
+    ) -> str:
+        """调用 LLM 模型
+        
+        统一接口封装。当前为占位实现，未来替换为真实 Omni 调用。
+        
+        Args:
+            user_intent: 用户意图
+            world_snapshot: 世界状态
+            available_tools: 可用能力
+            domain_rules: 领域规则
+        
+        Returns:
+            模型返回的文本
+        """
+        # 占位实现：使用 mock 响应
+        # 未来替换为：
+        # - Omni HTTP 调用
+        # - Omni SDK 调用
+        # - 其他 LLM API 调用
+        
+        # 导入 prompt_manager 以获取 prompt
+        try:
+            from prompt_manager import PlannerPromptManager
+            prompt_manager = PlannerPromptManager()
+            prompt = prompt_manager.build_plan_prompt(
+                user_intent=user_intent,
+                world_snapshot=world_snapshot,
+                available_tools=available_tools,
+                domain_rules=domain_rules,
+            )
+        except ImportError:
+            # 如果 prompt_manager 不可用，生成简单 prompt
+            prompt = self._generate_simple_prompt(
+                user_intent, world_snapshot, available_tools, domain_rules
+            )
+        
+        # 调用实际模型（占位）
+        return self._mock_llm_call(prompt)
+    
+    def _mock_llm_call(self, prompt: str) -> str:
+        """Mock LLM 调用
+        
+        占位实现，用于测试和开发。
+        未来替换为真实 LLM 调用。
+
+        设计原则：
+        - 不包含具体业务逻辑（农业/降温/喂食等）
+        - 只根据测试标记返回不同类型的响应
+        - 保持接口占位符的角色
+
+        Args:
+            prompt: 输入 prompt
+        
+        Returns:
+            Mock 响应文本（JSON 格式）
+        """
+        # 通过测试标记区分返回类型
+        if "__TEST_INVALID_JSON__" in prompt:
+            return self._generate_invalid_json_response()
+        elif "__TEST_ASK_USER__" in prompt:
+            return self._generate_ask_user_response()
+        else:
+            return self._generate_valid_generic_response()
+
+    def _generate_valid_generic_response(self) -> str:
+        """生成通用合法 Plan JSON 响应"""
+        plan = {
+            "plan_id": f"plan_{uuid.uuid4().hex[:8]}",
+            "goal": "处理用户请求",
+            "reasoning": "根据用户意图生成执行计划",
+            "risk_level": "low",
+            "requires_confirmation": False,
+            "confirmation_message": None,
+            "steps": [
+                {
+                    "step_id": 1,
+                    "action": "query",
+                    "tool_call_type": "query_world",
+                    "parameters": {},
+                    "preconditions": {},
+                    "fallback": None,
+                    "status": "pending",
+                    "description": "查询当前状态",
+                    "requires_confirmation": False,
+                    "confirmation_message": None,
+                    "metadata": {},
+                }
+            ],
+            "status": "created",
+            "source": "llm",
+            "metadata": {},
+        }
+        return json.dumps(plan, ensure_ascii=False)
+
+    def _generate_ask_user_response(self) -> str:
+        """生成需要询问用户的 Plan JSON 响应"""
+        plan = {
+            "plan_id": f"plan_{uuid.uuid4().hex[:8]}",
+            "goal": "需要用户确认",
+            "reasoning": "该请求需要用户进一步确认",
+            "risk_level": "medium",
+            "requires_confirmation": False,
+            "confirmation_message": "请确认您的意图",
+            "steps": [
+                {
+                    "step_id": 1,
+                    "action": "ask_user",
+                    "tool_call_type": "ask_user",
+                    "parameters": {"question": "请确认您的具体需求"},
+                    "preconditions": {},
+                    "fallback": None,
+                    "status": "pending",
+                    "description": "询问用户确认",
+                    "requires_confirmation": False,
+                    "confirmation_message": None,
+                    "metadata": {},
+                }
+            ],
+            "status": "created",
+            "source": "llm",
+            "metadata": {},
+        }
+        return json.dumps(plan, ensure_ascii=False)
+
+    def _generate_invalid_json_response(self) -> str:
+        """生成无效的 JSON 响应（用于测试 fallback）"""
+        return '{"plan_id": "broken", "goal": "incomplete json"'
+    
+    def _generate_simple_prompt(
+        self,
+        user_intent: str,
+        world_snapshot: dict[str, Any],
+        available_tools: list[str],
+        domain_rules: dict[str, Any],
+    ) -> str:
+        """生成简单的 prompt（当 prompt_manager 不可用时）"""
+        return f"""用户意图: {user_intent}
+世界状态: {json.dumps(world_snapshot, ensure_ascii=False)}
+可用能力: {', '.join(available_tools)}
+领域规则: {json.dumps(domain_rules, ensure_ascii=False)}
+
+请生成标准 Plan JSON。"""
+    
+    def _extract_json_from_response(self, response_text: str) -> dict[str, Any]:
+        """从模型响应中提取 JSON
+        
+        支持以下格式：
+        - 纯 JSON
+        - JSON 前后有解释文字
+        - JSON 在 markdown 代码块中
+        
+        Args:
+            response_text: 模型响应文本
+        
+        Returns:
+            提取的 JSON dict
+        
+        Raises:
+            LLMResponseParseError: 解析失败
+        """
+        response_text = response_text.strip()
+        
+        # 尝试直接解析
+        try:
+            return json.loads(response_text)
+        except json.JSONDecodeError:
+            pass
+        
+        # 尝试从 markdown 代码块中提取
+        json_pattern = r'```(?:json)?\s*([\s\S]*?)\s*```'
+        matches = re.findall(json_pattern, response_text)
+        for match in matches:
+            try:
+                return json.loads(match.strip())
+            except json.JSONDecodeError:
+                continue
+        
+        # 尝试提取任何 {...} 块
+        brace_pattern = r'\{[\s\S]*\}'
+        matches = re.findall(brace_pattern, response_text)
+        if matches:
+            # 尝试从后向前找第一个有效 JSON
+            for i in range(len(matches) - 1, -1, -1):
+                try:
+                    return json.loads(matches[i])
+                except json.JSONDecodeError:
+                    continue
+        
+        raise LLMResponseParseError(
+            f"无法从响应中提取 JSON:\n{response_text[:500]}..."
+        )
+    
+    def _validate_plan_json(self, plan_json: dict[str, Any]) -> None:
+        """校验 Plan JSON 基本结构
+        
+        Args:
+            plan_json: Plan JSON dict
+        
+        Raises:
+            LLMResponseParseError: 校验失败
+        """
+        required_fields = ["goal", "steps"]
+        for field in required_fields:
+            if field not in plan_json:
+                raise LLMResponseParseError(f"缺少必需字段: {field}")
+        
+        if not isinstance(plan_json["steps"], list):
+            raise LLMResponseParseError("steps 必须是列表")
+        
+        # 校验每个步骤
+        for i, step in enumerate(plan_json["steps"]):
+            if "action" not in step:
+                raise LLMResponseParseError(f"步骤 {i} 缺少 action 字段")
+    
+    def _build_safe_fallback_plan_json(
+        self,
+        user_intent: str,
+        reason: str = "",
+    ) -> dict[str, Any]:
+        """构建安全的 fallback Plan JSON
+        
+        当 LLM 调用失败时，返回一个安全的 fallback plan。
+        优先使用 ASK_USER 或 SPEAK，不直接执行风险动作。
+        
+        Args:
+            user_intent: 用户原始意图
+            reason: 回退原因
+        
+        Returns:
+            安全的 Plan JSON
+        """
+        return {
+            "plan_id": f"plan_fallback_{uuid.uuid4().hex[:8]}",
+            "goal": f"无法处理的请求: {user_intent}",
+            "reasoning": f"LLM 处理失败或响应无效 ({reason})，返回安全 fallback",
+            "risk_level": "low",
+            "requires_confirmation": False,
+            "confirmation_message": None,
+            "steps": [
+                {
+                    "step_id": 1,
+                    "action": "ask_user",
+                    "tool_call_type": "ask_user",
+                    "parameters": {
+                        "question": f"抱歉，我无法理解或处理您的请求: {user_intent}。请重新描述。"
+                    },
+                    "preconditions": {},
+                    "fallback": None,
+                    "status": "pending",
+                    "description": "询问用户澄清意图",
+                    "requires_confirmation": False,
+                    "confirmation_message": None,
+                    "metadata": {"fallback_reason": reason},
+                }
+            ],
+            "status": "created",
+            "source": "llm_fallback",
+            "metadata": {"fallback": True, "reason": reason},
+        }
+
+
+# =============================================================================
+# 同步调用接口（供 Planner 使用）
+# =============================================================================
+
+def call_llm_for_plan(
+    user_intent: str,
+    world_snapshot: dict[str, Any],
+    available_tools: list[str],
+    domain_rules: dict[str, Any] | None = None,
+    planner_config: dict[str, Any] | None = None,
+    **kwargs,
+) -> dict[str, Any]:
+    """便捷函数：调用 LLM 生成 Plan JSON
+    
+    创建一个临时客户端并调用 generate_plan_json。
+    
+    Args:
+        user_intent: 用户意图
+        world_snapshot: 世界状态
+        available_tools: 可用能力
+        domain_rules: 领域规则
+        planner_config: Planner 配置
+        **kwargs: 传递给 LLMPlannerClient 的其他参数
+    
+    Returns:
+        Plan JSON dict
+    """
+    client = LLMPlannerClient(**kwargs)
+    return client.generate_plan_json(
+        user_intent=user_intent,
+        world_snapshot=world_snapshot,
+        available_tools=available_tools,
+        domain_rules=domain_rules,
+        planner_config=planner_config,
+    )
+
+
+# =============================================================================
+# 主程序入口（测试示例）
+# =============================================================================
+
+if __name__ == "__main__":
+    print("=" * 70)
+    print("LLM Planner Client 测试")
+    print("=" * 70)
+
+    client = LLMPlannerClient()
+
+    # 场景 1：通用请求（默认返回合法 Plan）
+    print("\n[场景 1] 通用请求")
+    print("-" * 40)
+    plan_json = client.generate_plan_json(
+        user_intent="打开风扇降温",
+        world_snapshot={"temperature": 32, "humidity": 70},
+        available_tools=["adjust_fan", "speak", "query"],
+        domain_rules={"high_risk_actions": ["turn_off"]},
+    )
+    print(f"Goal: {plan_json.get('goal')}")
+    print(f"Steps: {len(plan_json.get('steps', []))}")
+    print(f"Action: {plan_json['steps'][0]['action'] if plan_json.get('steps') else 'N/A'}")
+
+    # 场景 2：测试 ASK_USER 响应
+    print("\n[场景 2] 测试 ASK_USER 响应")
+    print("-" * 40)
+    plan_json = client.generate_plan_json(
+        user_intent="确认操作 __TEST_ASK_USER__",
+        world_snapshot={"temperature": 28},
+        available_tools=["feed", "speak", "query"],
+    )
+    print(f"Goal: {plan_json.get('goal')}")
+    print(f"Tool Type: {plan_json['steps'][0]['tool_call_type'] if plan_json.get('steps') else 'N/A'}")
+
+    # 场景 3：测试 Invalid JSON fallback
+    print("\n[场景 3] 测试 Invalid JSON fallback")
+    print("-" * 40)
+    plan_json = client.generate_plan_json(
+        user_intent="测试 __TEST_INVALID_JSON__",
+        world_snapshot={"temperature": 30},
+        available_tools=["adjust_fan", "speak"],
+    )
+    print(f"Goal: {plan_json.get('goal')}")
+    print(f"Source: {plan_json.get('source')}")
+    print(f"Action: {plan_json['steps'][0]['action'] if plan_json.get('steps') else 'N/A'}")
+
+    # 场景 4：测试 JSON 提取
+    print("\n[场景 4] JSON 提取测试")
+    print("-" * 40)
+    test_response = '根据您的请求，我生成以下计划：{"plan_id": "plan_test123", "goal": "测试计划", "reasoning": "这是一个测试", "risk_level": "low", "requires_confirmation": false, "confirmation_message": null, "steps": [{"step_id": 1, "action": "query", "tool_call_type": "query_world", "parameters": {}, "preconditions": {}, "fallback": null, "status": "pending", "description": "查询状态", "requires_confirmation": false, "confirmation_message": null, "metadata": {}}], "status": "created", "source": "llm", "metadata": {}}这是标准格式的 Plan。'
+    try:
+        extracted = client._extract_json_from_response(test_response)
+        print(f"成功提取 JSON: plan_id={extracted.get('plan_id')}")
+    except LLMResponseParseError as e:
+        print(f"提取失败: {e}")
+
+    # 场景 5：测试 markdown 代码块提取
+    print("\n[场景 5] Markdown 代码块提取测试")
+    print("-" * 40)
+    test_response_markdown = '''这是响应：
+```json
+{
+  "plan_id": "plan_markdown",
+  "goal": "测试 markdown",
+  "reasoning": "测试",
+  "risk_level": "low",
+  "requires_confirmation": false,
+  "confirmation_message": null,
+  "steps": [
+    {
+      "step_id": 1,
+      "action": "query",
+      "tool_call_type": "query_world",
+      "parameters": {},
+      "preconditions": {},
+      "fallback": null,
+      "status": "pending",
+      "description": "查询",
+      "requires_confirmation": false,
+      "confirmation_message": null,
+      "metadata": {}
+    }
+  ],
+  "status": "created",
+  "source": "llm",
+  "metadata": {}
+}
+```'''
+    try:
+        extracted = client._extract_json_from_response(test_response_markdown)
+        print(f"成功提取 JSON: plan_id={extracted.get('plan_id')}")
+    except LLMResponseParseError as e:
+        print(f"提取失败: {e}")
+
+    print("\n" + "=" * 70)
+    print("LLM Planner Client 测试完成")
+    print("=" * 70)
+

brain/planner.py

@@ -0,0 +1,1440 @@
+"""
+planner.py - AI Agent 通用 Planner 核心模块
+
+该模块负责根据世界状态、用户意图、可用能力生成标准化的 Plan 对象。
+
+职责边界：
+    - 接收输入：world_snapshot, user_intent, available_tools, domain_rules, planner_mode
+    - 输出：标准 Plan 对象
+    - 不执行 capability
+    - 不依赖 ROS2
+    - 不包含场景硬编码逻辑
+
+Planner 支持三种模式：
+    - rule：纯规则生成
+    - llm：LLM 生成（调用 llm_client）
+    - hybrid：优先规则，复杂情况调用 LLM
+
+使用方式：
+    from planner import Planner, PlannerConfig
+    from tool_protocol import RiskLevel
+
+    config = PlannerConfig(default_risk_level=RiskLevel.LOW)
+    planner = Planner(config)
+    plan = planner.generate_plan(
+        world_snapshot={"temperature": 35},
+        user_intent="降温",
+        available_tools=["adjust_fan", "speak"],
+    )
+    
+    # 使用 LLM 模式
+    plan = planner.generate_plan(
+        world_snapshot={"temperature": 35},
+        user_intent="降温",
+        available_tools=["adjust_fan", "speak"],
+        planner_mode="llm",
+    )
+"""
+
+from __future__ import annotations
+
+import time
+import uuid
+from dataclasses import dataclass, field
+from typing import Any
+
+from tool_protocol import (
+    Plan,
+    PlanStep,
+    RiskLevel,
+    PlanStatus,
+    StepStatus,
+    ToolCallType,
+)
+
+
+# =============================================================================
+# 通用意图映射层
+# =============================================================================
+
+# 通用意图关键词到 action 名称的映射
+# 用途：当用户意图包含某个关键词时，映射到对应的 action
+# 可扩展：后续可通过配置文件或知识库动态加载
+INTENT_TO_ACTION: dict[str, str] = {
+    # 动作类
+    "喂": "feed",
+    "喂食": "feed",
+    "喂养": "feed",
+    "打开风扇": "adjust_fan",
+    "关闭风扇": "adjust_fan",
+    "调整风扇": "adjust_fan",
+    "降温": "adjust_fan",
+    "升温": "adjust_fan",
+    "开灯": "control_light",
+    "关灯": "control_light",
+    "调整灯光": "control_light",
+    "打开": "turn_on",
+    "关闭": "turn_off",
+    "启动": "turn_on",
+    "停止": "turn_off",
+    "调节": "adjust",
+    "移动": "move",
+    "前往": "move",
+    "走到": "move",
+    # 查询类
+    "查询": "query",
+    "查看": "query",
+    "检查": "inspect",
+    "巡检": "inspect",
+    "监测": "monitor",
+    "监控": "monitor",
+    # 通信类
+    "播报": "speak",
+    "报告": "speak",
+    "说": "speak",
+    "告诉": "speak",
+    "通知": "notify",
+    "提醒": "remind",
+    "警告": "warn",
+    "询问": "ask",
+}
+
+# action 到 ToolCallType 的默认映射
+ACTION_TO_TOOL_CALL_TYPE: dict[str, ToolCallType] = {
+    "feed": ToolCallType.EXECUTE,
+    "adjust_fan": ToolCallType.EXECUTE,
+    "control_light": ToolCallType.EXECUTE,
+    "turn_on": ToolCallType.EXECUTE,
+    "turn_off": ToolCallType.EXECUTE,
+    "adjust": ToolCallType.EXECUTE,
+    "move": ToolCallType.EXECUTE,
+    "query": ToolCallType.QUERY_WORLD,
+    "inspect": ToolCallType.QUERY_WORLD,
+    "monitor": ToolCallType.QUERY_WORLD,
+    "speak": ToolCallType.SPEAK,
+    "notify": ToolCallType.SPEAK,
+    "remind": ToolCallType.SPEAK,
+    "warn": ToolCallType.SPEAK,
+    "ask": ToolCallType.ASK_USER,
+}
+
+# 高风险关键词列表（通用）
+HIGH_RISK_KEYWORDS: list[str] = [
+    "紧急", "危险", "停止", "关机", "断电", "全部", "清空",
+    "emergency", "danger", "stop", "shutdown", "all", "clear",
+]
+
+# 中风险关键词列表（通用）
+MEDIUM_RISK_KEYWORDS: list[str] = [
+    "调整", "改变", "修改", "设置", "配置",
+    "adjust", "change", "modify", "set", "configure",
+]
+
+
+# =============================================================================
+# Planner 配置
+# =============================================================================
+
+@dataclass
+class PlannerConfig:
+    """Planner 配置类
+    
+    用于配置 Planner 的行为参数。
+    
+    属性：
+        default_risk_level: 默认风险等级
+        require_confirmation_on_medium_risk: 中风险是否需要确认
+        require_confirmation_on_high_risk: 高风险是否需要确认
+        max_plan_steps: 单个 Plan 最大步骤数
+        default_source: 默认计划来源
+    
+    示例：
+        >>> config = PlannerConfig(
+        ...     default_risk_level=RiskLevel.LOW,
+        ...     require_confirmation_on_medium_risk=True,
+        ...     require_confirmation_on_high_risk=True,
+        ...     max_plan_steps=10,
+        ...     default_source="hybrid"
+        ... )
+    """
+    
+    default_risk_level: RiskLevel = RiskLevel.LOW
+    require_confirmation_on_medium_risk: bool = True
+    require_confirmation_on_high_risk: bool = True
+    max_plan_steps: int = 10
+    default_source: str = "hybrid"
+
+
+# =============================================================================
+# Planner 核心类
+# =============================================================================
+
+class Planner:
+    """通用 Planner 类
+    
+    根据世界状态、用户意图、可用能力生成标准化 Plan 对象。
+    
+    属性：
+        config: Planner 配置
+        intent_to_action: 意图到 action 的映射字典
+        action_to_tool_type: action 到 ToolCallType 的映射字典
+    
+    示例：
+        >>> planner = Planner()
+        >>> plan = planner.generate_plan(
+        ...     world_snapshot={"temperature": 30},
+        ...     user_intent="降温",
+        ...     available_tools=["adjust_fan", "speak"]
+        ... )
+    """
+    
+    def __init__(
+        self,
+        config: PlannerConfig | None = None,
+        intent_to_action: dict[str, str] | None = None,
+        action_to_tool_type: dict[str, ToolCallType] | None = None,
+    ) -> None:
+        """初始化 Planner
+        
+        Args:
+            config: Planner 配置，None 时使用默认配置
+            intent_to_action: 自定义意图映射，None 时使用内置映射
+            action_to_tool_type: 自定义 action 类型映射，None 时使用内置映射
+        """
+        self.config = config or PlannerConfig()
+        self.intent_to_action = intent_to_action or INTENT_TO_ACTION.copy()
+        self.action_to_tool_type = action_to_tool_type or ACTION_TO_TOOL_CALL_TYPE.copy()
+        self._cached_normalized_snapshot: dict[str, Any] | None = None
+        self._cached_snapshot_raw: dict[str, Any] | None = None
+    
+    def generate_plan(
+        self,
+        world_snapshot: dict[str, Any],
+        user_intent: str,
+        available_tools: list[str],
+        domain_rules: dict[str, Any] | None = None,
+        planner_mode: str = "hybrid",
+    ) -> Plan:
+        """生成执行计划
+        
+        根据输入信息生成标准化的 Plan 对象。
+        
+        Args:
+            world_snapshot: 世界状态快照
+            user_intent: 用户意图描述
+            available_tools: 可用能力列表
+            domain_rules: 领域规则约束，None 表示无约束
+            planner_mode: 规划模式，可选 "rule" / "llm" / "hybrid"
+        
+        Returns:
+            标准化的 Plan 对象
+        
+        Raises:
+            ValueError: 当 planner_mode 不合法
+        """
+        if not user_intent or not user_intent.strip():
+            return self._generate_empty_intent_plan(world_snapshot, available_tools)
+        
+        mode = planner_mode.lower()
+        if mode == "rule":
+            return self._generate_rule_based_plan(
+                world_snapshot, user_intent, available_tools, domain_rules
+            )
+        elif mode == "llm":
+            return self._generate_llm_plan(
+                world_snapshot, user_intent, available_tools, domain_rules
+            )
+        elif mode == "hybrid":
+            return self._generate_hybrid_plan(
+                world_snapshot, user_intent, available_tools, domain_rules
+            )
+        else:
+            raise ValueError(f"无效的 planner_mode: {planner_mode}，有效值为: rule/llm/hybrid")
+    
+    def _generate_rule_based_plan(
+        self,
+        world_snapshot: dict[str, Any],
+        user_intent: str,
+        available_tools: list[str],
+        domain_rules: dict[str, Any] | None,
+    ) -> Plan:
+        """基于规则生成 Plan
+        
+        内部方法，使用规则引擎生成计划。
+
+        Args:
+            world_snapshot: 世界状态快照
+            user_intent: 用户意图描述
+            available_tools: 可用能力列表
+            domain_rules: 领域规则约束
+        
+        Returns:
+            Plan 对象
+        """
+        normalized = self._normalize_world_snapshot(world_snapshot)
+        action = self._map_intent_to_action(user_intent)
+        context = self._extract_recent_context(normalized)
+
+        requires_confirm, confirm_msg = self._needs_confirmation(
+            normalized, user_intent, action, domain_rules
+        )
+        risk_level = self._assess_risk(user_intent, action, normalized, domain_rules)
+
+        plan_id = f"plan_{uuid.uuid4().hex[:8]}"
+
+        if action is None:
+            return self._generate_unknown_intent_plan(
+                plan_id, user_intent, normalized, available_tools, risk_level, context
+            )
+
+        if not self._validate_tool_available(action, available_tools):
+            return self._generate_tool_unavailable_plan(
+                plan_id, user_intent, action, normalized, risk_level, context
+            )
+
+        return self._generate_executable_plan(
+            plan_id, user_intent, action, normalized,
+            risk_level, requires_confirm, confirm_msg, context
+        )
+    
+    def _generate_llm_plan(
+        self,
+        world_snapshot: dict[str, Any],
+        user_intent: str,
+        available_tools: list[str],
+        domain_rules: dict[str, Any] | None,
+    ) -> Plan:
+        """LLM 规划器实现
+        
+        内部方法，调用 LLM 生成计划。
+
+        Args:
+            world_snapshot: 世界状态快照
+            user_intent: 用户意图描述
+            available_tools: 可用能力列表
+            domain_rules: 领域规则约束
+        
+        Returns:
+            Plan 对象
+        """
+        # 导入 LLM 客户端（延迟导入避免循环依赖）
+        try:
+            from llm_client import LLMPlannerClient
+            llm_client = LLMPlannerClient()
+        except ImportError:
+            # LLM 客户端不可用，返回安全 fallback
+            return self._generate_safe_fallback_plan(
+                user_intent=user_intent,
+                reason="llm_client_unavailable",
+            )
+
+        # 提取上下文（使用 normalize 后的结果）
+        normalized = self._normalize_world_snapshot(world_snapshot)
+        context = self._extract_recent_context(normalized)
+
+        # 构造 Planner 配置
+        planner_config = {
+            "default_risk_level": self.config.default_risk_level.value,
+            "require_confirmation_on_medium_risk": self.config.require_confirmation_on_medium_risk,
+            "require_confirmation_on_high_risk": self.config.require_confirmation_on_high_risk,
+            "max_plan_steps": self.config.max_plan_steps,
+        }
+
+        try:
+            # 调用 LLM 生成 Plan JSON
+            plan_json = llm_client.generate_plan_json(
+                user_intent=user_intent,
+                world_snapshot=normalized,
+                available_tools=available_tools,
+                domain_rules=domain_rules,
+                planner_config=planner_config,
+            )
+
+            # 使用 Plan.from_dict() 转换
+            plan = Plan.from_dict(plan_json)
+
+            return plan
+
+        except Exception as e:
+            # LLM 调用失败，返回安全 fallback
+            return self._generate_safe_fallback_plan(
+                user_intent=user_intent,
+                reason=f"llm_error: {e}",
+            )
+    
+    def _generate_hybrid_plan(
+        self,
+        world_snapshot: dict[str, Any],
+        user_intent: str,
+        available_tools: list[str],
+        domain_rules: dict[str, Any] | None,
+    ) -> Plan:
+        """混合模式生成 Plan
+        
+        内部方法，优先使用规则，简单情况直接生成，复杂情况调用 LLM。
+
+        Args:
+            world_snapshot: 世界状态快照
+            user_intent: 用户意图描述
+            available_tools: 可用能力列表
+            domain_rules: 领域规则约束
+        
+        Returns:
+            Plan 对象
+        """
+        normalized = self._normalize_world_snapshot(world_snapshot)
+        action = self._map_intent_to_action(user_intent)
+        context = self._extract_recent_context(normalized)
+
+        # 判断是否需要委托给 LLM
+        if self._should_delegate_to_llm(
+            normalized, user_intent, available_tools, domain_rules, action
+        ):
+            return self._generate_llm_plan(
+                normalized, user_intent, available_tools, domain_rules
+            )
+
+        # 规则可处理，直接生成
+        requires_confirm, confirm_msg = self._needs_confirmation(
+            normalized, user_intent, action, domain_rules
+        )
+        risk_level = self._assess_risk(user_intent, action, normalized, domain_rules)
+
+        plan_id = f"plan_{uuid.uuid4().hex[:8]}"
+
+        if action is None:
+            return self._generate_unknown_intent_plan(
+                plan_id, user_intent, normalized, available_tools, risk_level, context
+            )
+
+        if not self._validate_tool_available(action, available_tools):
+            return self._generate_tool_unavailable_plan(
+                plan_id, user_intent, action, normalized, risk_level, context
+            )
+
+        return self._generate_executable_plan(
+            plan_id, user_intent, action, normalized,
+            risk_level, requires_confirm, confirm_msg, context
+        )
+    
+    def _generate_executable_plan(
+        self,
+        plan_id: str,
+        user_intent: str,
+        action: str,
+        world_snapshot: dict[str, Any],
+        risk_level: RiskLevel,
+        requires_confirm: bool,
+        confirm_msg: str | None,
+        context: dict[str, Any],
+    ) -> Plan:
+        """生成可执行的 Plan
+        
+        内部方法，生成包含具体执行步骤的 Plan。
+
+        当 requires_confirm == True 时，生成 ASK_USER/SPEAK 询问步骤而非 EXECUTE 执行步骤，
+        以确保 Executor 不会在用户确认前误执行风险动作。
+
+        Args:
+            plan_id: 计划 ID
+            user_intent: 用户意图
+            action: 动作名称
+            world_snapshot: 世界状态（应传入 normalized 后的结果）
+            risk_level: 风险等级
+            requires_confirm: 是否需要确认
+            confirm_msg: 确认消息
+            context: 提取的上下文
+        
+        Returns:
+            可执行的 Plan 对象
+        """
+        reasoning_parts = [
+            f"用户意图: {user_intent}",
+            f"匹配动作: {action}",
+            f"风险等级: {risk_level.value}",
+        ]
+        if context:
+            reasoning_parts.append(f"相关上下文: {context}")
+        if requires_confirm and confirm_msg:
+            reasoning_parts.append(f"需要确认: {confirm_msg}")
+
+        plan = Plan(
+            plan_id=plan_id,
+            goal=user_intent,
+            reasoning="; ".join(reasoning_parts),
+            risk_level=risk_level,
+            requires_confirmation=requires_confirm,
+            confirmation_message=confirm_msg,
+            status=PlanStatus.CREATED,
+            source=self.config.default_source,
+        )
+
+        # 当需要确认时，生成询问/告知步骤而非执行步骤
+        if requires_confirm:
+            step = PlanStep(
+                step_id=1,
+                action="ask_user",
+                tool_call_type=ToolCallType.ASK_USER,
+                parameters={
+                    "user_intent": user_intent,
+                    "context": context,
+                    "suggested_action": action,
+                    "question": confirm_msg or f"您确定要执行 '{action}' 吗？",
+                },
+                description=f"询问用户确认: {action}",
+                requires_confirmation=True,
+                confirmation_message=confirm_msg,
+            )
+        else:
+            # 不需要确认时，生成正常的执行步骤
+            step = PlanStep(
+                step_id=1,
+                action=action,
+                tool_call_type=self.action_to_tool_type.get(action, ToolCallType.EXECUTE),
+                parameters={"user_intent": user_intent, "context": context},
+                description=f"执行动作: {action}",
+            )
+
+        plan.add_step(step)
+
+        return plan
+    
+    def _generate_tool_unavailable_plan(
+        self,
+        plan_id: str,
+        user_intent: str,
+        action: str,
+        world_snapshot: dict[str, Any],
+        risk_level: RiskLevel,
+        context: dict[str, Any],
+    ) -> Plan:
+        """生成工具不可用时的 Plan
+        
+        内部方法，当请求的能力不在可用列表中时调用。
+        
+        Args:
+            plan_id: 计划 ID
+            user_intent: 用户意图
+            action: 尝试的动作名称
+            world_snapshot: 世界状态
+            risk_level: 风险等级
+            context: 提取的上下文
+        
+        Returns:
+            提示能力不可用的 Plan 对象
+        """
+        plan = Plan(
+            plan_id=plan_id,
+            goal=f"处理不可用能力请求: {user_intent}",
+            reasoning=(
+                f"用户请求执行动作 '{action}'，但该能力当前不可用。"
+                f"用户意图: {user_intent}"
+            ),
+            risk_level=RiskLevel.LOW,
+            requires_confirmation=False,
+            status=PlanStatus.CREATED,
+            source=self.config.default_source,
+        )
+        
+        step = PlanStep(
+            step_id=1,
+            action="speak",
+            tool_call_type=ToolCallType.SPEAK,
+            parameters={
+                "message": f"抱歉，当前系统不支持 '{action}' 操作。"
+                           f"您的请求 '{user_intent}' 无法完成。",
+                "level": "info",
+            },
+            description=f"通知用户能力不可用: {action}",
+        )
+        plan.add_step(step)
+        
+        return plan
+    
+    def _generate_unknown_intent_plan(
+        self,
+        plan_id: str,
+        user_intent: str,
+        world_snapshot: dict[str, Any],
+        available_tools: list[str],
+        risk_level: RiskLevel,
+        context: dict[str, Any],
+    ) -> Plan:
+        """生成意图不清晰时的 Plan
+        
+        内部方法，当无法从用户意图中识别出具体动作时调用。
+        
+        Args:
+            plan_id: 计划 ID
+            user_intent: 用户意图
+            world_snapshot: 世界状态
+            available_tools: 可用能力列表
+            risk_level: 风险等级
+            context: 提取的上下文
+        
+        Returns:
+            需要进一步确认意图的 Plan 对象
+        """
+        plan = Plan(
+            plan_id=plan_id,
+            goal=f"澄清用户意图: {user_intent}",
+            reasoning=(
+                f"无法从用户意图 '{user_intent}' 中识别出具体动作。"
+                f"可用能力: {available_tools}"
+            ),
+            risk_level=RiskLevel.LOW,
+            requires_confirmation=False,
+            status=PlanStatus.CREATED,
+            source=self.config.default_source,
+        )
+        
+        query_step = PlanStep(
+            step_id=1,
+            action="clarify_intent",
+            tool_call_type=ToolCallType.ASK_USER,
+            parameters={
+                "original_intent": user_intent,
+                "available_actions": available_tools,
+            },
+            description=f"询问用户确认具体意图: {user_intent}",
+            requires_confirmation=True,
+            confirmation_message=f"您是想要执行以下哪个操作？{', '.join(available_tools[:5])}",
+        )
+        plan.add_step(query_step)
+        
+        return plan
+    
+    def _generate_empty_intent_plan(
+        self,
+        world_snapshot: dict[str, Any],
+        available_tools: list[str],
+    ) -> Plan:
+        """生成空意图的 Plan
+        
+        内部方法，当用户意图为空时调用。
+        
+        Args:
+            world_snapshot: 世界状态
+            available_tools: 可用能力列表
+        
+        Returns:
+            提示输入意图的 Plan 对象
+        """
+        plan = Plan(
+            plan_id=f"plan_{uuid.uuid4().hex[:8]}",
+            goal="等待用户输入",
+            reasoning="用户未提供有效意图",
+            risk_level=RiskLevel.LOW,
+            requires_confirmation=False,
+            status=PlanStatus.CREATED,
+            source=self.config.default_source,
+        )
+        
+        step = PlanStep(
+            step_id=1,
+            action="speak",
+            tool_call_type=ToolCallType.SPEAK,
+            parameters={"message": "您好，请告诉我您想要执行的操作。"},
+            description="等待用户输入有效意图",
+        )
+        plan.add_step(step)
+        
+        return plan
+    
+    def _needs_confirmation(
+        self,
+        world_snapshot: dict[str, Any],
+        user_intent: str,
+        action: str | None,
+        domain_rules: dict[str, Any] | None,
+    ) -> tuple[bool, str | None]:
+        """判断是否需要用户确认
+        
+        根据世界状态、用户意图、动作类型和领域规则判断。
+        优先使用 _normalize_world_snapshot() 转换后的结果。
+
+        Args:
+            world_snapshot: 世界状态快照
+            user_intent: 用户意图
+            action: 识别的动作名称
+            domain_rules: 领域规则
+        
+        Returns:
+            (是否需要确认, 确认消息)
+        """
+        if action is None:
+            return True, "无法识别您的意图，请确认您想要的操作。"
+
+        normalized = self._normalize_world_snapshot(world_snapshot)
+        if self._has_world_conflict(normalized, action, domain_rules):
+            return True, f"检测到与当前状态的潜在冲突，请确认是否继续执行 '{action}'？"
+
+        risk_level = self._assess_risk(user_intent, action, normalized, domain_rules)
+
+        if risk_level == RiskLevel.HIGH and self.config.require_confirmation_on_high_risk:
+            return True, f"该操作风险较高 '{action}'，是否确认执行？"
+
+        if risk_level == RiskLevel.MEDIUM and self.config.require_confirmation_on_medium_risk:
+            return True, f"执行操作 '{action}' 前，请确认。"
+
+        return False, None
+    
+    def _has_world_conflict(
+        self,
+        world_snapshot: dict[str, Any],
+        action: str,
+        domain_rules: dict[str, Any] | None,
+    ) -> bool:
+        """检查世界状态是否存在冲突
+        
+        内部方法，检测可能导致执行冲突的状态。
+        优先使用 _normalize_world_snapshot() 转换后的结果。
+
+        Args:
+            world_snapshot: 世界状态快照
+            action: 动作名称
+            domain_rules: 领域规则
+        
+        Returns:
+            是否存在冲突
+        """
+        normalized = self._normalize_world_snapshot(world_snapshot)
+
+        if normalized.get("actuator_status"):
+            actuator = normalized.get("actuator_status", {})
+            if isinstance(actuator, dict):
+                for key in ["unavailable", "error", "offline"]:
+                    if key in actuator and actuator[key]:
+                        return True
+
+        if normalized.get("recent_actions"):
+            recent = normalized.get("recent_actions", [])
+            if isinstance(recent, list) and len(recent) > 0:
+                last_action = recent[-1] if recent else None
+                if last_action and last_action.get("action") == action:
+                    return True
+
+        return False
+    
+    def _assess_risk(
+        self,
+        user_intent: str,
+        action: str | None,
+        world_snapshot: dict[str, Any],
+        domain_rules: dict[str, Any] | None,
+    ) -> RiskLevel:
+        """评估风险等级
+        
+        内部方法，根据多个因素综合评估风险。
+        优先使用 _normalize_world_snapshot() 转换后的结果。
+
+        Args:
+            user_intent: 用户意图
+            action: 动作名称
+            world_snapshot: 世界状态
+            domain_rules: 领域规则
+        
+        Returns:
+            风险等级
+        """
+        normalized = self._normalize_world_snapshot(world_snapshot)
+
+        if action is None:
+            return RiskLevel.LOW
+
+        for keyword in HIGH_RISK_KEYWORDS:
+            if keyword in user_intent:
+                return RiskLevel.HIGH
+
+        for keyword in MEDIUM_RISK_KEYWORDS:
+            if keyword in user_intent:
+                return RiskLevel.MEDIUM
+
+        if domain_rules and "high_risk_actions" in domain_rules:
+            if action in domain_rules["high_risk_actions"]:
+                return RiskLevel.HIGH
+
+        return self.config.default_risk_level
+    
+    def _should_delegate_to_llm(
+        self,
+        world_snapshot: dict[str, Any],
+        user_intent: str,
+        available_tools: list[str],
+        domain_rules: dict[str, Any] | None,
+        action: str | None,
+    ) -> bool:
+        """判断是否应该委托给 LLM
+        
+        内部方法，根据多个因素判断是否需要 LLM 介入。
+        
+        委托条件：
+        1. action 无法明确映射（意图复杂）
+        2. 世界状态存在冲突
+        3. 需要结合领域规则做高层判断
+        4. 多步骤规划需求
+        5. 意图语义模糊但可理解
+        
+        Args:
+            world_snapshot: 世界状态快照
+            user_intent: 用户意图描述
+            available_tools: 可用能力列表
+            domain_rules: 领域规则约束
+            action: 识别的动作（None 表示未识别）
+        
+        Returns:
+            是否应该委托给 LLM
+        """
+        domain_rules = domain_rules or {}
+        
+        # 条件1：action 无法明确映射
+        if action is None:
+            # 检查是否有任何关键词匹配
+            has_partial_match = False
+            for keyword in self.intent_to_action.keys():
+                if keyword in user_intent:
+                    has_partial_match = True
+                    break
+            # 有部分匹配但无法确定具体 action，需要 LLM
+            if has_partial_match:
+                return True
+        
+        # 条件2：世界状态存在冲突
+        if self._has_world_conflict(world_snapshot, action, domain_rules):
+            # 检查冲突是否严重到需要 LLM
+            if self._is_complex_conflict(world_snapshot, action):
+                return True
+        
+        # 条件3：涉及领域规则限制
+        if domain_rules:
+            high_risk_actions = domain_rules.get("high_risk_actions", [])
+            medium_risk_actions = domain_rules.get("medium_risk_actions", [])
+            if action in high_risk_actions or action in medium_risk_actions:
+                # 风险动作需要更复杂判断
+                return True
+        
+        # 条件4：多步骤意图（包含连接词）
+        multi_step_keywords = ["然后", "接下来", "之后", "再", "和", "以及", "并且", "再然后"]
+        if any(kw in user_intent for kw in multi_step_keywords):
+            return True
+        
+        # 条件5：意图包含多个动作词
+        action_count = 0
+        for keyword in self.intent_to_action.keys():
+            if keyword in user_intent:
+                action_count += 1
+        if action_count > 1:
+            return True
+        
+        # 条件6：意图不完整但包含上下文暗示
+        incomplete_indicators = ["它", "那个", "这个", "刚才", "上次", "再"]
+        if any(ind in user_intent for ind in incomplete_indicators):
+            if action is None:
+                return True
+        
+        return False
+    
+    def _is_complex_conflict(
+        self,
+        world_snapshot: dict[str, Any],
+        action: str | None,
+    ) -> bool:
+        """判断是否为复杂冲突
+        
+        复杂冲突需要 LLM 介入判断。
+        优先使用 _normalize_world_snapshot() 转换后的结果。
+
+        Args:
+            world_snapshot: 世界状态
+            action: 动作名称
+        
+        Returns:
+            是否为复杂冲突
+        """
+        normalized = self._normalize_world_snapshot(world_snapshot)
+
+        # 检查 recent_actions 中是否有相同动作
+        recent_actions = normalized.get("recent_actions", [])
+        if isinstance(recent_actions, list) and len(recent_actions) > 0:
+            last_action = recent_actions[-1] if recent_actions else {}
+            if isinstance(last_action, dict) and last_action.get("action") == action:
+                # 相同动作，检查时间间隔
+                if "timestamp" in last_action:
+                    time_diff = time.time() - last_action.get("timestamp", 0)
+                    # 短时间内重复动作视为复杂冲突
+                    if time_diff < 600:  # 10分钟内
+                        return True
+
+        # 检查是否有多个警告
+        warnings = normalized.get("warnings", [])
+        if isinstance(warnings, list) and len(warnings) > 1:
+            return True
+
+        return False
+    
+    def _generate_safe_fallback_plan(
+        self,
+        user_intent: str,
+        reason: str = "",
+    ) -> Plan:
+        """生成安全的 fallback Plan
+        
+        当 LLM 调用失败时，返回一个安全的 fallback plan。
+        优先使用 ASK_USER 或 SPEAK，不直接执行风险动作。
+        
+        Args:
+            user_intent: 用户原始意图
+            reason: 回退原因
+        
+        Returns:
+            安全的 Plan 对象
+        """
+        plan = Plan(
+            plan_id=f"plan_fallback_{uuid.uuid4().hex[:8]}",
+            goal=f"无法处理的请求: {user_intent}",
+            reasoning=f"LLM 处理失败或响应无效 ({reason})，返回安全 fallback",
+            risk_level=RiskLevel.LOW,
+            requires_confirmation=False,
+            confirmation_message=None,
+            status=PlanStatus.CREATED,
+            source="llm_fallback",
+            metadata={"fallback": True, "reason": reason},
+        )
+        
+        step = PlanStep(
+            step_id=1,
+            action="ask_user",
+            tool_call_type=ToolCallType.ASK_USER,
+            parameters={
+                "question": f"抱歉，我无法理解或处理您的请求: {user_intent}。请重新描述您的需求。",
+            },
+            preconditions={},
+            fallback=None,
+            status=StepStatus.PENDING,
+            description="询问用户澄清意图",
+            requires_confirmation=False,
+            confirmation_message=None,
+            metadata={"fallback_reason": reason},
+        )
+        plan.add_step(step)
+        
+        return plan
+    
+    def _validate_tool_available(self, tool_name: str, available_tools: list[str]) -> bool:
+        """验证工具是否可用
+        
+        内部方法，检查请求的动作是否在可用能力列表中。
+        
+        Args:
+            tool_name: 工具/动作名称
+            available_tools: 可用能力列表
+        
+        Returns:
+            是否可用
+        """
+        if not available_tools:
+            return False
+        return tool_name in available_tools
+    
+    def _normalize_world_snapshot(self, world_snapshot: dict[str, Any]) -> dict[str, Any]:
+        """将真实 /world/snapshot 结构统一转换为 Planner 扁平上下文
+
+        真实 world_snapshot 结构可能包含嵌套的 environment、system、entities 等字段，
+        此方法将其转换为扁平的、易于访问的格式。
+
+        对齐真实结构：
+        - environment: temperature, humidity, light_level, warnings, errors
+        - system: device_status, actuator_status, mode
+        - entities: 与 actuator/device/recent action 相关的信息
+
+        Args:
+            world_snapshot: 原始世界状态快照
+
+        Returns:
+            扁平的 normalized dict
+        """
+        # 如果快照没有变化，使用缓存
+        if self._cached_snapshot_raw is world_snapshot and self._cached_normalized_snapshot is not None:
+            return self._cached_normalized_snapshot
+
+        normalized: dict[str, Any] = {
+            "temperature": None,
+            "humidity": None,
+            "light_level": None,
+            "warnings": [],
+            "errors": [],
+            "device_status": {},
+            "actuator_status": {},
+            "mode": None,
+            "recent_actions": [],
+            "raw_entities": {},
+        }
+
+        # 处理 environment 字段（常见于 /world/snapshot）
+        environment = world_snapshot.get("environment", {})
+        if isinstance(environment, dict):
+            normalized["temperature"] = environment.get("temperature") or environment.get("temperature_ambient")
+            normalized["humidity"] = environment.get("humidity") or environment.get("humidity_ambient")
+            normalized["light_level"] = environment.get("light_level") or environment.get("illuminance")
+            normalized["warnings"] = environment.get("warnings", [])
+            normalized["errors"] = environment.get("errors", [])
+
+        # 处理 system 字段
+        system = world_snapshot.get("system", {})
+        if isinstance(system, dict):
+            normalized["device_status"] = system.get("device_status", {})
+            normalized["actuator_status"] = system.get("actuator_status", {})
+            normalized["mode"] = system.get("mode") or system.get("operating_mode")
+
+        # 直接字段（已经是扁平格式）
+        for key in ["temperature", "humidity", "light_level", "warnings", "errors",
+                    "device_status", "actuator_status", "mode"]:
+            if key not in environment and key in world_snapshot:
+                if normalized.get(key) is None or normalized.get(key) == [] or normalized.get(key) == {}:
+                    normalized[key] = world_snapshot.get(key)
+
+        # 处理 entities 字段（提取与 actuator/device 相关信息）
+        entities = world_snapshot.get("entities", {})
+        if isinstance(entities, dict):
+            actuators = entities.get("actuator", [])
+            devices = entities.get("device", [])
+            if isinstance(actuators, list):
+                for act in actuators:
+                    if isinstance(act, dict):
+                        device_id = act.get("entity_id") or act.get("device_id", "unknown")
+                        normalized["raw_entities"][f"actuator_{device_id}"] = act
+            if isinstance(devices, list):
+                for dev in devices:
+                    if isinstance(dev, dict):
+                        device_id = dev.get("entity_id") or dev.get("device_id", "unknown")
+                        normalized["raw_entities"][f"device_{device_id}"] = dev
+
+        # 处理 recent_actions（可能在顶层或 recent_context 下）
+        recent_context = world_snapshot.get("recent_context", {}) or world_snapshot.get("recent_actions", [])
+        if isinstance(recent_context, dict):
+            normalized["recent_actions"] = recent_context.get("recent_actions", [])
+        elif isinstance(recent_context, list):
+            normalized["recent_actions"] = recent_context
+
+        # 如果顶层直接有 recent_actions
+        if not normalized["recent_actions"] and "recent_actions" in world_snapshot:
+            normalized["recent_actions"] = world_snapshot.get("recent_actions", [])
+
+        # 缓存结果
+        self._cached_snapshot_raw = world_snapshot
+        self._cached_normalized_snapshot = normalized
+
+        return normalized
+
+    def _extract_recent_context(self, world_snapshot: dict[str, Any]) -> dict[str, Any]:
+        """提取与当前决策相关的上下文
+        
+        内部方法，从世界状态中提取关键信息。
+        优先使用 _normalize_world_snapshot() 转换后的结果。
+        
+        Args:
+            world_snapshot: 世界状态快照
+        
+        Returns:
+            相关的上下文字典
+        """
+        normalized = self._normalize_world_snapshot(world_snapshot)
+
+        context_keys = [
+            "temperature",
+            "humidity",
+            "light_level",
+            "device_status",
+            "actuator_status",
+            "recent_actions",
+            "warnings",
+            "errors",
+        ]
+
+        context = {}
+        for key in context_keys:
+            if key in normalized and normalized[key] is not None:
+                context[key] = normalized[key]
+
+        if normalized.get("raw_entities"):
+            context["raw_entities"] = normalized["raw_entities"]
+
+        return context
+    
+    def _map_intent_to_action(self, user_intent: str) -> str | None:
+        """将用户意图映射到 action
+        
+        内部方法，通过关键词匹配查找对应的 action。
+        按关键词长度从长到短排序后再匹配，避免短关键词优先匹配导致误判。
+
+        Args:
+            user_intent: 用户意图描述
+        
+        Returns:
+            匹配到的 action 名称，未匹配到返回 None
+        """
+        if not user_intent:
+            return None
+
+        intent_lower = user_intent.lower()
+
+        # 按关键词长度从长到短排序，避免"喂"比"喂食"先匹配
+        for keyword in sorted(self.intent_to_action.keys(), key=len, reverse=True):
+            if keyword in intent_lower or keyword in user_intent:
+                return self.intent_to_action[keyword]
+
+        return None
+    
+    def register_intent_mapping(self, keyword: str, action: str) -> None:
+        """注册新的意图映射
+        
+        公共方法，运行时添加新的意图到 action 的映射。
+        
+        Args:
+            keyword: 意图关键词
+            action: 对应的 action 名称
+        """
+        self.intent_to_action[keyword] = action
+    
+    def register_tool_type(self, action: str, tool_type: ToolCallType) -> None:
+        """注册 action 到 ToolCallType 的映射
+        
+        公共方法，运行时添加新的映射。
+        
+        Args:
+            action: action 名称
+            tool_type: 对应的 ToolCallType
+        """
+        self.action_to_tool_type[action] = tool_type
+
+
+# =============================================================================
+# 主程序入口（测试示例）
+# =============================================================================
+
+if __name__ == "__main__":
+    import json
+    
+    print("=" * 70)
+    print("Planner 测试演示")
+    print("=" * 70)
+    
+    # 初始化 Planner
+    planner = Planner()
+    
+    # -------------------------------------------------------------------------
+    # 构造测试数据
+    # -------------------------------------------------------------------------
+    world_snapshot = {
+        "temperature": 32.5,
+        "humidity": 70,
+        "actuator_status": {
+            "fan": "running",
+            "light": "on",
+        },
+        "recent_actions": [
+            {"action": "feed", "timestamp": time.time() - 300},
+        ],
+        "warnings": [],
+        "errors": [],
+    }
+    
+    available_tools = [
+        "feed",
+        "adjust_fan",
+        "speak",
+        "query",
+        "move",
+        "inspect",
+    ]
+    
+    # -------------------------------------------------------------------------
+    # 场景 1：直接可执行（工具存在 + 风险低）
+    # -------------------------------------------------------------------------
+    print("\n[场景 1] 直接可执行")
+    print("-" * 40)
+    
+    user_intent_1 = "打开风扇降温"
+    plan_1 = planner.generate_plan(
+        world_snapshot=world_snapshot,
+        user_intent=user_intent_1,
+        available_tools=available_tools,
+        planner_mode="rule",
+    )
+    
+    print(f"用户意图: {user_intent_1}")
+    print(f"Plan ID: {plan_1.plan_id}")
+    print(f"目标: {plan_1.goal}")
+    print(f"风险等级: {plan_1.risk_level.value}")
+    print(f"需要确认: {plan_1.requires_confirmation}")
+    print(f"步骤数: {len(plan_1.steps)}")
+    if plan_1.steps:
+        step = plan_1.steps[0]
+        print(f"  → Step {step.step_id}: {step.action} ({step.tool_call_type.value})")
+    
+    # -------------------------------------------------------------------------
+    # 场景 2：需要确认（风险中高或世界冲突）
+    # -------------------------------------------------------------------------
+    print("\n[场景 2] 需要确认")
+    print("-" * 40)
+    
+    world_snapshot_conflict = {
+        "temperature": 32.5,
+        "actuator_status": {
+            "fan": "error",
+        },
+        "recent_actions": [
+            {"action": "adjust_fan", "timestamp": time.time() - 60},
+        ],
+    }
+    
+    user_intent_2 = "再次降温"
+    plan_2 = planner.generate_plan(
+        world_snapshot=world_snapshot_conflict,
+        user_intent=user_intent_2,
+        available_tools=available_tools,
+        planner_mode="rule",
+    )
+    
+    print(f"用户意图: {user_intent_2}")
+    print(f"Plan ID: {plan_2.plan_id}")
+    print(f"需要确认: {plan_2.requires_confirmation}")
+    print(f"确认消息: {plan_2.confirmation_message}")
+    print(f"步骤数: {len(plan_2.steps)}")
+    if plan_2.steps:
+        step = plan_2.steps[0]
+        print(f"  → Step {step.step_id}: {step.action} ({step.tool_call_type.value})")
+        print(f"     需要确认: {step.requires_confirmation}")
+    
+    # -------------------------------------------------------------------------
+    # 场景 3：工具不存在
+    # -------------------------------------------------------------------------
+    print("\n[场景 3] 工具不存在")
+    print("-" * 40)
+    
+    user_intent_3 = "打开空调"
+    available_tools_3 = ["fan", "light", "speak"]
+    
+    plan_3 = planner.generate_plan(
+        world_snapshot=world_snapshot,
+        user_intent=user_intent_3,
+        available_tools=available_tools_3,
+        planner_mode="rule",
+    )
+    
+    print(f"用户意图: {user_intent_3}")
+    print(f"可用工具: {available_tools_3}")
+    print(f"Plan ID: {plan_3.plan_id}")
+    print(f"目标: {plan_3.goal}")
+    print(f"步骤数: {len(plan_3.steps)}")
+    if plan_3.steps:
+        step = plan_3.steps[0]
+        print(f"  → Step {step.step_id}: {step.action} ({step.tool_call_type.value})")
+        print(f"     消息: {step.parameters.get('message', '')}")
+    
+    # -------------------------------------------------------------------------
+    # 场景 4：LLM 模式（真实实现）
+    # -------------------------------------------------------------------------
+    print("\n[场景 4] LLM 模式")
+    print("-" * 40)
+    
+    user_intent_4 = "打开风扇降温"
+    plan_4 = planner.generate_plan(
+        world_snapshot=world_snapshot,
+        user_intent=user_intent_4,
+        available_tools=available_tools,
+        planner_mode="llm",
+    )
+    
+    print(f"用户意图: {user_intent_4}")
+    print(f"Plan ID: {plan_4.plan_id}")
+    print(f"来源: {plan_4.source}")
+    print(f"目标: {plan_4.goal}")
+    print(f"风险等级: {plan_4.risk_level.value}")
+    print(f"步骤数: {len(plan_4.steps)}")
+    if plan_4.steps:
+        step = plan_4.steps[0]
+        print(f"  → Step {step.step_id}: {step.action} ({step.tool_call_type.value})")
+    
+    # -------------------------------------------------------------------------
+    # 场景 5：LLM 模式 - 喂食询问
+    # -------------------------------------------------------------------------
+    print("\n[场景 5] LLM 模式 - 喂食询问")
+    print("-" * 40)
+    
+    user_intent_5 = "再喂一次"
+    world_snapshot_recent_feed = {
+        "temperature": 28,
+        "last_feed_time": "2026-04-10T10:00:00",
+        "recent_actions": [
+            {"action": "feed", "timestamp": time.time() - 300},
+        ],
+    }
+    plan_5 = planner.generate_plan(
+        world_snapshot=world_snapshot_recent_feed,
+        user_intent=user_intent_5,
+        available_tools=["feed", "speak", "query"],
+        planner_mode="llm",
+    )
+    
+    print(f"用户意图: {user_intent_5}")
+    print(f"Plan ID: {plan_5.plan_id}")
+    print(f"来源: {plan_5.source}")
+    print(f"目标: {plan_5.goal}")
+    print(f"风险等级: {plan_5.risk_level.value}")
+    print(f"步骤数: {len(plan_5.steps)}")
+    if plan_5.steps:
+        step = plan_5.steps[0]
+        print(f"  → Step {step.step_id}: {step.action} ({step.tool_call_type.value})")
+        if step.tool_call_type == ToolCallType.ASK_USER:
+            print(f"     询问: {step.parameters.get('question', 'N/A')}")
+    
+    # -------------------------------------------------------------------------
+    # 场景 6：Hybrid 模式 - 简单场景走规则
+    # -------------------------------------------------------------------------
+    print("\n[场景 6] Hybrid 模式 - 简单场景")
+    print("-" * 40)
+    
+    user_intent_6 = "查询温度"
+    plan_6 = planner.generate_plan(
+        world_snapshot=world_snapshot,
+        user_intent=user_intent_6,
+        available_tools=available_tools,
+        planner_mode="hybrid",
+    )
+    
+    print(f"用户意图: {user_intent_6}")
+    print(f"Plan ID: {plan_6.plan_id}")
+    print(f"来源: {plan_6.source}")
+    print(f"风险等级: {plan_6.risk_level.value}")
+    
+    # -------------------------------------------------------------------------
+    # 场景 7：Hybrid 模式 - 复杂场景走 LLM
+    # -------------------------------------------------------------------------
+    print("\n[场景 7] Hybrid 模式 - 复杂场景")
+    print("-" * 40)
+    
+    user_intent_7 = "先降温然后喂食"  # 多步骤意图
+    plan_7 = planner.generate_plan(
+        world_snapshot=world_snapshot,
+        user_intent=user_intent_7,
+        available_tools=available_tools,
+        planner_mode="hybrid",
+    )
+    
+    print(f"用户意图: {user_intent_7}")
+    print(f"Plan ID: {plan_7.plan_id}")
+    print(f"来源: {plan_7.source}")
+    print(f"目标: {plan_7.goal}")
+    print(f"风险等级: {plan_7.risk_level.value}")
+    print(f"步骤数: {len(plan_7.steps)}")
+    for step in plan_7.steps:
+        print(f"  → Step {step.step_id}: {step.action} ({step.tool_call_type.value})")
+    
+    # -------------------------------------------------------------------------
+    # 场景 8：意图不清晰 - 走规则未知意图
+    # -------------------------------------------------------------------------
+    print("\n[场景 8] 意图不清晰")
+    print("-" * 40)
+    
+    user_intent_8 = "随便弄一下"
+    plan_8 = planner.generate_plan(
+        world_snapshot=world_snapshot,
+        user_intent=user_intent_8,
+        available_tools=available_tools,
+        planner_mode="rule",
+    )
+    
+    print(f"用户意图: {user_intent_8}")
+    print(f"Plan ID: {plan_8.plan_id}")
+    print(f"目标: {plan_8.goal}")
+    print(f"步骤数: {len(plan_8.steps)}")
+    if plan_8.steps:
+        step = plan_8.steps[0]
+        print(f"  → Step {step.step_id}: {step.action} ({step.tool_call_type.value})")
+    
+    # -------------------------------------------------------------------------
+    # 场景 9：工具不存在
+    # -------------------------------------------------------------------------
+    print("\n[场景 9] 工具不存在")
+    print("-" * 40)
+    
+    user_intent_9 = "打开空调"
+    available_tools_9 = ["fan", "light", "speak"]
+    
+    plan_9 = planner.generate_plan(
+        world_snapshot=world_snapshot,
+        user_intent=user_intent_9,
+        available_tools=available_tools_9,
+        planner_mode="rule",
+    )
+    
+    print(f"用户意图: {user_intent_9}")
+    print(f"可用工具: {available_tools_9}")
+    print(f"Plan ID: {plan_9.plan_id}")
+    print(f"目标: {plan_9.goal}")
+    print(f"步骤数: {len(plan_9.steps)}")
+    if plan_9.steps:
+        step = plan_9.steps[0]
+        print(f"  → Step {step.step_id}: {step.action} ({step.tool_call_type.value})")
+        print(f"     消息: {step.parameters.get('message', '')}")
+    
+    # -------------------------------------------------------------------------
+    # 场景 10：LLM 工具不存在场景
+    # -------------------------------------------------------------------------
+    print("\n[场景 10] LLM 模式 - 工具不存在")
+    print("-" * 40)
+    
+    user_intent_10 = "打开空调"
+    plan_10 = planner.generate_plan(
+        world_snapshot={"temperature": 30},
+        user_intent=user_intent_10,
+        available_tools=["adjust_fan", "speak"],
+        planner_mode="llm",
+    )
+    
+    print(f"用户意图: {user_intent_10}")
+    print(f"Plan ID: {plan_10.plan_id}")
+    print(f"来源: {plan_10.source}")
+    print(f"目标: {plan_10.goal}")
+    print(f"步骤数: {len(plan_10.steps)}")
+    if plan_10.steps:
+        step = plan_10.steps[0]
+        print(f"  → Step {step.step_id}: {step.action} ({step.tool_call_type.value})")
+    
+    # -------------------------------------------------------------------------
+    # 输出完整 JSON
+    # -------------------------------------------------------------------------
+    print("\n[完整 JSON 输出]")
+    print("-" * 40)
+    
+    print("\n场景 1 Plan JSON (rule 模式):")
+    print(json.dumps(plan_1.to_dict(), indent=2, ensure_ascii=False))
+    
+    print("\n场景 4 Plan JSON (llm 模式):")
+    print(json.dumps(plan_4.to_dict(), indent=2, ensure_ascii=False))
+    
+    # -------------------------------------------------------------------------
+    # 测试自定义意图映射
+    # -------------------------------------------------------------------------
+    print("\n[自定义意图映射测试]")
+    print("-" * 40)
+    
+    planner.register_intent_mapping("打扫", "clean")
+    planner.register_tool_type("clean", ToolCallType.EXECUTE)
+    
+    user_intent_custom = "帮我打扫一下"
+    plan_custom = planner.generate_plan(
+        world_snapshot=world_snapshot,
+        user_intent=user_intent_custom,
+        available_tools=["clean", "speak"],
+        planner_mode="rule",
+    )
+    
+    print(f"用户意图: {user_intent_custom}")
+    print(f"识别动作: {plan_custom.steps[0].action if plan_custom.steps else 'None'}")
+    
+    print("\n" + "=" * 70)
+    print("Planner 测试演示完成")
+    print("=" * 70)
+

brain/planner_config.yaml

@@ -0,0 +1,81 @@
+# Planner 配置示例
+#
+# 该配置文件用于配置 Planner 的行为参数
+
+planner:
+  # 规划模式: "rule" / "llm" / "hybrid"
+  mode: "hybrid"
+
+  # 默认风险等级: "low" / "medium" / "high"
+  default_risk_level: "low"
+
+  # 中风险操作是否需要用户确认
+  require_confirmation_on_medium_risk: true
+
+  # 高风险操作是否需要用户确认
+  require_confirmation_on_high_risk: true
+
+  # 单个 Plan 最大步骤数
+  max_plan_steps: 10
+
+  # 默认计划来源: "rule_engine" / "llm" / "hybrid"
+  default_source: "hybrid"
+
+  # 可用的能力列表
+  available_tools:
+    - "feed"
+    - "adjust_fan"
+    - "speak"
+    - "query"
+    - "move"
+    - "inspect"
+    - "control_light"
+    - "turn_on"
+    - "turn_off"
+
+  # 意图关键词到 action 的映射
+  # 通用意图示例，可根据实际场景扩展
+  intent_to_action:
+    "喂食": "feed"
+    "喂养": "feed"
+    "降温": "adjust_fan"
+    "打开风扇": "adjust_fan"
+    "关闭风扇": "adjust_fan"
+    "播报": "speak"
+    "报告": "speak"
+    "通知": "speak"
+    "查询": "query"
+    "查看": "query"
+    "移动": "move"
+    "前往": "move"
+    "打开": "turn_on"
+    "关闭": "turn_off"
+    "停止": "turn_off"
+
+  # 高风险动作列表（需特殊确认）
+  high_risk_actions:
+    - "turn_off"
+    - "emergency_stop"
+    - "shutdown"
+    - "reset"
+
+  # 中风险动作列表
+  medium_risk_actions:
+    - "adjust_fan"
+    - "control_light"
+    - "adjust"
+    - "modify"
+
+  # 确认规则配置
+  confirmation_rules:
+    # 重复执行同一动作是否需要确认
+    repeated_action_requires_confirmation: true
+
+    # 执行器不可用时是否需要确认
+    unavailable_actuator_requires_confirmation: true
+
+    # 高风险操作是否需要确认
+    high_risk_requires_confirmation: true
+
+    # 中风险操作是否需要确认
+    medium_risk_requires_confirmation: true

brain/planner_config_loader.py

@@ -0,0 +1,621 @@
+"""
+planner_config_loader.py - Planner 配置加载模块
+
+该模块负责从统一 config.yaml 中读取 Planner 相关配置，
+并将其转换为 planner.py 可直接使用的配置对象和运行参数。
+
+职责边界：
+    - 读取、校验、转换配置
+    - 不做规划逻辑
+    - 不做 ROS 通信
+    - 不包含场景硬编码
+
+使用方式：
+    from planner_config_loader import load_planner_runtime_config
+
+    runtime_config = load_planner_runtime_config("planner_config.yaml")
+    planner = Planner(runtime_config["planner_config"])
+    plan = planner.generate_plan(
+        world_snapshot={...},
+        user_intent="降温",
+        available_tools=runtime_config["available_tools"],
+        domain_rules=runtime_config["domain_rules"],
+        planner_mode=runtime_config["planner_mode"],
+    )
+"""
+
+from __future__ import annotations
+
+import os
+from dataclasses import dataclass, field
+from typing import Any
+
+try:
+    import yaml
+    YAML_AVAILABLE = True
+except ImportError:
+    YAML_AVAILABLE = False
+
+from planner import PlannerConfig
+from tool_protocol import RiskLevel
+
+
+# =============================================================================
+# 配置校验异常
+# =============================================================================
+
+class ConfigValidationError(ValueError):
+    """配置校验异常"""
+    pass
+
+
+# =============================================================================
+# 运行时配置数据类
+# =============================================================================
+
+@dataclass
+class PlannerRuntimeConfig:
+    """Planner 运行时配置
+    
+    聚合所有 Planner 运行所需的配置项。
+    
+    属性：
+        planner_mode: 规划模式
+        planner_config: PlannerConfig 对象
+        available_tools: 可用能力列表
+        tool_descriptions: 工具语义描述列表
+        intent_to_action: 意图到 action 的映射
+        domain_rules: 领域规则
+    
+    示例：
+        >>> config = PlannerRuntimeConfig(
+        ...     planner_mode="hybrid",
+        ...     planner_config=PlannerConfig(),
+        ...     available_tools=["feed", "speak"],
+        ...     tool_descriptions=[{"name": "feed", "description": "..."}],
+        ...     intent_to_action={"降温": "adjust_fan"},
+        ...     domain_rules={"high_risk_actions": []},
+        ... )
+    """
+    
+    planner_mode: str = "hybrid"
+    planner_config: PlannerConfig = field(default_factory=PlannerConfig)
+    available_tools: list[str] = field(default_factory=list)
+    tool_descriptions: list[dict[str, Any]] = field(default_factory=list)
+    intent_to_action: dict[str, str] = field(default_factory=dict)
+    domain_rules: dict[str, Any] = field(default_factory=dict)
+    
+    def to_dict(self) -> dict[str, Any]:
+        """转换为字典
+        
+        Returns:
+            包含所有配置项的字典
+        """
+        return {
+            "planner_mode": self.planner_mode,
+            "planner_config": {
+                "default_risk_level": self.planner_config.default_risk_level.value,
+                "require_confirmation_on_medium_risk": self.planner_config.require_confirmation_on_medium_risk,
+                "require_confirmation_on_high_risk": self.planner_config.require_confirmation_on_high_risk,
+                "max_plan_steps": self.planner_config.max_plan_steps,
+                "default_source": self.planner_config.default_source,
+            },
+            "available_tools": self.available_tools,
+            "tool_descriptions": self.tool_descriptions,
+            "intent_to_action": self.intent_to_action,
+            "domain_rules": self.domain_rules,
+        }
+
+
+# =============================================================================
+# 配置加载函数
+# =============================================================================
+
+def load_yaml_config(config_path: str) -> dict[str, Any]:
+    """加载 YAML 配置文件
+    
+    Args:
+        config_path: 配置文件路径
+    
+    Returns:
+        解析后的配置字典
+    
+    Raises:
+        FileNotFoundError: 配置文件不存在
+        ValueError: YAML 格式错误
+        ImportError: PyYAML 未安装
+    """
+    if not os.path.exists(config_path):
+        raise FileNotFoundError(f"配置文件不存在: {config_path}")
+    
+    if not YAML_AVAILABLE:
+        raise ImportError(
+            "PyYAML 未安装，请运行: pip install pyyaml"
+        )
+    
+    try:
+        with open(config_path, "r", encoding="utf-8") as f:
+            config = yaml.safe_load(f)
+    except yaml.YAMLError as e:
+        raise ValueError(f"YAML 格式错误 in {config_path}: {e}")
+    
+    if config is None:
+        return {}
+    
+    return config
+
+
+def load_planner_section(config: dict[str, Any]) -> dict[str, Any]:
+    """提取 planner 配置段
+    
+    Args:
+        config: 完整配置字典
+    
+    Returns:
+        planner 配置段，缺省时返回空字典
+    """
+    return config.get("planner", {})
+
+
+def build_planner_config(planner_section: dict[str, Any]) -> PlannerConfig:
+    """构造 PlannerConfig 对象
+    
+    Args:
+        planner_section: planner 配置段
+    
+    Returns:
+        PlannerConfig 对象
+    
+    Raises:
+        ConfigValidationError: 配置值非法
+    """
+    # 获取各字段，缺省时使用默认值
+    risk_level_str = planner_section.get("default_risk_level", "low")
+    require_medium = planner_section.get("require_confirmation_on_medium_risk", True)
+    require_high = planner_section.get("require_confirmation_on_high_risk", True)
+    max_steps = planner_section.get("max_plan_steps", 10)
+    default_source = planner_section.get("default_source", "hybrid")
+    
+    # 校验并转换 risk_level
+    risk_level_str = str(risk_level_str).lower()
+    if risk_level_str not in ("low", "medium", "high"):
+        raise ConfigValidationError(
+            f"无效的 default_risk_level: '{risk_level_str}'，有效值为: low/medium/high"
+        )
+    
+    risk_level = RiskLevel(risk_level_str)
+    
+    # 校验 max_plan_steps
+    if not isinstance(max_steps, int) or max_steps <= 0:
+        raise ConfigValidationError(
+            f"无效的 max_plan_steps: {max_steps}，必须为大于 0 的整数"
+        )
+    
+    # 校验 default_source
+    valid_sources = ("rule_engine", "llm", "hybrid")
+    if default_source not in valid_sources:
+        raise ConfigValidationError(
+            f"无效的 default_source: '{default_source}'，有效值为: {valid_sources}"
+        )
+    
+    return PlannerConfig(
+        default_risk_level=risk_level,
+        require_confirmation_on_medium_risk=require_medium,
+        require_confirmation_on_high_risk=require_high,
+        max_plan_steps=max_steps,
+        default_source=default_source,
+    )
+
+
+def load_available_tools(planner_section: dict[str, Any]) -> list[str]:
+    """加载可用能力列表
+    
+    Args:
+        planner_section: planner 配置段
+    
+    Returns:
+        去重后的工具列表，缺省时返回空列表
+    """
+    raw_tools = planner_section.get("available_tools", [])
+    
+    if not isinstance(raw_tools, list):
+        return []
+    
+    # 去重，保持顺序
+    seen = set()
+    tools = []
+    for tool in raw_tools:
+        if isinstance(tool, str) and tool and tool not in seen:
+            seen.add(tool)
+            tools.append(tool)
+    
+    return tools
+
+
+def load_tool_descriptions(planner_section: dict[str, Any]) -> list[dict[str, Any]]:
+    """加载工具语义描述列表
+    
+    Args:
+        planner_section: planner 配置段
+    
+    Returns:
+        工具描述列表，每项包含 name, description, tool_call_type, category
+        缺失时返回空列表
+        配置非法时过滤无效项并打印 warning
+    """
+    raw_descriptions = planner_section.get("tool_descriptions", [])
+    
+    if not isinstance(raw_descriptions, list):
+        print(f"[PlannerConfigLoader] Warning: tool_descriptions 应为列表类型，忽略")
+        return []
+    
+    valid_descriptions = []
+    for i, item in enumerate(raw_descriptions):
+        if not isinstance(item, dict):
+            print(f"[PlannerConfigLoader] Warning: tool_descriptions[{i}] 应为字典，忽略")
+            continue
+        
+        # 必须包含 name 和 description
+        name = item.get("name")
+        description = item.get("description")
+        
+        if not name or not isinstance(name, str):
+            print(f"[PlannerConfigLoader] Warning: tool_descriptions[{i}] 缺少有效 name 字段，忽略")
+            continue
+        
+        if not description or not isinstance(description, str):
+            print(f"[PlannerConfigLoader] Warning: tool_descriptions[{i}] 缺少有效 description 字段，忽略")
+            continue
+        
+        # 构建有效描述，补充默认值
+        valid_item = {
+            "name": str(name).strip(),
+            "description": str(description).strip(),
+            "tool_call_type": str(item.get("tool_call_type", "execute")).strip(),
+            "category": str(item.get("category", "action")).strip(),
+        }
+        valid_descriptions.append(valid_item)
+    
+    if len(valid_descriptions) < len(raw_descriptions):
+        print(f"[PlannerConfigLoader] Warning: 过滤了 {len(raw_descriptions) - len(valid_descriptions)} 个无效 tool_descriptions 项")
+    
+    return valid_descriptions
+
+
+def load_intent_to_action(planner_section: dict[str, Any]) -> dict[str, str]:
+    """加载意图到 action 的映射
+    
+    Args:
+        planner_section: planner 配置段
+    
+    Returns:
+        关键词到 action 的映射字典
+    
+    Raises:
+        ConfigValidationError: key 或 value 为空
+    """
+    raw_mapping = planner_section.get("intent_to_action", {})
+    
+    if not isinstance(raw_mapping, dict):
+        return {}
+    
+    result = {}
+    for key, value in raw_mapping.items():
+        key_str = str(key).strip()
+        value_str = str(value).strip()
+        
+        if not key_str:
+            raise ConfigValidationError("intent_to_action 的 key 不能为空字符串")
+        if not value_str:
+            raise ConfigValidationError(
+                f"intent_to_action 的 value 不能为空字符串，key='{key_str}'"
+            )
+        
+        result[key_str] = value_str
+    
+    return result
+
+
+def load_high_risk_actions(planner_section: dict[str, Any]) -> list[str]:
+    """加载高风险动作列表
+    
+    Args:
+        planner_section: planner 配置段
+    
+    Returns:
+        高风险动作列表
+    """
+    raw_list = planner_section.get("high_risk_actions", [])
+    
+    if not isinstance(raw_list, list):
+        return []
+    
+    return [str(item) for item in raw_list if item]
+
+
+def load_medium_risk_actions(planner_section: dict[str, Any]) -> list[str]:
+    """加载中风险动作列表
+    
+    Args:
+        planner_section: planner 配置段
+    
+    Returns:
+        中风险动作列表
+    """
+    raw_list = planner_section.get("medium_risk_actions", [])
+    
+    if not isinstance(raw_list, list):
+        return []
+    
+    return [str(item) for item in raw_list if item]
+
+
+def load_confirmation_rules(planner_section: dict[str, Any]) -> dict[str, bool]:
+    """加载确认规则配置
+    
+    Args:
+        planner_section: planner 配置段
+    
+    Returns:
+        确认规则字典
+    """
+    raw_rules = planner_section.get("confirmation_rules", {})
+    
+    if not isinstance(raw_rules, dict):
+        raw_rules = {}
+    
+    return {
+        "repeated_action_requires_confirmation": bool(
+            raw_rules.get("repeated_action_requires_confirmation", True)
+        ),
+        "unavailable_actuator_requires_confirmation": bool(
+            raw_rules.get("unavailable_actuator_requires_confirmation", True)
+        ),
+        "high_risk_requires_confirmation": bool(
+            raw_rules.get("high_risk_requires_confirmation", True)
+        ),
+        "medium_risk_requires_confirmation": bool(
+            raw_rules.get("medium_risk_requires_confirmation", True)
+        ),
+    }
+
+
+def load_domain_rules(planner_section: dict[str, Any]) -> dict[str, Any]:
+    """加载领域规则
+    
+    Args:
+        planner_section: planner 配置段
+    
+    Returns:
+        领域规则字典
+    """
+    return {
+        "high_risk_actions": load_high_risk_actions(planner_section),
+        "medium_risk_actions": load_medium_risk_actions(planner_section),
+        "confirmation_rules": load_confirmation_rules(planner_section),
+    }
+
+
+def validate_planner_mode(mode: str) -> str:
+    """校验并规范化 planner 模式
+    
+    Args:
+        mode: 原始模式字符串
+    
+    Returns:
+        规范化后的模式字符串
+    
+    Raises:
+        ConfigValidationError: 模式非法
+    """
+    mode_lower = str(mode).lower().strip()
+    valid_modes = ("rule", "llm", "hybrid")
+    
+    if mode_lower not in valid_modes:
+        raise ConfigValidationError(
+            f"无效的 planner mode: '{mode}'，有效值为: {valid_modes}"
+        )
+    
+    return mode_lower
+
+
+def load_planner_runtime_config(config_path: str) -> PlannerRuntimeConfig:
+    """加载 Planner 运行时配置（统一入口）
+    
+    读取 YAML 文件，提取 planner 配置段，
+    进行校验和转换，返回可直接使用的配置对象。
+    
+    Args:
+        config_path: 配置文件路径
+    
+    Returns:
+        PlannerRuntimeConfig 对象
+    
+    Raises:
+        FileNotFoundError: 配置文件不存在
+        ValueError: YAML 格式错误
+        ConfigValidationError: 配置值非法
+        ImportError: PyYAML 未安装
+    """
+    # 加载并解析 YAML
+    full_config = load_yaml_config(config_path)
+    
+    # 提取 planner 配置段
+    planner_section = load_planner_section(full_config)
+    
+    # 校验并加载 planner_mode
+    planner_mode = planner_section.get("mode", "hybrid")
+    planner_mode = validate_planner_mode(planner_mode)
+    
+    # 构造 PlannerConfig
+    planner_config = build_planner_config(planner_section)
+    
+    # 加载其他配置项
+    available_tools = load_available_tools(planner_section)
+    tool_descriptions = load_tool_descriptions(planner_section)
+    intent_to_action = load_intent_to_action(planner_section)
+    domain_rules = load_domain_rules(planner_section)
+    
+    # 构建并返回运行时配置
+    return PlannerRuntimeConfig(
+        planner_mode=planner_mode,
+        planner_config=planner_config,
+        available_tools=available_tools,
+        tool_descriptions=tool_descriptions,
+        intent_to_action=intent_to_action,
+        domain_rules=domain_rules,
+    )
+
+
+def load_planner_runtime_config_as_dict(config_path: str) -> dict[str, Any]:
+    """加载 Planner 运行时配置（返回字典形式）
+    
+    与 load_planner_runtime_config 功能相同，但返回字典格式。
+    兼容不需要 dataclass 的场景。
+    
+    Args:
+        config_path: 配置文件路径
+    
+    Returns:
+        包含所有配置项的字典
+    """
+    runtime_config = load_planner_runtime_config(config_path)
+    
+    return {
+        "planner_mode": runtime_config.planner_mode,
+        "planner_config": runtime_config.planner_config,
+        "available_tools": runtime_config.available_tools,
+        "intent_to_action": runtime_config.intent_to_action,
+        "domain_rules": runtime_config.domain_rules,
+    }
+
+
+# =============================================================================
+# 主程序入口（测试示例）
+# =============================================================================
+
+if __name__ == "__main__":
+    import json
+    import os
+    
+    print("=" * 70)
+    print("Planner 配置加载测试")
+    print("=" * 70)
+    
+    # 获取当前脚本所在目录
+    script_dir = os.path.dirname(os.path.abspath(__file__))
+    config_path = os.path.join(script_dir, "planner_config.yaml")
+    
+    # 检查配置文件是否存在
+    if not os.path.exists(config_path):
+        print(f"\n配置文件不存在: {config_path}")
+        print("创建示例配置文件...")
+        
+        # 创建一个临时配置用于测试
+        from planner import PlannerConfig
+        from tool_protocol import RiskLevel
+        
+        runtime_config = PlannerRuntimeConfig(
+            planner_mode="hybrid",
+            planner_config=PlannerConfig(),
+            available_tools=["feed", "adjust_fan", "speak"],
+            intent_to_action={"降温": "adjust_fan"},
+            domain_rules={
+                "high_risk_actions": ["turn_off"],
+                "medium_risk_actions": ["adjust_fan"],
+                "confirmation_rules": {},
+            },
+        )
+    else:
+        # 加载配置
+        print(f"\n加载配置文件: {config_path}")
+        runtime_config = load_planner_runtime_config(config_path)
+    
+    # -------------------------------------------------------------------------
+    # 打印配置信息
+    # -------------------------------------------------------------------------
+    print("\n[1] planner_mode")
+    print("-" * 40)
+    print(f"  {runtime_config.planner_mode}")
+    
+    print("\n[2] PlannerConfig")
+    print("-" * 40)
+    pc = runtime_config.planner_config
+    print(f"  default_risk_level: {pc.default_risk_level.value}")
+    print(f"  require_confirmation_on_medium_risk: {pc.require_confirmation_on_medium_risk}")
+    print(f"  require_confirmation_on_high_risk: {pc.require_confirmation_on_high_risk}")
+    print(f"  max_plan_steps: {pc.max_plan_steps}")
+    print(f"  default_source: {pc.default_source}")
+    
+    print("\n[3] available_tools")
+    print("-" * 40)
+    tools = runtime_config.available_tools
+    print(f"  数量: {len(tools)}")
+    for i, tool in enumerate(tools, 1):
+        print(f"    {i}. {tool}")
+    
+    print("\n[4] tool_descriptions")
+    print("-" * 40)
+    tool_descs = runtime_config.tool_descriptions
+    print(f"  数量: {len(tool_descs)}")
+    for i, desc in enumerate(tool_descs, 1):
+        print(f"    {i}. name={desc.get('name')}, type={desc.get('tool_call_type')}, category={desc.get('category')}")
+        print(f"       description: {desc.get('description', '')[:50]}...")
+    
+    print("\n[5] intent_to_action")
+    print("-" * 40)
+    mapping = runtime_config.intent_to_action
+    print(f"  数量: {len(mapping)}")
+    for keyword, action in mapping.items():
+        print(f"    '{keyword}' → '{action}'")
+    
+    print("\n[6] domain_rules")
+    print("-" * 40)
+    rules = runtime_config.domain_rules
+    print(f"  high_risk_actions: {rules.get('high_risk_actions', [])}")
+    print(f"  medium_risk_actions: {rules.get('medium_risk_actions', [])}")
+    print(f"  confirmation_rules:")
+    for key, value in rules.get("confirmation_rules", {}).items():
+        print(f"    {key}: {value}")
+    
+    # -------------------------------------------------------------------------
+    # 测试校验功能
+    # -------------------------------------------------------------------------
+    print("\n[7] 配置校验测试")
+    print("-" * 40)
+    
+    # 测试无效 risk_level
+    try:
+        bad_section = {"default_risk_level": "invalid"}
+        build_planner_config(bad_section)
+        print("  ❌ 无效 risk_level 未被捕获")
+    except ConfigValidationError as e:
+        print(f"  ✓ 无效 risk_level 正确捕获: {e}")
+    
+    # 测试无效 max_plan_steps
+    try:
+        bad_section = {"max_plan_steps": 0}
+        build_planner_config(bad_section)
+        print("  ❌ 无效 max_plan_steps 未被捕获")
+    except ConfigValidationError as e:
+        print(f"  ✓ 无效 max_plan_steps 正确捕获: {e}")
+    
+    # 测试空的 intent key
+    try:
+        bad_section = {"intent_to_action": {"": "feed"}}
+        load_intent_to_action(bad_section)
+        print("  ❌ 空的 intent key 未被捕获")
+    except ConfigValidationError as e:
+        print(f"  ✓ 空的 intent key 正确捕获")
+    
+    # -------------------------------------------------------------------------
+    # 完整 JSON 输出
+    # -------------------------------------------------------------------------
+    print("\n[8] 完整配置 JSON")
+    print("-" * 40)
+    print(json.dumps(runtime_config.to_dict(), indent=2, ensure_ascii=False))
+    
+    print("\n" + "=" * 70)
+    print("Planner 配置加载测试完成")
+    print("=" * 70)
+

brain/prompt_manager.py

@@ -0,0 +1,450 @@
+"""
+prompt_manager.py - Planner Prompt 构造器
+
+该模块负责构造适合 LLM Planner 的 Prompt，使 LLM 能够生成标准化的 Plan JSON。
+
+职责边界：
+    - 构造高质量 Prompt
+    - 管理 system instruction
+    - 不调用 LLM
+    - 不执行 capability
+
+设计原则：
+    - Prompt 必须清晰、无歧义
+    - 必须明确告诉 LLM 角色和约束
+    - 必须提供完整的输出格式规范
+    - 必须考虑安全边界
+
+使用方式：
+    from prompt_manager import PlannerPromptManager
+    
+    manager = PlannerPromptManager()
+    prompt = manager.build_plan_prompt(
+        user_intent="降温",
+        world_snapshot={"temperature": 32},
+        available_tools=["adjust_fan", "speak"],
+        domain_rules={"high_risk_actions": ["turn_off"]},
+    )
+"""
+
+from __future__ import annotations
+
+import json
+from typing import Any
+
+
+# =============================================================================
+# Prompt 模板常量
+# =============================================================================
+
+# Planner System Instruction
+PLANNER_SYSTEM_INSTRUCTION = """你是一个专业的 AI Agent Planner。
+
+你的职责是根据用户意图和当前世界状态，生成标准化的行动计划（Plan）。
+
+## 核心约束
+
+1. 只生成 Plan，不执行动作
+   - 你只输出 Plan JSON，不调用任何能力
+   - 实际执行由 Executor 负责
+
+2. 只能使用给定的能力
+   - 必须从 available_tools 列表中选择
+   - 禁止虚构或使用未提供的能力
+
+3. 必须输出严格 JSON
+   - 输出必须是一个可被 Plan.from_dict() 和 PlanStep.from_dict() 解析的标准 JSON 对象
+   - 禁止输出 markdown
+   - 禁止输出解释文字
+   - 禁止输出额外前后缀
+   - 禁止缺失必需字段
+   - 禁止使用未定义字段替代标准字段
+
+4. 安全优先原则
+   - 高风险动作必须设置 requires_confirmation=true
+   - 不确定时优先 ask_user 或 speak
+   - 禁止直接执行危险动作
+
+5. 世界状态冲突处理
+   - 当世界状态显示可能存在冲突时
+   - 优先询问用户确认（ask_user）
+   - 不要自动执行可能重复或有风险的动作
+
+## 工具类型说明
+
+- execute: 执行具体动作
+- ask_user: 询问用户确认
+- speak: 语音/文本输出
+- query_world: 查询世界状态
+- query_knowledge: 查询知识库
+- noop: 空操作
+
+## 风险等级
+
+- low: 低风险，可自动执行
+- medium: 中风险，建议确认
+- high: 高风险，必须确认
+
+## 输出格式要求
+
+输出 JSON 必须包含以下字段：
+
+Plan 顶层字段：
+- plan_id: string (自动生成，唯一标识)
+- goal: string (计划目标描述)
+- reasoning: string (推理过程说明)
+- risk_level: "low" | "medium" | "high"
+- requires_confirmation: true | false
+- confirmation_message: string | null
+- steps: array (步骤数组)
+- status: "created" | "pending" | "executing" | "completed" | "failed"
+- source: string (计划来源，如 "llm"）
+- metadata: object (元数据)
+
+每个 step 必须包含以下字段：
+- step_id: number (步骤序号，从 1 开始)
+- action: string (能力名称)
+- tool_call_type: "execute" | "ask_user" | "speak" | "query_world" | "query_knowledge" | "noop"
+- parameters: object (执行参数)
+- preconditions: object (前置条件)
+- fallback: null | object (失败回退)
+- status: "pending" | "executing" | "completed" | "failed" | "skipped"
+- description: string (步骤描述)
+- requires_confirmation: true | false
+- confirmation_message: string | null
+- metadata: object (步骤元数据)
+
+重要：直接输出 JSON 对象，不要包裹在任何代码块或前后缀中。"""
+"""
+
+# 简单场景 Prompt 模板
+SIMPLE_SCENARIO_TEMPLATE = """## 用户意图
+{user_intent}
+
+## 当前世界状态
+{world_snapshot}
+
+## 可用能力
+{available_tools}
+
+{domain_rules_section}
+
+{recent_context_section}
+
+请根据以上信息，生成一个标准 Plan JSON。"""
+
+
+# 无可用能力时的 Prompt
+NO_TOOLS_TEMPLATE = """## 用户意图
+{user_intent}
+
+## 当前世界状态
+{world_snapshot}
+
+## 警告：没有可用能力
+
+你的系统目前没有任何可用能力。
+请生成一个 SPEAK plan，告知用户当前无法满足其请求。
+
+请生成一个标准 Plan JSON。"""
+
+
+# 意图不明确时的 Prompt
+UNCLEAR_INTENT_TEMPLATE = """## 用户意图
+{user_intent}
+
+## 当前世界状态
+{world_snapshot}
+
+## 可用能力
+{available_tools}
+
+## 警告：用户意图不明确
+
+用户请求不够明确，系统无法确定具体行动。
+请生成一个 ASK_USER plan，询问用户澄清其意图。
+
+请生成一个标准 Plan JSON。"""
+
+
+# =============================================================================
+# Planner Prompt 管理器
+# =============================================================================
+
+class PlannerPromptManager:
+    """Planner Prompt 管理器
+    
+    负责构造适合 LLM Planner 的 Prompt。
+    
+    属性：
+        system_instruction: 系统指令
+        include_reasoning: 是否在输出中包含推理过程
+    
+    示例：
+        >>> manager = PlannerPromptManager()
+        >>> prompt = manager.build_plan_prompt(
+        ...     user_intent="降温",
+        ...     world_snapshot={"temperature": 32},
+        ...     available_tools=["adjust_fan"],
+        ... )
+    """
+    
+    def __init__(
+        self,
+        system_instruction: str | None = None,
+        include_reasoning: bool = True,
+    ):
+        """初始化 Prompt 管理器
+        
+        Args:
+            system_instruction: 自定义系统指令，None 则使用默认指令
+            include_reasoning: 是否包含推理过程
+        """
+        self.system_instruction = system_instruction or PLANNER_SYSTEM_INSTRUCTION
+        self.include_reasoning = include_reasoning
+    
+    def build_plan_prompt(
+        self,
+        user_intent: str,
+        world_snapshot: dict[str, Any],
+        available_tools: list[str],
+        domain_rules: dict[str, Any] | None = None,
+        recent_context: dict[str, Any] | None = None,
+    ) -> str:
+        """构建 Plan Prompt
+        
+        主要入口方法。根据输入构建完整的 prompt。
+
+        设计原则：
+        - 专注于 prompt 构造，不做复杂的业务判断
+        - 复杂场景判断由 planner.py 负责
+        - 只保留必要的轻量分支：
+          1. 无可用工具
+          2. 意图明显不清晰
+          3. 默认普通 prompt
+
+        Args:
+            user_intent: 用户意图
+            world_snapshot: 世界状态快照
+            available_tools: 可用能力列表
+            domain_rules: 领域规则
+            recent_context: 最近上下文（传递给 LLM 参考）
+        
+        Returns:
+            完整的 prompt 字符串
+        """
+        domain_rules = domain_rules or {}
+        recent_context = recent_context or {}
+
+        # 轻量判断分支
+        if not available_tools:
+            scenario_prompt = self._build_no_tools_prompt(user_intent, world_snapshot)
+        elif self._is_unclear_intent_simple(user_intent):
+            scenario_prompt = self._build_unclear_intent_prompt(
+                user_intent, world_snapshot, available_tools
+            )
+        else:
+            scenario_prompt = self._build_default_prompt(
+                user_intent, world_snapshot, available_tools, domain_rules, recent_context
+            )
+
+        # 组合完整 prompt
+        return f"{self.system_instruction}\n\n{scenario_prompt}"
+
+    def _is_unclear_intent_simple(self, user_intent: str) -> bool:
+        """简单判断意图是否不清晰
+        
+        轻量方法，只做最基础的检查。
+        复杂判断由 planner.py 负责。
+        
+        Args:
+            user_intent: 用户意图
+        
+        Returns:
+            意图是否不清晰
+        """
+        # 意图过短
+        if len(user_intent.strip()) < 2:
+            return True
+        return False
+    
+    def build_system_instruction(self) -> str:
+        """获取系统指令
+        
+        Returns:
+            系统指令字符串
+        """
+        return self.system_instruction
+
+    def _build_default_prompt(
+        self,
+        user_intent: str,
+        world_snapshot: dict[str, Any],
+        available_tools: list[str],
+        domain_rules: dict[str, Any],
+        recent_context: dict[str, Any],
+    ) -> str:
+        """构建默认 Prompt
+        
+        统一处理大多数场景的 prompt 构造。
+        
+        Args:
+            user_intent: 用户意图
+            world_snapshot: 世界状态
+            available_tools: 可用能力
+            domain_rules: 领域规则
+            recent_context: 最近上下文
+        
+        Returns:
+            场景 prompt 字符串
+        """
+        # 格式化世界状态
+        if isinstance(world_snapshot, dict):
+            world_str = json.dumps(world_snapshot, ensure_ascii=False, indent=2)
+        else:
+            world_str = str(world_snapshot)
+
+        # 格式化可用工具
+        tools_str = ", ".join(f'"{t}"' for t in available_tools)
+
+        # 格式化领域规则
+        if domain_rules:
+            rules_str = f"\n\n## 领域规则\n{json.dumps(domain_rules, ensure_ascii=False, indent=2)}"
+        else:
+            rules_str = ""
+
+        # 格式化最近上下文
+        if recent_context:
+            context_str = f"\n\n## 历史上下文\n{json.dumps(recent_context, ensure_ascii=False, indent=2)}"
+        else:
+            context_str = ""
+
+        return SIMPLE_SCENARIO_TEMPLATE.format(
+            user_intent=user_intent,
+            world_snapshot=world_str,
+            available_tools=tools_str,
+            domain_rules_section=rules_str,
+            recent_context_section=context_str,
+        )
+    
+    def _build_no_tools_prompt(
+        self,
+        user_intent: str,
+        world_snapshot: dict[str, Any],
+    ) -> str:
+        """构建无可用工具时的 Prompt"""
+        if isinstance(world_snapshot, dict):
+            world_str = json.dumps(world_snapshot, ensure_ascii=False, indent=2)
+        else:
+            world_str = str(world_snapshot)
+        
+        return NO_TOOLS_TEMPLATE.format(
+            user_intent=user_intent,
+            world_snapshot=world_str,
+        )
+    
+    def _build_unclear_intent_prompt(
+        self,
+        user_intent: str,
+        world_snapshot: dict[str, Any],
+        available_tools: list[str],
+    ) -> str:
+        """构建意图不明确时的 Prompt"""
+        if isinstance(world_snapshot, dict):
+            world_str = json.dumps(world_snapshot, ensure_ascii=False, indent=2)
+        else:
+            world_str = str(world_snapshot)
+        
+        tools_str = ", ".join(f'"{t}"' for t in available_tools)
+        
+        return UNCLEAR_INTENT_TEMPLATE.format(
+            user_intent=user_intent,
+            world_snapshot=world_str,
+            available_tools=tools_str,
+        )
+
+
+# =============================================================================
+# 主程序入口（测试示例）
+# =============================================================================
+
+if __name__ == "__main__":
+    print("=" * 70)
+    print("Planner Prompt Manager 测试")
+    print("=" * 70)
+    
+    manager = PlannerPromptManager()
+    
+    # 测试 1：简单场景
+    print("\n[测试 1] 简单场景 - 降温")
+    print("-" * 40)
+    prompt = manager.build_plan_prompt(
+        user_intent="打开风扇降温",
+        world_snapshot={"temperature": 32, "humidity": 70},
+        available_tools=["adjust_fan", "speak", "query"],
+        domain_rules={"high_risk_actions": ["turn_off"]},
+    )
+    print(f"Prompt 长度: {len(prompt)} 字符")
+    print(f"包含 system instruction: {'AI Agent Planner' in prompt}")
+    print(f"包含 JSON 格式: {'plan_id' in prompt}")
+    
+    # 测试 2：意图不明确
+    print("\n[测试 2] 意图不明确")
+    print("-" * 40)
+    prompt = manager.build_plan_prompt(
+        user_intent="那个事情",
+        world_snapshot={"status": "normal"},
+        available_tools=["query", "speak"],
+    )
+    print(f"Prompt 长度: {len(prompt)} 字符")
+    print(f"包含 ASK_USER 提示: {'询问用户' in prompt or 'ask_user' in prompt.lower()}")
+    
+    # 测试 3：无可用工具
+    print("\n[测试 3] 无可用工具")
+    print("-" * 40)
+    prompt = manager.build_plan_prompt(
+        user_intent="打开空调",
+        world_snapshot={"temperature": 30},
+        available_tools=[],
+    )
+    print(f"Prompt 长度: {len(prompt)} 字符")
+    print(f"包含无可用能力提示: {'没有可用能力' in prompt}")
+    
+    # 测试 4：正常意图（含历史上下文）
+    print("\n[测试 4] 正常意图")
+    print("-" * 40)
+    prompt = manager.build_plan_prompt(
+        user_intent="先降温然后喂食",
+        world_snapshot={"temperature": 32, "hunger_level": 80},
+        available_tools=["adjust_fan", "feed", "speak"],
+        domain_rules={"high_risk_actions": ["turn_off"], "medium_risk_actions": ["adjust"]},
+        recent_context={"recent_actions": ["adjust_fan"]},
+    )
+    print(f"Prompt 长度: {len(prompt)} 字符")
+    print(f"包含历史上下文: {'历史上下文' in prompt}")
+
+    # 测试 5：打印完整 prompt 示例
+    print("\n[测试 5] 完整 Prompt 示例")
+    print("-" * 40)
+    prompt = manager.build_plan_prompt(
+        user_intent="打开风扇降温",
+        world_snapshot={"temperature": 32, "humidity": 70},
+        available_tools=["adjust_fan", "speak"],
+        domain_rules={"high_risk_actions": ["turn_off"]},
+    )
+    # 只打印前 500 字符
+    print(prompt[:500])
+    print("...")
+
+    # 测试 6：验证 system instruction 中没有 markdown 代码块模板
+    print("\n[测试 6] 验证 JSON 输出约束")
+    print("-" * 40)
+    system_instr = manager.build_system_instruction()
+    print(f"包含 markdown 代码块: {'```' in system_instr}")
+    print(f"包含禁止输出 markdown: {'禁止输出 markdown' in system_instr}")
+    print(f"包含禁止解释文字: {'禁止输出解释文字' in system_instr}")
+
+    print("\n" + "=" * 70)
+    print("Planner Prompt Manager 测试完成")
+    print("=" * 70)
+

brain/tool_protocol.py

@@ -0,0 +1,846 @@
+"""
+tool_protocol.py - AI Agent 标准协议层
+
+该模块定义 AI Agent 系统中 "计划" 和 "动作" 的标准数据结构，
+用于连接 WorldModel → Planner → Executor → Capability。
+
+设计原则：
+    1. 协议层只定义结构，不执行任何 Capability
+    2. 协议层不负责 ROS 通信，不依赖 rclpy
+    3. 完全通用，无场景硬编码
+    4. JSON 序列化友好，适合 LLM 直接输出
+
+使用方式：
+    from tool_protocol import Plan, PlanStep, ExecutionResult
+
+    # 创建计划
+    plan = Plan(plan_id="plan_001", goal="执行任务")
+    plan.add_step(PlanStep(step_id=1, action="action_name", parameters={}))
+    
+    # 序列化为 JSON
+    json_data = plan.to_dict()
+    
+    # 从 JSON 恢复
+    plan = Plan.from_dict(json_data)
+"""
+
+from __future__ import annotations
+
+import time
+import uuid
+from dataclasses import dataclass, field
+from enum import Enum
+from typing import Any, Optional
+
+
+# =============================================================================
+# 枚举定义
+# =============================================================================
+
+class RiskLevel(Enum):
+    """计划风险等级枚举"""
+    LOW = "low"
+    MEDIUM = "medium"
+    HIGH = "high"
+
+
+class StepStatus(Enum):
+    """单个步骤状态枚举"""
+    PENDING = "pending"
+    RUNNING = "running"
+    SUCCESS = "success"
+    FAILED = "failed"
+    SKIPPED = "skipped"
+    WAIT_CONFIRMATION = "wait_confirmation"
+
+
+class PlanStatus(Enum):
+    """整体计划状态枚举"""
+    CREATED = "created"
+    APPROVED = "approved"
+    EXECUTING = "executing"
+    COMPLETED = "completed"
+    FAILED = "failed"
+    CANCELLED = "cancelled"
+    WAIT_CONFIRMATION = "wait_confirmation"
+
+
+class ToolCallType(Enum):
+    """步骤动作类型枚举
+    
+    用于区分步骤的性质，帮助 Planner 和 Executor 理解步骤意图。
+    """
+    EXECUTE = "execute"           # 执行具体动作
+    ASK_USER = "ask_user"         # 询问用户确认
+    SPEAK = "speak"               # 语音/文本输出
+    QUERY_WORLD = "query_world"   # 查询世界状态
+    QUERY_KNOWLEDGE = "query_knowledge"  # 查询知识库
+    NOOP = "noop"                 # 空操作
+
+
+# =============================================================================
+# 数据类定义
+# =============================================================================
+
+@dataclass
+class PlanStep:
+    """计划步骤
+    
+    表示计划中的单个执行步骤，包含动作、参数、前置条件等信息。
+    
+    属性：
+        step_id: 步骤唯一标识，在一个 Plan 内必须唯一
+        action: 动作名称，如 "execute_action", "speak", "query_world"
+        tool_call_type: 动作类型，用于区分步骤性质
+        parameters: 动作参数字典
+        preconditions: 执行前检查条件
+        fallback: 失败后的回退动作名称，None 表示无回退
+        status: 当前步骤状态
+        description: 步骤描述说明
+        requires_confirmation: 是否需要用户确认
+        confirmation_message: 确认提示消息
+        metadata: 可扩展元数据
+    
+    示例：
+        >>> step = PlanStep(
+        ...     step_id=1,
+        ...     action="adjust_fan",
+        ...     tool_call_type=ToolCallType.EXECUTE,
+        ...     parameters={"fan_id": "fan_001", "speed": 3},
+        ...     description="调整风扇速度"
+        ... )
+        >>> step.update_status(StepStatus.RUNNING)
+    """
+    
+    step_id: int
+    action: str
+    parameters: dict = field(default_factory=dict)
+    preconditions: dict = field(default_factory=dict)
+    fallback: str | None = None
+    status: StepStatus = StepStatus.PENDING
+    description: str = ""
+    tool_call_type: ToolCallType = ToolCallType.EXECUTE
+    requires_confirmation: bool = False
+    confirmation_message: str | None = None
+    metadata: dict = field(default_factory=dict)
+    
+    def __post_init__(self) -> None:
+        """数据校验"""
+        if not self.action or not self.action.strip():
+            raise ValueError("action 不能为空")
+        if self.step_id < 0:
+            raise ValueError("step_id 必须为非负整数")
+    
+    def update_status(self, new_status: StepStatus) -> None:
+        """更新步骤状态
+        
+        Args:
+            new_status: 新的状态值
+        """
+        self.status = new_status
+    
+    def to_dict(self) -> dict[str, Any]:
+        """序列化为字典
+        
+        Returns:
+            包含所有字段的字典，Enum 转为字符串值
+        """
+        return {
+            "step_id": self.step_id,
+            "action": self.action,
+            "tool_call_type": self.tool_call_type.value,
+            "parameters": self.parameters,
+            "preconditions": self.preconditions,
+            "fallback": self.fallback,
+            "status": self.status.value,
+            "description": self.description,
+            "requires_confirmation": self.requires_confirmation,
+            "confirmation_message": self.confirmation_message,
+            "metadata": self.metadata,
+        }
+    
+    @classmethod
+    def from_dict(cls, data: dict[str, Any]) -> PlanStep:
+        """从字典反序列化
+        
+        Args:
+            data: 包含步骤数据的字典
+        
+        Returns:
+            PlanStep 实例
+        """
+        return cls(
+            step_id=data["step_id"],
+            action=data["action"],
+            tool_call_type=ToolCallType(data.get("tool_call_type", "execute")),
+            parameters=data.get("parameters", {}),
+            preconditions=data.get("preconditions", {}),
+            fallback=data.get("fallback"),
+            status=StepStatus(data.get("status", "pending")),
+            description=data.get("description", ""),
+            requires_confirmation=data.get("requires_confirmation", False),
+            confirmation_message=data.get("confirmation_message"),
+            metadata=data.get("metadata", {}),
+        )
+
+
+@dataclass
+class Plan:
+    """计划
+    
+    表示一个完整的执行计划，包含目标、步骤列表、风险等级等信息。
+    由 Planner 生成，由 Executor 执行。
+    
+    属性：
+        plan_id: 计划唯一标识
+        goal: 计划目标描述
+        reasoning: 推理过程说明
+        risk_level: 风险等级
+        requires_confirmation: 是否需要用户确认后执行
+        confirmation_message: 确认提示消息
+        steps: 步骤列表
+        status: 计划状态
+        created_at: 创建时间戳
+        source: 计划来源，如 "rule_engine", "llm", "hybrid"
+        metadata: 可扩展元数据
+    
+    示例：
+        >>> plan = Plan(
+        ...     plan_id="plan_001",
+        ...     goal="完成任务A",
+        ...     reasoning="因为条件满足",
+        ...     risk_level=RiskLevel.LOW,
+        ...     source="llm"
+        ... )
+        >>> plan.add_step(PlanStep(step_id=1, action="step1", parameters={}))
+        >>> plan.add_step(PlanStep(step_id=2, action="step2", parameters={}))
+        >>> pending = plan.get_pending_steps()
+    """
+    
+    plan_id: str
+    goal: str
+    reasoning: str = ""
+    risk_level: RiskLevel = RiskLevel.LOW
+    requires_confirmation: bool = False
+    confirmation_message: str | None = None
+    steps: list[PlanStep] = field(default_factory=list)
+    status: PlanStatus = PlanStatus.CREATED
+    created_at: float = field(default_factory=time.time)
+    source: str = "llm"
+    metadata: dict = field(default_factory=dict)
+    
+    def __post_init__(self) -> None:
+        """数据校验"""
+        if not self.plan_id or not self.plan_id.strip():
+            raise ValueError("plan_id 不能为空")
+        if not self.goal or not self.goal.strip():
+            raise ValueError("goal 不能为空")
+    
+    def add_step(self, step: PlanStep) -> None:
+        """添加步骤
+        
+        Args:
+            step: 要添加的 PlanStep
+        
+        Raises:
+            ValueError: 如果 step_id 已存在
+        """
+        existing_ids = {s.step_id for s in self.steps}
+        if step.step_id in existing_ids:
+            raise ValueError(f"step_id {step.step_id} 已存在，请使用唯一ID")
+        self.steps.append(step)
+    
+    def get_pending_steps(self) -> list[PlanStep]:
+        """获取所有待执行的步骤
+        
+        Returns:
+            状态为 PENDING 的步骤列表
+        """
+        return [s for s in self.steps if s.status == StepStatus.PENDING]
+    
+    def get_current_step(self) -> PlanStep | None:
+        """获取当前正在执行的步骤
+        
+        Returns:
+            第一个状态为 RUNNING 的步骤，如果没有则返回 None
+        """
+        for step in self.steps:
+            if step.status == StepStatus.RUNNING:
+                return step
+        return None
+    
+    def get_waiting_confirmation_steps(self) -> list[PlanStep]:
+        """获取所有等待确认的步骤
+        
+        Returns:
+            状态为 WAIT_CONFIRMATION 的步骤列表
+        """
+        return [s for s in self.steps if s.status == StepStatus.WAIT_CONFIRMATION]
+    
+    def is_finished(self) -> bool:
+        """判断计划是否完成
+        
+        完成条件：所有步骤都已执行（非 PENDING、RUNNING、WAIT_CONFIRMATION）
+        
+        Returns:
+            如果计划完成返回 True
+        """
+        return all(
+            s.status not in (StepStatus.PENDING, StepStatus.RUNNING, StepStatus.WAIT_CONFIRMATION)
+            for s in self.steps
+        )
+    
+    def is_success(self) -> bool:
+        """判断计划是否完全成功
+        
+        成功条件：
+        1. 至少存在一个步骤
+        2. 所有步骤状态都为 SUCCESS
+        
+        Returns:
+            如果所有步骤都成功返回 True
+        """
+        if not self.steps:
+            return False
+        return all(s.status == StepStatus.SUCCESS for s in self.steps)
+    
+    def get_failed_steps(self) -> list[PlanStep]:
+        """获取所有失败的步骤
+        
+        Returns:
+            状态为 FAILED 的步骤列表
+        """
+        return [s for s in self.steps if s.status == StepStatus.FAILED]
+    
+    def next_step_id(self) -> int:
+        """获取下一个可用的步骤 ID
+        
+        Returns:
+            新的 step_id（当前最大 ID + 1）
+        """
+        if not self.steps:
+            return 1
+        return max(s.step_id for s in self.steps) + 1
+    
+    def to_dict(self) -> dict[str, Any]:
+        """序列化为字典
+        
+        Returns:
+            包含所有字段的字典，Enum 转为字符串值
+        """
+        return {
+            "plan_id": self.plan_id,
+            "goal": self.goal,
+            "reasoning": self.reasoning,
+            "risk_level": self.risk_level.value,
+            "requires_confirmation": self.requires_confirmation,
+            "confirmation_message": self.confirmation_message,
+            "steps": [s.to_dict() for s in self.steps],
+            "status": self.status.value,
+            "created_at": self.created_at,
+            "source": self.source,
+            "metadata": self.metadata,
+        }
+    
+    @classmethod
+    def from_dict(cls, data: dict[str, Any]) -> Plan:
+        """从字典反序列化
+        
+        Args:
+            data: 包含计划数据的字典
+        
+        Returns:
+            Plan 实例
+        """
+        steps = [
+            PlanStep.from_dict(s) for s in data.get("steps", [])
+        ]
+        return cls(
+            plan_id=data["plan_id"],
+            goal=data["goal"],
+            reasoning=data.get("reasoning", ""),
+            risk_level=RiskLevel(data.get("risk_level", "low")),
+            requires_confirmation=data.get("requires_confirmation", False),
+            confirmation_message=data.get("confirmation_message"),
+            steps=steps,
+            status=PlanStatus(data.get("status", "created")),
+            created_at=data.get("created_at", time.time()),
+            source=data.get("source", "llm"),
+            metadata=data.get("metadata", {}),
+        )
+    
+    @classmethod
+    def create_new(
+        cls,
+        goal: str,
+        reasoning: str = "",
+        risk_level: RiskLevel = RiskLevel.LOW,
+        source: str = "llm",
+    ) -> Plan:
+        """创建新计划的便捷工厂方法
+        
+        Args:
+            goal: 计划目标
+            reasoning: 推理过程
+            risk_level: 风险等级
+            source: 计划来源
+        
+        Returns:
+            新的 Plan 实例
+        """
+        return cls(
+            plan_id=f"plan_{uuid.uuid4().hex[:8]}",
+            goal=goal,
+            reasoning=reasoning,
+            risk_level=risk_level,
+            source=source,
+        )
+
+
+@dataclass
+class ExecutionResult:
+    """执行结果
+    
+    表示 Executor 执行某个步骤后的结果。
+    用于回传执行状态、输出数据等信息。
+    
+    属性：
+        plan_id: 关联的计划 ID
+        step_id: 执行的步骤 ID
+        success: 是否执行成功
+        status: 执行状态
+        message: 执行消息描述
+        output: 执行输出数据
+        timestamp: 执行时间戳
+    
+    示例：
+        >>> result = ExecutionResult(
+        ...     plan_id="plan_001",
+        ...     step_id=1,
+        ...     success=True,
+        ...     status=StepStatus.SUCCESS,
+        ...     message="执行成功",
+        ...     output={"result": "ok"}
+        ... )
+        >>> data = result.to_dict()
+    """
+    
+    plan_id: str
+    step_id: int
+    success: bool
+    status: StepStatus
+    message: str = ""
+    output: dict = field(default_factory=dict)
+    timestamp: float = field(default_factory=time.time)
+    
+    def __post_init__(self) -> None:
+        """数据校验"""
+        if not self.plan_id or not self.plan_id.strip():
+            raise ValueError("plan_id 不能为空")
+        if self.step_id < 0:
+            raise ValueError("step_id 必须为非负整数")
+        # success 与 status 的一致性校验
+        if self.status == StepStatus.SUCCESS and not self.success:
+            raise ValueError("status=SUCCESS 时 success 必须为 True")
+        if self.status == StepStatus.FAILED and self.success:
+            raise ValueError("status=FAILED 时 success 必须为 False")
+    
+    def to_dict(self) -> dict[str, Any]:
+        """序列化为字典
+        
+        Returns:
+            包含所有字段的字典，Enum 转为字符串值
+        """
+        return {
+            "plan_id": self.plan_id,
+            "step_id": self.step_id,
+            "success": self.success,
+            "status": self.status.value,
+            "message": self.message,
+            "output": self.output,
+            "timestamp": self.timestamp,
+        }
+    
+    @classmethod
+    def from_dict(cls, data: dict[str, Any]) -> ExecutionResult:
+        """从字典反序列化
+        
+        Args:
+            data: 包含执行结果数据的字典
+        
+        Returns:
+            ExecutionResult 实例
+        """
+        return cls(
+            plan_id=data["plan_id"],
+            step_id=data["step_id"],
+            success=data.get("success", False),
+            status=StepStatus(data.get("status", "failed")),
+            message=data.get("message", ""),
+            output=data.get("output", {}),
+            timestamp=data.get("timestamp", time.time()),
+        )
+    
+    @classmethod
+    def success_result(
+        cls,
+        plan_id: str,
+        step_id: int,
+        message: str = "",
+        output: dict | None = None,
+    ) -> ExecutionResult:
+        """创建成功执行结果的便捷方法
+        
+        Args:
+            plan_id: 计划 ID
+            step_id: 步骤 ID
+            message: 成功消息
+            output: 输出数据
+        
+        Returns:
+            成功的 ExecutionResult 实例
+        """
+        return cls(
+            plan_id=plan_id,
+            step_id=step_id,
+            success=True,
+            status=StepStatus.SUCCESS,
+            message=message or "执行成功",
+            output=output or {},
+        )
+    
+    @classmethod
+    def failure_result(
+        cls,
+        plan_id: str,
+        step_id: int,
+        message: str,
+        output: dict | None = None,
+    ) -> ExecutionResult:
+        """创建失败执行结果的便捷方法
+        
+        Args:
+            plan_id: 计划 ID
+            step_id: 步骤 ID
+            message: 失败原因
+            output: 输出数据
+        
+        Returns:
+            失败的 ExecutionResult 实例
+        """
+        return cls(
+            plan_id=plan_id,
+            step_id=step_id,
+            success=False,
+            status=StepStatus.FAILED,
+            message=message,
+            output=output or {},
+        )
+    
+    @classmethod
+    def skipped_result(
+        cls,
+        plan_id: str,
+        step_id: int,
+        message: str = "步骤已跳过",
+    ) -> ExecutionResult:
+        """创建跳过执行结果的便捷方法
+        
+        Args:
+            plan_id: 计划 ID
+            step_id: 步骤 ID
+            message: 跳过原因
+        
+        Returns:
+            跳过的 ExecutionResult 实例
+        """
+        return cls(
+            plan_id=plan_id,
+            step_id=step_id,
+            success=True,
+            status=StepStatus.SKIPPED,
+            message=message,
+            output={},
+        )
+
+
+# =============================================================================
+# 工具函数
+# =============================================================================
+
+def create_plan_from_llm_response(
+    llm_output: dict[str, Any],
+    source: str = "llm",
+) -> Plan:
+    """从 LLM 输出创建 Plan 的辅助函数
+    
+    将 LLM 返回的 JSON 结构转换为 Plan 对象。
+    
+    Args:
+        llm_output: LLM 返回的字典数据
+        source: 计划来源
+    
+    Returns:
+        Plan 实例
+    
+    示例：
+        >>> llm_response = {
+        ...     "goal": "完成任务",
+        ...     "reasoning": "分析后决定",
+        ...     "risk_level": "low",
+        ...     "steps": [
+        ...         {"step_id": 1, "action": "step1", "parameters": {}}
+        ...     ]
+        ... }
+        >>> plan = create_plan_from_llm_response(llm_response)
+    """
+    plan = Plan(
+        plan_id=llm_output.get("plan_id", f"plan_{uuid.uuid4().hex[:8]}"),
+        goal=llm_output["goal"],
+        reasoning=llm_output.get("reasoning", ""),
+        risk_level=RiskLevel(llm_output.get("risk_level", "low")),
+        requires_confirmation=llm_output.get("requires_confirmation", False),
+        confirmation_message=llm_output.get("confirmation_message"),
+        source=source,
+        metadata=llm_output.get("metadata", {}),
+    )
+    
+    for step_data in llm_output.get("steps", []):
+        step = PlanStep.from_dict(step_data)
+        plan.add_step(step)
+    
+    return plan
+
+
+# =============================================================================
+# 主程序入口（测试示例）
+# =============================================================================
+
+if __name__ == "__main__":
+    import json
+    
+    print("=" * 70)
+    print("Tool Protocol 测试演示")
+    print("=" * 70)
+    
+    # -------------------------------------------------------------------------
+    # 1. 创建 Plan
+    # -------------------------------------------------------------------------
+    print("\n[1] 创建 Plan")
+    print("-" * 40)
+    
+    plan = Plan.create_new(
+        goal="执行环境监测任务",
+        reasoning="检测到环境参数异常，需要执行降温操作",
+        risk_level=RiskLevel.MEDIUM,
+        source="llm",
+    )
+    print(f"创建计划: {plan.plan_id}")
+    print(f"目标: {plan.goal}")
+    print(f"风险等级: {plan.risk_level.value}")
+    print(f"来源: {plan.source}")
+    
+    # -------------------------------------------------------------------------
+    # 2. 添加 PlanStep
+    # -------------------------------------------------------------------------
+    print("\n[2] 添加 PlanStep")
+    print("-" * 40)
+    
+    # 步骤1：查询世界状态
+    step1 = PlanStep(
+        step_id=1,
+        action="query_world_state",
+        tool_call_type=ToolCallType.QUERY_WORLD,
+        parameters={"query": "temperature"},
+        description="查询当前温度",
+        metadata={"source": "world_model"},
+    )
+    plan.add_step(step1)
+    
+    # 步骤2：询问用户确认
+    step2 = PlanStep(
+        step_id=2,
+        action="confirm_action",
+        tool_call_type=ToolCallType.ASK_USER,
+        parameters={"prompt": "是否启动降温设备？"},
+        description="等待用户确认",
+        requires_confirmation=True,
+        confirmation_message="当前温度过高，是否启动降温？",
+    )
+    plan.add_step(step2)
+    
+    # 步骤3：执行动作
+    step3 = PlanStep(
+        step_id=3,
+        action="adjust_cooling",
+        tool_call_type=ToolCallType.EXECUTE,
+        parameters={"device_id": "cooler_001", "level": 3},
+        preconditions={"temperature_above": 30},
+        fallback="alert_operator",
+        description="调整降温设备",
+        metadata={"device": "cooler", "priority": "high"},
+    )
+    plan.add_step(step3)
+    
+    # 步骤4：语音通知
+    step4 = PlanStep(
+        step_id=4,
+        action="speak_notification",
+        tool_call_type=ToolCallType.SPEAK,
+        parameters={"message": "降温设备已启动", "volume": 0.8},
+        description="通知操作员",
+    )
+    plan.add_step(step4)
+    
+    print(f"添加了 {len(plan.steps)} 个步骤")
+    for step in plan.steps:
+        print(f"  - Step {step.step_id}: {step.action} ({step.tool_call_type.value})")
+    
+    # -------------------------------------------------------------------------
+    # 3. 序列化为 Dict/JSON
+    # -------------------------------------------------------------------------
+    print("\n[3] 序列化为 Dict/JSON")
+    print("-" * 40)
+    
+    plan_dict = plan.to_dict()
+    plan_json = json.dumps(plan_dict, indent=2, ensure_ascii=False)
+    print("Plan JSON:")
+    print(plan_json[:500] + "..." if len(plan_json) > 500 else plan_json)
+    
+    # -------------------------------------------------------------------------
+    # 4. 从 Dict 恢复
+    # -------------------------------------------------------------------------
+    print("\n[4] 从 Dict 恢复 Plan")
+    print("-" * 40)
+    
+    restored_plan = Plan.from_dict(plan_dict)
+    print(f"恢复计划 ID: {restored_plan.plan_id}")
+    print(f"恢复步骤数: {len(restored_plan.steps)}")
+    print(f"恢复风险等级: {restored_plan.risk_level.value}")
+    
+    # 验证恢复正确性
+    assert plan.plan_id == restored_plan.plan_id
+    assert len(plan.steps) == len(restored_plan.steps)
+    print("✓ 序列化/反序列化验证通过")
+    
+    # -------------------------------------------------------------------------
+    # 5. Plan 操作演示
+    # -------------------------------------------------------------------------
+    print("\n[5] Plan 操作演示")
+    print("-" * 40)
+    
+    # 更新步骤状态
+    print(f"初始待执行步骤数: {len(plan.get_pending_steps())}")
+    
+    step1.update_status(StepStatus.RUNNING)
+    print(f"Step 1 状态更新为: {step1.status.value}")
+    
+    step1.update_status(StepStatus.SUCCESS)
+    print(f"Step 1 状态更新为: {step1.status.value}")
+    
+    step2.update_status(StepStatus.WAIT_CONFIRMATION)
+    waiting = plan.get_waiting_confirmation_steps()
+    print(f"等待确认的步骤: {[s.step_id for s in waiting]}")
+    
+    print(f"计划是否完成: {plan.is_finished()}")
+    print(f"计划是否成功: {plan.is_success()}")
+    
+    # -------------------------------------------------------------------------
+    # 6. ExecutionResult 演示
+    # -------------------------------------------------------------------------
+    print("\n[6] ExecutionResult 演示")
+    print("-" * 40)
+    
+    # 创建成功结果
+    result1 = ExecutionResult.success_result(
+        plan_id=plan.plan_id,
+        step_id=1,
+        message="温度查询成功",
+        output={"temperature": 32.5, "humidity": 65},
+    )
+    print(f"成功结果: {result1.to_dict()}")
+    
+    # 创建失败结果
+    result2 = ExecutionResult.failure_result(
+        plan_id=plan.plan_id,
+        step_id=3,
+        message="设备通信失败",
+        output={"error_code": "E503", "device": "cooler_001"},
+    )
+    print(f"失败结果: {result2.to_dict()}")
+    
+    # 创建跳过结果
+    result3 = ExecutionResult.skipped_result(
+        plan_id=plan.plan_id,
+        step_id=4,
+        message="条件不满足，跳过通知",
+    )
+    print(f"跳过结果: {result3.to_dict()}")
+    
+    # -------------------------------------------------------------------------
+    # 7. 从 LLM 输出创建 Plan
+    # -------------------------------------------------------------------------
+    print("\n[7] 从 LLM 输出创建 Plan")
+    print("-" * 40)
+    
+    llm_response = {
+        "goal": "自动巡检任务",
+        "reasoning": "定时任务触发，执行标准巡检流程",
+        "risk_level": "low",
+        "requires_confirmation": False,
+        "steps": [
+            {
+                "step_id": 1,
+                "action": "move_to_location",
+                "tool_call_type": "execute",
+                "parameters": {"target": "zone_A"},
+                "description": "移动到巡检区域A",
+            },
+            {
+                "step_id": 2,
+                "action": "capture_sensor_data",
+                "tool_call_type": "execute",
+                "parameters": {"sensors": ["temp", "humidity"]},
+                "description": "采集传感器数据",
+            },
+            {
+                "step_id": 3,
+                "action": "speak_report",
+                "tool_call_type": "speak",
+                "parameters": {"content": "巡检完成"},
+                "description": "报告巡检结果",
+            },
+        ],
+    }
+    
+    llm_plan = create_plan_from_llm_response(llm_response, source="llm")
+    print(f"从 LLM 创建计划: {llm_plan.plan_id}")
+    print(f"目标: {llm_plan.goal}")
+    print(f"步骤数: {len(llm_plan.steps)}")
+    
+    # -------------------------------------------------------------------------
+    # 8. 枚举使用演示
+    # -------------------------------------------------------------------------
+    print("\n[8] 枚举使用演示")
+    print("-" * 40)
+    
+    print("RiskLevel 枚举:")
+    for level in RiskLevel:
+        print(f"  - {level.name} = {level.value}")
+    
+    print("\nStepStatus 枚举:")
+    for status in StepStatus:
+        print(f"  - {status.name} = {status.value}")
+    
+    print("\nPlanStatus 枚举:")
+    for status in PlanStatus:
+        print(f"  - {status.name} = {status.value}")
+    
+    print("\nToolCallType 枚举:")
+    for tool_type in ToolCallType:
+        print(f"  - {tool_type.name} = {tool_type.value}")
+    
+    print("\n" + "=" * 70)
+    print("测试演示完成")
+    print("=" * 70)
+

capabilities/camera/src/camera/CMakeLists.txt

@@ -0,0 +1,96 @@
+cmake_minimum_required(VERSION 3.8)
+project(camera)
+
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+  add_compile_options(-Wall -Wextra -Wpedantic)
+endif()
+
+set(CMAKE_CXX_STANDARD 17)
+set(CMAKE_CXX_STANDARD_REQUIRED ON)
+
+find_package(ament_cmake REQUIRED)
+find_package(rclcpp REQUIRED)
+find_package(sensor_msgs REQUIRED)
+find_package(OpenCV REQUIRED)
+
+set(HOBOT_INCLUDE_DIR "/usr/hobot/include")
+set(HOBOT_LIB_DIR "/usr/hobot/lib")
+set(HOBOT_CJSON_INCLUDE_DIR "/usr/include/cjson")
+
+# Allow overriding Hobot backend enablement
+option(CAMERA_ENABLE_HOBOT "Enable Hobot VIO/VFLOW backend" ON)
+
+# Robust Hobot detection for this board:
+# - Libraries live in /usr/hobot/lib (sample Makefile links libmultimedia.so)
+# - Some headers live in /usr/include (e.g. sp_vio.h)
+set(HAVE_HOBOT FALSE)
+if(CAMERA_ENABLE_HOBOT AND EXISTS "${HOBOT_LIB_DIR}/libmultimedia.so")
+  set(HAVE_HOBOT TRUE)
+endif()
+
+message(STATUS "CAMERA_ENABLE_HOBOT=${CAMERA_ENABLE_HOBOT} HAVE_HOBOT=${HAVE_HOBOT} HOBOT_INCLUDE_DIR=${HOBOT_INCLUDE_DIR} HOBOT_LIB_DIR=${HOBOT_LIB_DIR}")
+
+include_directories(
+  include
+  ${OpenCV_INCLUDE_DIRS}
+)
+
+add_executable(camera_node
+  src/camera_node.cpp
+  src/mipi_vflow_backend.cpp
+)
+
+ament_target_dependencies(camera_node
+  rclcpp
+  sensor_msgs
+)
+
+target_link_libraries(camera_node
+  ${OpenCV_LIBS}
+)
+
+target_compile_definitions(camera_node PRIVATE
+  $<$<BOOL:${HAVE_HOBOT}>:CAMERA_HAVE_HOBOT=1>
+)
+
+if(HAVE_HOBOT)
+  target_include_directories(camera_node PRIVATE
+    ${HOBOT_INCLUDE_DIR}
+    ${HOBOT_INCLUDE_DIR}/aarch64-linux-gnu
+    /usr/include
+    ${HOBOT_CJSON_INCLUDE_DIR}
+    /app/multimedia_samples/utils
+    /app/multimedia_samples
+    /app/multimedia_samples/vp_sensors
+    /app/multimedia_samples/vp_sensors/include
+  )
+
+  file(GLOB_RECURSE VP_SENSORS_SRCS
+    /app/multimedia_samples/vp_sensors/*.c
+  )
+
+  if(VP_SENSORS_SRCS)
+    target_sources(camera_node PRIVATE ${VP_SENSORS_SRCS})
+  else()
+    message(WARNING "vp_sensors sources not found under /app/multimedia_samples/vp_sensors; MIPI backend may fail to link")
+  endif()
+
+  target_link_directories(camera_node PRIVATE
+    ${HOBOT_LIB_DIR}
+  )
+
+  target_link_libraries(camera_node
+    vpf
+    vio
+    cam
+    hbmem
+    multimedia
+  )
+endif()
+
+install(TARGETS camera_node
+  DESTINATION lib/${PROJECT_NAME}
+)
+
+ament_package()
+

capabilities/camera/src/camera/README.md

@@ -0,0 +1,125 @@
+# camera (ROS2 Humble)
+
+面向低延迟的彩色相机 ROS2 节点（默认 MJPEG），支持：
+
+- 按摄像头名称筛选
+- 按稳定标识（`/dev/v4l/by-id` 或 `/dev/v4l/by-path`）筛选
+- 按 `width` 自动匹配相机支持分辨率
+- USB / MIPI 类型筛选
+
+发布话题：
+
+- `/camera/image_raw` (`sensor_msgs/msg/Image`, `bgr8`)
+
+---
+
+## 1. 编译
+
+```bash
+cd ~/opt/dev/project/aiagent/capabilities
+source /opt/ros/humble/setup.bash
+colcon build --packages-select camera --cmake-args -DCMAKE_BUILD_TYPE=Release
+source install/setup.bash
+```
+
+---
+
+## 2. 启动
+
+### 2.1 默认启动（自动选择）
+
+```bash
+ros2 run camera camera_node
+```
+
+### 2.2 按名称选择（示例）
+
+```bash
+ros2 run camera camera_node --ros-args \
+  -p camera_name:="USB Camera" \
+  -p width:=640 \
+  -p fps:=30 \
+  -p output_topic:=/camera/color/image_raw
+```
+
+### 2.3 按稳定标识选择（推荐）
+
+先查看：
+
+```bash
+ls -l /dev/v4l/by-id | cat
+ls -l /dev/v4l/by-path | cat
+```
+
+启动时：
+
+```bash
+ros2 run camera camera_node --ros-args \
+  -p stable_id:="usb-046d" \
+  -p width:=640
+```
+
+### 2.4 明确指定设备节点
+
+```bash
+ros2 run camera camera_node --ros-args -p device_path:=/dev/video0 -p width:=640
+```
+
+### 2.5 指定总线类型
+
+```bash
+# 仅 USB
+ros2 run camera camera_node --ros-args -p bus_type:=usb -p width:=640
+
+# 仅 MIPI
+ros2 run camera camera_node --ros-args -p bus_type:=mipi -p width:=640
+```
+
+---
+
+## 3. 参数说明
+
+- `frame_id` (string, 默认 `camera_link`)
+- `bus_type` (string, 默认 `auto`): `auto` / `usb` / `mipi`
+- `camera_name` (string, 默认空): 按名称子串匹配
+- `stable_id` (string, 默认空): 按 by-id/by-path 名称子串匹配
+- `device_path` (string, 默认空): 显式设备，如 `/dev/video0`
+- `width` (int, 默认 `640`): 目标输出宽度，自动选择最匹配分辨率
+- `height` (int, 默认 `0`): 高度提示（可选）
+- `fps` (int, 默认 `30`): 采集帧率参数
+- `pixel_format` (string, 默认 `MJPG`)
+- `output_topic` (string, 默认 `/camera/image_raw`): 输出话题名
+- `topic_name` (string, 默认空): `output_topic` 的别名，若非空则优先使用
+
+分辨率策略：
+
+- 优先选择与 `width` 最接近的 MJPEG 离散分辨率；
+- 若提供 `height`，会同时考虑高度接近度；
+- 若设备未枚举到 MJPEG 分辨率，则回退到请求值。
+
+---
+
+## 4. 低延迟建议
+
+1. 使用 `pixel_format=MJPG`（默认）。
+2. 节点内已设置 `CAP_PROP_BUFFERSIZE=1`，减少缓存堆积。
+3. 订阅端使用 `best_effort`、小队列。
+4. 降低分辨率与 fps 以获得更稳时延。
+5. 避免系统中多个进程同时占用同一摄像头。
+
+---
+
+## 5. 运行检查
+
+```bash
+ros2 topic list
+ros2 topic hz /camera/image_raw
+ros2 topic echo /camera/image_raw --once
+```
+
+可视化：
+
+```bash
+ros2 run rqt_image_view rqt_image_view
+```
+