#!/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()

