Initial commit: 无人机行为规划后端系统

Made-with: Cursor

src/drone_planning/__init__.py

@@ -0,0 +1,6 @@
+"""
+无人机行为规划后端系统
+从自然语言到无人机行为树 JSON 的四层流水线
+"""
+
+__version__ = "0.1.0"

src/drone_planning/api/__init__.py

@@ -0,0 +1 @@
+"""API 模块"""

src/drone_planning/api/routes.py

@@ -0,0 +1,115 @@
+"""
+API 路由 - POST /api/plan
+
+全链路：Router -> (Fast-Path 短路) | Composer -> Planner -> Tree Wrapper
+"""
+
+from __future__ import annotations
+
+import time
+from typing import Any
+
+from fastapi import APIRouter, HTTPException
+from pydantic import BaseModel
+
+from drone_planning.execution.tree_wrapper import parse_json_to_tree, tree_to_ascii, wrap_and_build_tree
+from drone_planning.pipeline.composer import _load_node_schema, build_system_prompt
+from drone_planning.pipeline.planner import plan
+from drone_planning.pipeline.router import route
+from drone_planning.rag.retriever import RAGRetriever
+
+router = APIRouter(prefix="/api", tags=["planning"])
+
+
+class PlanRequest(BaseModel):
+    text: str = ""
+
+
+@router.post("/plan")
+def api_plan(body: PlanRequest) -> dict[str, Any]:
+    """
+    自然语言 -> 行为树规划
+
+    请求体: {"text": "用户自然语言指令"}
+    """
+    user_text = body.text or ""
+    timing: dict[str, float] = {}
+
+    try:
+        # Layer 1: 意图路由
+        t0 = time.perf_counter()
+        router_result = route(user_text)
+        timing["Layer1_Router"] = (time.perf_counter() - t0) * 1000
+
+        if router_result.is_fast_path:
+            return {
+                "success": True,
+                "fast_path": True,
+                "intents": router_result.intents,
+                "entities": router_result.entities,
+                "message": "已短路，直接执行原子命令",
+                "tree_json": None,
+                "tree_ascii": None,
+                "timing_ms": timing,
+            }
+
+        # RAG 检索（阶段二）
+        t0 = time.perf_counter()
+        retriever = RAGRetriever()
+        rag_context = retriever.retrieve_context(
+            intents=router_result.intents,
+            entities=router_result.entities,
+            user_text=user_text,
+        )
+        timing["RAG_检索"] = (time.perf_counter() - t0) * 1000
+
+        # Layer 2: Composer
+        t0 = time.perf_counter()
+        schema_json = _load_node_schema()
+        system_prompt, composer_meta = build_system_prompt(
+            intents=router_result.intents,
+            entities=router_result.entities,
+            schema_json=schema_json,
+            rag_context=rag_context,
+        )
+        timing["Layer2_Composer"] = (time.perf_counter() - t0) * 1000
+
+        # Layer 3: Planner（支持 Tool 循环）
+        t0 = time.perf_counter()
+        tool_call_log: list[dict[str, Any]] = []
+        tree_json = plan(
+            system_prompt=system_prompt,
+            user_text=user_text,
+            tool_call_log=tool_call_log,
+        )
+        timing["Layer3_Planner"] = (time.perf_counter() - t0) * 1000
+
+        # Layer 4: 解析 + 包装
+        t0 = time.perf_counter()
+        root_node = tree_json.get("root") or tree_json
+        if isinstance(root_node, dict):
+            business_tree = parse_json_to_tree(root_node)
+        else:
+            raise ValueError("Planner 返回的 root 格式异常")
+
+        final_tree = wrap_and_build_tree(business_tree)
+        tree_ascii = tree_to_ascii(final_tree)
+        timing["Layer4_Execution"] = (time.perf_counter() - t0) * 1000
+
+        return {
+            "success": True,
+            "fast_path": False,
+            "intents": router_result.intents,
+            "entities": router_result.entities,
+            "rag_context": rag_context,
+            "tool_call_log": tool_call_log,
+            "tree_json": tree_json,
+            "tree_ascii": tree_ascii,
+            "composer_meta": composer_meta,
+            "timing_ms": timing,
+        }
+
+    except FileNotFoundError as e:
+        raise HTTPException(status_code=500, detail=f"配置缺失: {e}")
+    except Exception as e:
+        raise HTTPException(status_code=500, detail=f"规划失败: {e}")

src/drone_planning/core/__init__.py

@@ -0,0 +1 @@
+"""核心模块：黑板、状态等"""

src/drone_planning/core/blackboard.py

@@ -0,0 +1,97 @@
+"""
+Layer 0：感知层 - 全局状态黑板
+
+维护无人机当前状态，供 Layer 4 执行包装层判断是否需要自动插入起飞逻辑。
+后续可扩展：电量、模式、传感器状态等。
+"""
+
+from __future__ import annotations
+
+from dataclasses import dataclass
+from typing import Any
+
+
+@dataclass
+class Position:
+    """ENU 坐标系下的位置（东-北-天）"""
+
+    x: float
+    y: float
+    z: float
+
+    def to_dict(self) -> dict[str, float]:
+        """转换为字典，便于 JSON 序列化"""
+        return {"x": self.x, "y": self.y, "z": self.z}
+
+    @classmethod
+    def from_dict(cls, d: dict[str, Any]) -> Position:
+        """从字典创建"""
+        return cls(x=float(d["x"]), y=float(d["y"]), z=float(d["z"]))
+
+
+class DroneStateBlackboard:
+    """
+    无人机状态黑板（全局单例）
+
+    供 Layer 4 的 wrap_and_build_tree 判断：
+    - 若 is_in_air == False，自动在业务树前插入 [SystemCheck -> Takeoff] 子树
+    - 若 is_in_air == True，直接执行业务树
+    """
+
+    _instance: DroneStateBlackboard | None = None
+
+    def __new__(cls) -> DroneStateBlackboard:
+        if cls._instance is None:
+            cls._instance = super().__new__(cls)
+        return cls._instance
+
+    def __init__(self) -> None:
+        # 避免重复初始化覆盖已有状态
+        if hasattr(self, "_initialized") and self._initialized:
+            return
+
+        self._initialized = True
+        self._is_in_air: bool = False
+        self._position: Position = Position(0.0, 0.0, 0.0)
+        # 预留扩展字段
+        self._extra: dict[str, Any] = {}
+
+    @property
+    def is_in_air(self) -> bool:
+        """是否在空中（True=已起飞，False=在地面）"""
+        return self._is_in_air
+
+    @is_in_air.setter
+    def is_in_air(self, value: bool) -> None:
+        self._is_in_air = value
+
+    @property
+    def position(self) -> Position:
+        """当前 ENU 坐标"""
+        return self._position
+
+    @position.setter
+    def position(self, value: Position) -> None:
+        self._position = value
+
+    def set_position(self, x: float, y: float, z: float) -> None:
+        """便捷设置位置"""
+        self._position = Position(x=x, y=y, z=z)
+
+    def get_position_dict(self) -> dict[str, float]:
+        """获取位置字典 {x, y, z}"""
+        return self._position.to_dict()
+
+    def set_extra(self, key: str, value: Any) -> None:
+        """设置扩展字段（如 battery, mode）"""
+        self._extra[key] = value
+
+    def get_extra(self, key: str, default: Any = None) -> Any:
+        """获取扩展字段"""
+        return self._extra.get(key, default)
+
+    def reset(self) -> None:
+        """重置为地面初始状态（用于测试或仿真重置）"""
+        self._is_in_air = False
+        self._position = Position(0.0, 0.0, 0.0)
+        self._extra.clear()

src/drone_planning/execution/__init__.py

@@ -0,0 +1 @@
+"""执行层：py_trees 节点与树包装"""

src/drone_planning/execution/nodes.py

@@ -0,0 +1,71 @@
+"""
+Layer 4：执行层动作库 - Mock 节点实现
+
+继承 py_trees.behaviour.Behaviour，update() 中打印中文日志并返回 SUCCESS。
+后续预留 ROS2/PX4 接口。
+"""
+
+from __future__ import annotations
+
+from py_trees import common
+from py_trees.behaviour import Behaviour
+
+
+class SystemCheckCondition(Behaviour):
+    """系统检查条件：模拟起飞前自检"""
+
+    def __init__(self, name: str = "SystemCheck"):
+        super().__init__(name)
+
+    def update(self) -> common.Status:
+        self.logger.info("[SystemCheck] 执行系统检查：电池、传感器、通信... OK")
+        return common.Status.SUCCESS
+
+
+class TakeoffAction(Behaviour):
+    """起飞动作"""
+
+    def __init__(self, name: str = "Takeoff"):
+        super().__init__(name)
+
+    def update(self) -> common.Status:
+        self.logger.info("[Takeoff] 执行起飞... OK")
+        return common.Status.SUCCESS
+
+
+class LandAction(Behaviour):
+    """降落动作"""
+
+    def __init__(self, name: str = "Land"):
+        super().__init__(name)
+
+    def update(self) -> common.Status:
+        self.logger.info("[Land] 执行降落... OK")
+        return common.Status.SUCCESS
+
+
+class FlyToWaypointAction(Behaviour):
+    """飞往航点动作"""
+
+    def __init__(self, name: str, x: float, y: float, z: float):
+        super().__init__(name)
+        self.x = x
+        self.y = y
+        self.z = z
+
+    def update(self) -> common.Status:
+        self.logger.info(f"[FlyToWaypoint] 飞往 ({self.x}, {self.y}, {self.z})... OK")
+        return common.Status.SUCCESS
+
+
+class GenericAction(Behaviour):
+    """通用动作兜底：用于未单独实现的 action 类型"""
+
+    def __init__(self, name: str, action_type: str, params: dict | None = None):
+        super().__init__(name)
+        self.action_type = action_type
+        self.params = params or {}
+
+    def update(self) -> common.Status:
+        self.logger.info(f"[GenericAction] {self.action_type} params={self.params}... OK")
+        return common.Status.SUCCESS

src/drone_planning/execution/tree_wrapper.py

@@ -0,0 +1,107 @@
+"""
+Layer 4：执行包装与安全逻辑
+
+- parse_json_to_tree: 将 Planner 的 JSON 递归解析为 py_trees 对象
+- wrap_and_build_tree: 根据 is_in_air 自动插入 [SystemCheck -> Takeoff] 子树
+- tree_to_ascii: 将树以 ASCII/Unicode 文本形式打印
+"""
+
+from __future__ import annotations
+
+from typing import Any
+
+import py_trees
+from py_trees import display
+
+from drone_planning.core.blackboard import DroneStateBlackboard
+from drone_planning.execution.nodes import (
+    FlyToWaypointAction,
+    GenericAction,
+    SystemCheckCondition,
+    TakeoffAction,
+)
+
+
+def parse_json_to_tree(node_dict: dict[str, Any]) -> py_trees.behaviour.Behaviour:
+    """
+    递归将 JSON 节点转换为 py_trees 对象
+
+    Args:
+        node_dict: 单节点 dict，含 type、name、params、children
+
+    Returns:
+        py_trees.Behaviour 实例
+    """
+    node_type = node_dict.get("type", "GenericAction")
+    name = node_dict.get("name") or node_type
+    params = node_dict.get("params") or {}
+    children_data = node_dict.get("children") or []
+
+    # 控制流节点
+    if node_type == "Sequence":
+        children = [parse_json_to_tree(c) for c in children_data]
+        return py_trees.composites.Sequence(name=name, memory=True, children=children)
+    if node_type == "Selector":
+        children = [parse_json_to_tree(c) for c in children_data]
+        return py_trees.composites.Selector(name=name, memory=True, children=children)
+    if node_type == "Parallel":
+        policy = params.get("policy", "success_on_all")
+        children = [parse_json_to_tree(c) for c in children_data]
+        return py_trees.composites.Parallel(
+            name=name,
+            policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ALL
+            if policy == "success_on_all"
+            else py_trees.common.ParallelPolicy.SUCCESS_ON_ONE,
+            children=children,
+        )
+
+    # 动作节点
+    if node_type == "fly_to_waypoint":
+        x = float(params.get("x", 0))
+        y = float(params.get("y", 0))
+        z = float(params.get("z", 0))
+        return FlyToWaypointAction(name=name, x=x, y=y, z=z)
+
+    # 其他 action 用 GenericAction 兜底
+    return GenericAction(name=name, action_type=node_type, params=params)
+
+
+def wrap_and_build_tree(business_tree: py_trees.behaviour.Behaviour) -> py_trees.behaviour.Behaviour:
+    """
+    安全包装：若未在空中，自动在业务树前插入 [SystemCheck -> Takeoff]
+
+    Args:
+        business_tree: Planner 生成的业务树（已解析为 py_trees）
+
+    Returns:
+        最终可执行的完整树
+    """
+    bb = DroneStateBlackboard()
+    if bb.is_in_air:
+        return business_tree
+
+    # 在地面：必须先生成系统检查 -> 起飞 -> 业务树
+    safe_sequence = py_trees.composites.Sequence(
+        name="Safe_Execution",
+        memory=True,
+        children=[
+            SystemCheckCondition(),
+            TakeoffAction(),
+            business_tree,
+        ],
+    )
+    return safe_sequence
+
+
+def tree_to_ascii(root: py_trees.behaviour.Behaviour, show_status: bool = True) -> str:
+    """
+    将行为树以 ASCII/Unicode 文本形式打印
+
+    Args:
+        root: 树根节点
+        show_status: 是否显示状态
+
+    Returns:
+        可打印的字符串
+    """
+    return display.unicode_tree(root=root, show_status=show_status)

src/drone_planning/llm_client/__init__.py

@@ -0,0 +1 @@
+"""LLM 客户端：对接 llama-server（OpenAI 兼容 API）"""

src/drone_planning/llm_client/client.py

@@ -0,0 +1,152 @@
+"""
+LLM 客户端 - 对接 llama-server（OpenAI 兼容 API）
+
+- Chat 推理：默认 http://localhost:8081/v1
+- 支持 response_format 的 json_schema 结构化输出
+- 环境变量：LLM_BASE_URL（默认 8081）、OPENAI_API_KEY（可为空，本地部署通常不需要）
+"""
+
+from __future__ import annotations
+
+import json
+import os
+from typing import Any
+
+from openai import OpenAI
+
+
+# 环境变量配置，带默认值
+LLM_CHAT_BASE_URL = os.getenv("LLM_CHAT_BASE_URL", "http://localhost:8081/v1")
+LLM_EMBEDDING_BASE_URL = os.getenv("LLM_EMBEDDING_BASE_URL", "http://localhost:8090/v1")
+OPENAI_API_KEY = os.getenv("OPENAI_API_KEY", "not-needed")  # 本地 llama-server 通常不需要
+# 思考模式：默认关闭，测试时减少延迟
+ENABLE_THINKING = os.getenv("ENABLE_THINKING", "false").lower() in ("true", "1", "yes")
+EXTRA_BODY = {"chat_template_kwargs": {"enable_thinking": ENABLE_THINKING}}
+
+
+def get_chat_client() -> OpenAI:
+    """获取 Chat 推理客户端（8081 端口）"""
+    return OpenAI(
+        base_url=LLM_CHAT_BASE_URL,
+        api_key=OPENAI_API_KEY,
+    )
+
+
+def chat_completion(
+    messages: list[dict[str, Any]],
+    model: str = "qwen3",
+    temperature: float = 0.3,
+    response_format: dict[str, Any] | None = None,
+) -> str:
+    """
+    调用 Chat Completion API
+
+    Args:
+        messages: 消息列表 [{"role": "system/user/assistant", "content": "..."}]
+        model: 模型名，llama-server 通常忽略，可传任意值
+        temperature: 温度，低值更确定性
+        response_format: 可选，如 {"type": "json_schema", "json_schema": {...}}
+                        用于强制输出符合 JSON Schema 的结构
+
+    Returns:
+        assistant 的 content 文本
+    """
+    client = get_chat_client()
+    kwargs: dict[str, Any] = {
+        "model": model,
+        "messages": messages,
+        "temperature": temperature,
+        "extra_body": EXTRA_BODY,
+    }
+    if response_format is not None:
+        kwargs["response_format"] = response_format
+
+    response = client.chat.completions.create(**kwargs)
+    content = response.choices[0].message.content
+    return content or ""
+
+
+def chat_completion_with_tools(
+    messages: list[dict[str, Any]],
+    tools: list[dict[str, Any]],
+    model: str = "qwen3",
+    temperature: float = 0.2,
+) -> Any:
+    """
+    调用 Chat Completion，支持 tools（Function Calling）
+
+    返回完整 response 对象，便于检查 tool_calls。
+    注意：使用 tools 时通常不能同时使用 response_format。
+    """
+    client = get_chat_client()
+    return client.chat.completions.create(
+        model=model,
+        messages=messages,
+        tools=tools,
+        temperature=temperature,
+        extra_body=EXTRA_BODY,
+    )
+
+
+def chat_completion_json(
+    messages: list[dict[str, str]],
+    json_schema: dict[str, Any],
+    schema_name: str = "response",
+    model: str = "qwen3",
+    temperature: float = 0.3,
+) -> dict[str, Any]:
+    """
+    调用 Chat Completion，并强制返回符合 JSON Schema 的结构化输出
+
+    使用 OpenAI SDK 的 response_format={"type": "json_schema"} 确保输出格式正确。
+    若 llama-server 不支持 json_schema，会回退到 json_object 模式并手动解析。
+
+    Args:
+        messages: 消息列表
+        json_schema: JSON Schema 定义（符合 OpenAI Structured Outputs 规范）
+        schema_name: schema 名称
+        model: 模型名
+        temperature: 温度
+
+    Returns:
+        解析后的 dict
+    """
+    response_format = {
+        "type": "json_schema",
+        "json_schema": {
+            "name": schema_name,
+            "strict": True,
+            "schema": json_schema,
+        },
+    }
+
+    try:
+        content = chat_completion(
+            messages=messages,
+            model=model,
+            temperature=temperature,
+            response_format=response_format,
+        )
+    except Exception as e:
+        # 部分本地服务器可能不支持 json_schema，回退到 json_object
+        if "json_schema" in str(e).lower() or "response_format" in str(e).lower():
+            content = chat_completion(
+                messages=messages,
+                model=model,
+                temperature=temperature,
+                response_format={"type": "json_object"},
+            )
+        else:
+            raise e
+
+    # 去除可能的 markdown 代码块包裹
+    text = content.strip()
+    if text.startswith("```"):
+        lines = text.split("\n")
+        if lines[0].startswith("```json"):
+            lines = lines[1:]
+        if lines and lines[-1].strip() == "```":
+            lines = lines[:-1]
+        text = "\n".join(lines)
+
+    return json.loads(text)

src/drone_planning/pipeline/__init__.py

@@ -0,0 +1 @@
+"""流水线模块：意图路由、动态组装、宏观规划"""

src/drone_planning/pipeline/composer.py

@@ -0,0 +1,171 @@
+"""
+Layer 2：动态组装与外部计算（Dynamic Composer）
+
+根据 intents 和 entities 裁剪 node_schema，解析地点坐标，生成发给 Stage 2 LLM 的精简 System Prompt。
+阶段一：rag_context 默认 None；阶段二接入 RAG 后传入上下文。
+"""
+
+from __future__ import annotations
+
+import json
+from pathlib import Path
+from typing import Any
+
+from drone_planning.pipeline.router import BUSINESS_INTENTS
+
+# 意图 -> 相关 actions 映射（用于裁剪 schema）
+INTENT_TO_ACTIONS: dict[str, list[str]] = {
+    "fly_task": ["fly_to_waypoint", "fly_sequence", "move_direction"],
+    "search_task": ["search_pattern", "rotate_search"],
+    "track_task": ["track_object"],
+    "photo_task": ["take_photos"],
+    "interact_task": ["report_message", "manual_confirmation"],
+}
+
+# 默认包含的 control_flow 和 decorators（LLM 规划必需）
+DEFAULT_CONTROL_FLOW = ["Sequence", "Selector", "Parallel"]
+DEFAULT_DECORATORS = ["Timeout", "Repeat"]
+DEFAULT_CONDITIONS = ["object_detected"]
+
+
+def _load_node_schema() -> dict[str, Any]:
+    """加载 config/node_schema.json"""
+    base = Path(__file__).resolve().parent.parent.parent.parent
+    schema_path = base / "config" / "node_schema.json"
+    if not schema_path.exists():
+        raise FileNotFoundError(f"node_schema.json 未找到: {schema_path}")
+    with open(schema_path, "r", encoding="utf-8") as f:
+        return json.load(f)
+
+
+def extract_actions_for_intents(intents: list[str], schema_json: dict[str, Any]) -> dict[str, Any]:
+    """
+    根据 intents 裁剪出最小必要的 actions，放入最终 schema
+
+    Args:
+        intents: 业务意图列表
+        schema_json: 原始 node_schema.json 内容
+
+    Returns:
+        裁剪后的 schema 字典，包含 _instruction、actions、conditions、control_flow、decorators
+    """
+    actions_pool = schema_json.get("actions", {})
+    conditions_pool = schema_json.get("conditions", {})
+    control_flow_pool = schema_json.get("control_flow", {})
+    decorators_pool = schema_json.get("decorators", {})
+
+    # 收集需要的 action 名称
+    needed_actions: set[str] = set()
+    for intent in intents:
+        if intent in BUSINESS_INTENTS and intent in INTENT_TO_ACTIONS:
+            needed_actions.update(INTENT_TO_ACTIONS[intent])
+    if not needed_actions:
+        # 兜底：至少包含 fly_task 相关
+        needed_actions = set(INTENT_TO_ACTIONS.get("fly_task", ["fly_to_waypoint"]))
+
+    # 裁剪 actions
+    trimmed_actions = {k: v for k, v in actions_pool.items() if k in needed_actions}
+    if not trimmed_actions:
+        trimmed_actions = {"fly_to_waypoint": actions_pool.get("fly_to_waypoint", {})}
+
+    # 保留完整的 control_flow、decorators、conditions
+    trimmed_control = {k: control_flow_pool[k] for k in DEFAULT_CONTROL_FLOW if k in control_flow_pool}
+    trimmed_decorators = {k: decorators_pool[k] for k in DEFAULT_DECORATORS if k in decorators_pool}
+    trimmed_conditions = {k: conditions_pool[k] for k in DEFAULT_CONDITIONS if k in conditions_pool}
+
+    return {
+        "_instruction": schema_json.get("_instruction", "作为行为树规划器，你只能使用以下定义的节点和参数。"),
+        "actions": trimmed_actions,
+        "conditions": trimmed_conditions,
+        "control_flow": trimmed_control,
+        "decorators": trimmed_decorators,
+    }
+
+
+def build_system_prompt(
+    intents: list[str],
+    entities: dict[str, Any],
+    schema_json: dict[str, Any],
+    rag_context: dict[str, Any] | None = None,
+) -> tuple[str, dict[str, Any]]:
+    """
+    构建发给 Stage 2 LLM 的精简 System Prompt
+
+    Args:
+        intents: 业务意图列表
+        entities: 实体字典（含 locations、targets 等）
+        schema_json: 原始 node_schema
+        rag_context: RAG 检索上下文，阶段一默认 None
+
+    Returns:
+        (prompt_text, metadata) 其中 metadata 含 selected_actions、location_coords 等
+    """
+    metadata: dict[str, Any] = {"selected_actions": [], "base_location_coords": {}}
+
+    # 1. 裁剪 schema
+    trimmed_schema = extract_actions_for_intents(intents, schema_json)
+    metadata["selected_actions"] = list(trimmed_schema.get("actions", {}).keys())
+
+    # 2. 基准点坐标（RAG 仅提供，不计算相对位置）
+    base_coords = (rag_context or {}).get("base_location_coords") or {}
+    base_location_coords: dict[str, dict[str, float]] = {k: dict(v) for k, v in base_coords.items()}
+    metadata["base_location_coords"] = base_location_coords
+    relative_descriptions = (rag_context or {}).get("relative_descriptions") or []
+
+    # 3. 拼接 prompt 文本
+    parts: list[str] = []
+
+    # 指令
+    parts.append(trimmed_schema["_instruction"])
+    parts.append("")
+
+    # 基准点坐标（若有）
+    if base_location_coords:
+        parts.append("## 基准点坐标 base_location_coords（ENU，单位米）")
+        for loc, coord in base_location_coords.items():
+            parts.append(f"- {loc}: x={coord['x']}, y={coord['y']}, z={coord['z']}")
+        parts.append("")
+
+    # 节点定义
+    parts.append("## 可用节点")
+    parts.append(json.dumps(trimmed_schema, ensure_ascii=False, indent=2))
+    parts.append("")
+
+    # RAG 区块（阶段二）
+    if rag_context:
+        if rag_context.get("map_context"):
+            parts.append("## RAG 地图上下文")
+            parts.append(rag_context["map_context"])
+            parts.append("")
+        if rag_context.get("rule_context"):
+            parts.append("## RAG 规则约束")
+            parts.append(rag_context["rule_context"])
+            parts.append("")
+        if rag_context.get("few_shot_examples"):
+            parts.append("## RAG 示例行为树")
+            for ex in rag_context["few_shot_examples"][:2]:
+                parts.append(f"指令: {ex.get('instruction', '')}")
+                parts.append(f"树: {json.dumps(ex.get('tree_json', {}), ensure_ascii=False)}")
+            parts.append("")
+
+    # 坐标计算规则（仅 LLM 通过 mcp 工具计算，RAG 不负责）
+    parts.append("## 坐标计算规则")
+    parts.append(
+        "若用户只说绝对地点（如'去广场'），可直接使用 base_location_coords 中的坐标生成行为树。"
+        "若用户说相对位置（如'广场东边500米'、'大门北偏东30度100米'），"
+        "必须调用 calculate_relative_coordinate 工具，禁止心算。"
+    )
+    if relative_descriptions:
+        parts.append("")
+        parts.append("以下为相对描述，需调用工具计算：")
+        for rd in relative_descriptions:
+            base = rd.get("base", "")
+            coord = base_location_coords.get(base, {})
+            x, y = coord.get("x", 0), coord.get("y", 0)
+            parts.append(f"- {rd.get('target', '')}：基准 {base}(x={x}, y={y})，方向 {rd.get('direction', '')}，距离 {rd.get('distance', 0)} 米")
+    parts.append("")
+    parts.append("请根据用户指令，输出合法的行为树 JSON，根节点为 root，格式如 {\"type\": \"Sequence\", \"children\": [...]}")
+
+    prompt_text = "\n".join(parts)
+    metadata["prompt_length"] = len(prompt_text)
+    return prompt_text, metadata

src/drone_planning/pipeline/planner.py

@@ -0,0 +1,204 @@
+"""
+Layer 3：宏观行为规划（Stage 2 Macro Planner）
+
+调用 LLM 根据 Composer 的 System Prompt 和用户文本，生成业务行为树 JSON。
+支持 Function Calling：大模型可自主调用 calculate_relative_coordinate 进行相对坐标计算。
+"""
+
+from __future__ import annotations
+
+import json
+from typing import Any, Callable
+
+from drone_planning.llm_client.client import chat_completion, chat_completion_json, chat_completion_with_tools
+from drone_planning.tools.mcp_calc import calculate_relative_coordinate
+
+# OpenAI tools 定义：坐标计算工具
+CALC_COORDINATE_TOOL = {
+    "type": "function",
+    "function": {
+        "name": "calculate_relative_coordinate",
+        "description": "根据基准点坐标和方向、距离，计算目标点的绝对 ENU 坐标。用于处理「广场东边500米」「大门北偏东30度100米」等相对描述。",
+        "parameters": {
+            "type": "object",
+            "properties": {
+                "base_x": {"type": "number", "description": "基准点东向坐标（米）"},
+                "base_y": {"type": "number", "description": "基准点北向坐标（米）"},
+                "direction_str": {
+                    "type": "string",
+                    "description": "方向，如 east/东、northeast/东北、北偏东30度",
+                },
+                "distance": {"type": "number", "description": "距离（米）"},
+            },
+            "required": ["base_x", "base_y", "direction_str", "distance"],
+        },
+    },
+}
+
+PLANNER_TOOLS = [CALC_COORDINATE_TOOL]
+
+# 回退模式用的 JSON Schema
+PLANNER_JSON_SCHEMA = {
+    "type": "object",
+    "properties": {
+        "root": {
+            "type": "object",
+            "properties": {
+                "type": {"type": "string"},
+                "name": {"type": ["string", "null"]},
+                "params": {"type": "object"},
+                "children": {"type": "array", "items": {"type": "object"}},
+            },
+            "required": ["type"],
+            "additionalProperties": True,
+        },
+    },
+    "required": ["root"],
+    "additionalProperties": False,
+}
+
+# 工具名 -> 执行函数
+TOOL_HANDLERS: dict[str, Callable[..., Any]] = {
+    "calculate_relative_coordinate": lambda **kw: calculate_relative_coordinate(
+        base_x=kw["base_x"],
+        base_y=kw["base_y"],
+        direction_str=kw["direction_str"],
+        distance=kw["distance"],
+    ),
+}
+
+
+def _execute_tool(name: str, arguments: dict[str, Any]) -> Any:
+    """执行工具调用，返回结果"""
+    handler = TOOL_HANDLERS.get(name)
+    if not handler:
+        return {"error": f"未知工具: {name}"}
+    try:
+        return handler(**arguments)
+    except Exception as e:
+        return {"error": str(e)}
+
+
+def plan(
+    system_prompt: str,
+    user_text: str,
+    tool_call_log: list[dict[str, Any]] | None = None,
+) -> dict[str, Any]:
+    """
+    调用 LLM 生成行为树 JSON，支持 Tool 循环
+
+    Args:
+        system_prompt: Composer 生成的 System Prompt
+        user_text: 用户自然语言指令
+        tool_call_log: 可选，用于收集 Tool Call 交互日志（供 Playground 展示）
+
+    Returns:
+        行为树 dict，格式 {"root": {"type": "...", "children": [...], ...}}
+    """
+    log = tool_call_log if tool_call_log is not None else []
+    messages: list[dict[str, Any]] = [
+        {"role": "system", "content": system_prompt},
+        {"role": "user", "content": user_text.strip() or "请生成行为树"},
+    ]
+
+    max_tool_rounds = 5
+    for _ in range(max_tool_rounds):
+        try:
+            response = chat_completion_with_tools(
+                messages=messages,
+                tools=PLANNER_TOOLS,
+                temperature=0.2,
+            )
+        except Exception as e:
+            # 若模型不支持 tools，回退到无工具模式
+            return _plan_fallback(system_prompt, user_text)
+
+        choice = response.choices[0]
+        msg = choice.message
+
+        if msg.tool_calls:
+            # 有工具调用：执行并追加 assistant + 各 tool 结果
+            messages.append({
+                "role": "assistant",
+                "content": msg.content or "",
+                "tool_calls": [
+                    {
+                        "id": tc.id,
+                        "type": "function",
+                        "function": {"name": tc.function.name, "arguments": tc.function.arguments},
+                    }
+                    for tc in msg.tool_calls
+                ],
+            })
+            for tc in msg.tool_calls:
+                name = tc.function.name
+                try:
+                    args = json.loads(tc.function.arguments or "{}")
+                except json.JSONDecodeError:
+                    args = {}
+                result = _execute_tool(name, args)
+                log.append({
+                    "round": len(log) + 1,
+                    "tool": name,
+                    "arguments": args,
+                    "result": result,
+                })
+                messages.append({
+                    "role": "tool",
+                    "tool_call_id": tc.id,
+                    "content": json.dumps(result, ensure_ascii=False),
+                })
+            continue
+
+        # 无工具调用：解析最终行为树
+        content = msg.content or ""
+        if not content.strip():
+            return _plan_fallback(system_prompt, user_text)
+
+        text = content.strip()
+        if text.startswith("```"):
+            lines = text.split("\n")
+            if lines and "json" in lines[0].lower():
+                lines = lines[1:]
+            if lines and lines[-1].strip() == "```":
+                lines = lines[:-1]
+            text = "\n".join(lines)
+
+        try:
+            return json.loads(text)
+        except json.JSONDecodeError:
+            return _plan_fallback(system_prompt, user_text)
+
+    # 超过最大轮次，回退
+    return _plan_fallback(system_prompt, user_text)
+
+
+def _plan_fallback(system_prompt: str, user_text: str) -> dict[str, Any]:
+    """回退：无工具模式"""
+    messages = [
+        {"role": "system", "content": system_prompt},
+        {"role": "user", "content": user_text.strip() or "请生成行为树"},
+    ]
+    try:
+        raw = chat_completion_json(
+            messages=messages,
+            json_schema=PLANNER_JSON_SCHEMA,
+            schema_name="behavior_tree",
+            temperature=0.2,
+        )
+        return raw
+    except Exception:
+        content = chat_completion(
+            messages=messages,
+            temperature=0.2,
+            response_format={"type": "json_object"},
+        )
+        text = content.strip()
+        if text.startswith("```"):
+            lines = text.split("\n")
+            if lines and "json" in lines[0].lower():
+                lines = lines[1:]
+            if lines and lines[-1].strip() == "```":
+                lines = lines[:-1]
+            text = "\n".join(lines)
+        return json.loads(text)

src/drone_planning/pipeline/router.py

@@ -0,0 +1,205 @@
+"""
+Layer 1：意图路由层（Stage 1 Intent Router）
+
+调用 LLM 做极简意图分类和实体抽取，实现 Fast-Path 短路与冲突处理。
+"""
+
+from __future__ import annotations
+
+from dataclasses import dataclass, field
+from typing import Any
+
+from drone_planning.llm_client.client import chat_completion_json
+
+# ---------------------------------------------------------------------------
+# 意图集合定义（与需求文档保持一致）
+# ---------------------------------------------------------------------------
+ATOMIC_INTENTS = {"atomic_takeoff", "atomic_land", "atomic_hover"}
+BUSINESS_INTENTS = {"fly_task", "search_task", "track_task", "photo_task", "interact_task"}
+ALL_VALID_INTENTS = ATOMIC_INTENTS | BUSINESS_INTENTS
+
+# 兜底意图：当 intents 为空或包含未识别标签时使用
+FALLBACK_INTENTS = ["fly_task", "search_task"]
+
+# ---------------------------------------------------------------------------
+# Router 输出结构
+# ---------------------------------------------------------------------------
+ROUTER_JSON_SCHEMA = {
+    "type": "object",
+    "properties": {
+        "intents": {
+            "type": "array",
+            "items": {"type": "string"},
+            "description": "意图列表，仅使用 atomic_takeoff/atomic_land/atomic_hover 或 fly_task/search_task/track_task/photo_task/interact_task",
+        },
+        "entities": {
+            "type": "object",
+            "properties": {
+                "locations": {
+                    "type": "array",
+                    "items": {"type": "string"},
+                    "description": "地点实体列表，如 ['大门','广场']",
+                },
+                "targets": {
+                    "type": "array",
+                    "items": {"type": "string"},
+                    "description": "目标实体列表，如 ['汽车','行人']",
+                },
+                "direction": {"type": "string", "description": "方向：东/east、西/west、南/south、北/north、东北/northeast 等，或 front|back|left|right|up|down"},
+                "distance": {"type": "number", "description": "距离（米），如「东边500米」中的 500"},
+            },
+            "additionalProperties": True,
+            "description": "抽取的实体",
+        },
+    },
+    "required": ["intents", "entities"],
+    "additionalProperties": False,
+}
+
+ROUTER_SYSTEM_PROMPT = """你是指令意图分类器。根据用户自然语言，输出意图和实体。
+
+## 意图集合（只能使用以下标签，不要自创）
+
+**原子意图（仅用于简单控制指令）：**
+- atomic_takeoff：起飞
+- atomic_land：降落
+- atomic_hover：悬停
+
+**业务意图：**
+- fly_task：空间移动、路径、巡逻、飞到某地
+- search_task：搜索、侦查
+- track_task：跟踪
+- photo_task：拍照
+- interact_task：上报、请求确认
+
+## 实体要求
+- locations：基地点列表，如 ["大门","广场"]。对于「广场东边500米」，只填 ["广场"]，不要填 "广场东边500米"
+- targets：目标列表，如 ["汽车","行人","公交车"]
+- direction：东/east、西/west、南/south、北/north、东北/northeast 等，或 front|back|left|right|up|down
+- distance：数字（米），如「东边500米」中的 500
+
+## 规则
+1. 若用户只说"起飞"、"降落"、"悬停"，只输出对应 atomic 意图，entities 可为空对象。
+2. 若用户说复杂任务（如"飞到大门然后拍照"），输出业务意图，并抽取 locations、targets 等。
+3. 若同时包含原子和业务（如"起飞后去广场"），两者都输出，由系统后续处理。
+4. 对于「广场东边500米」「大门北偏东30度100米」等相对描述，必须拆分：locations=["广场"], direction="东", distance=500
+5. 不要输出未在意图集合中的标签。"""
+
+
+@dataclass
+class RouterResult:
+    """意图路由结果"""
+
+    intents: list[str] = field(default_factory=list)
+    entities: dict[str, Any] = field(default_factory=dict)
+    is_fast_path: bool = False
+    raw_llm_output: dict[str, Any] | None = None
+
+    def get_locations(self) -> list[str]:
+        """获取地点列表，保证返回 list"""
+        locs = self.entities.get("locations")
+        if isinstance(locs, list):
+            return [str(x) for x in locs]
+        if locs is not None:
+            return [str(locs)]
+        return []
+
+    def get_targets(self) -> list[str]:
+        """获取目标列表，保证返回 list"""
+        tgs = self.entities.get("targets")
+        if isinstance(tgs, list):
+            return [str(x) for x in tgs]
+        if tgs is not None:
+            return [str(tgs)]
+        return []
+
+
+def _resolve_conflicts(intents: list[str]) -> list[str]:
+    """
+    硬编码冲突处理逻辑
+
+    - 若 intents 同时包含 atomic 和 business：删除所有 atomic，仅保留 business
+    - 若 intents 全为 atomic：保持不变（由上层判断 Fast-Path）
+    - 若 intents 为空或包含未识别标签：兜底为 FALLBACK_INTENTS
+    """
+    if not intents:
+        return list(FALLBACK_INTENTS)
+
+    # 过滤掉未识别的标签
+    valid = [i for i in intents if i in ALL_VALID_INTENTS]
+    if not valid:
+        return list(FALLBACK_INTENTS)
+
+    has_atomic = any(i in ATOMIC_INTENTS for i in valid)
+    has_business = any(i in BUSINESS_INTENTS for i in valid)
+
+    # 若同时包含 atomic 和 business：删除 atomic，仅保留 business
+    if has_atomic and has_business:
+        return [i for i in valid if i in BUSINESS_INTENTS]
+
+    return valid
+
+
+def route(user_text: str) -> RouterResult:
+    """
+    意图路由主入口
+
+    Args:
+        user_text: 用户自然语言指令
+
+    Returns:
+        RouterResult：包含 intents、entities、is_fast_path
+    """
+    messages = [
+        {"role": "system", "content": ROUTER_SYSTEM_PROMPT},
+        {"role": "user", "content": user_text.strip() or "请分析意图"},
+    ]
+
+    try:
+        raw = chat_completion_json(
+            messages=messages,
+            json_schema=ROUTER_JSON_SCHEMA,
+            schema_name="router_response",
+            temperature=0.1,
+        )
+    except Exception as e:
+        # LLM 调用失败时兜底
+        return RouterResult(
+            intents=list(FALLBACK_INTENTS),
+            entities={},
+            is_fast_path=False,
+            raw_llm_output={"error": str(e)},
+        )
+
+    raw_intents = raw.get("intents") or []
+    raw_entities = raw.get("entities") or {}
+
+    if not isinstance(raw_intents, list):
+        raw_intents = [str(raw_intents)] if raw_intents else []
+
+    # 冲突处理
+    resolved_intents = _resolve_conflicts(raw_intents)
+
+    # 判断 Fast-Path：intents 非空且全部属于 ATOMIC_INTENTS
+    is_fast_path = (
+        len(resolved_intents) > 0
+        and all(i in ATOMIC_INTENTS for i in resolved_intents)
+    )
+
+    return RouterResult(
+        intents=resolved_intents,
+        entities=raw_entities if isinstance(raw_entities, dict) else {},
+        is_fast_path=is_fast_path,
+        raw_llm_output=raw,
+    )
+
+
+if __name__ == "__main__":
+    # 本地测试：python -m drone_planning.pipeline.router
+    import sys
+
+    text = sys.argv[1] if len(sys.argv) > 1 else "飞到大门然后拍照"
+    result = route(text)
+    print("intents:", result.intents)
+    print("entities:", result.entities)
+    print("is_fast_path:", result.is_fast_path)

src/drone_planning/rag/__init__.py

@@ -0,0 +1 @@
+"""RAG 模块：Embedding、向量存储、检索、灌入"""

src/drone_planning/rag/embedding_client.py

@@ -0,0 +1,63 @@
+"""
+RAG Embedding 客户端 - 实现 ChromaDB EmbeddingFunction 接口
+
+使用 OpenAI SDK 连接本地 llama-server 的 Embedding 服务（8090 端口），
+模型名可通过环境变量 EMBEDDING_MODEL 配置。
+"""
+
+from __future__ import annotations
+
+import os
+from typing import Any, List
+
+from chromadb.api.types import Documents, EmbeddingFunction, Embeddings
+from openai import OpenAI
+
+# 环境变量配置
+EMBEDDING_BASE_URL = os.getenv("LLM_EMBEDDING_BASE_URL", "http://localhost:8090/v1")
+EMBEDDING_MODEL = os.getenv("EMBEDDING_MODEL", "qwen3-embedding")
+OPENAI_API_KEY = os.getenv("OPENAI_API_KEY", "not-needed")
+
+
+class QwenEmbeddingFunction(EmbeddingFunction[Documents]):
+    """
+    基于 Qwen Embedding（llama-server 8090）的 ChromaDB EmbeddingFunction
+
+    实现 ChromaDB 的 EmbeddingFunction 协议，供 vector_store 使用。
+    """
+
+    def __init__(
+        self,
+        base_url: str | None = None,
+        model: str | None = None,
+        api_key: str | None = None,
+    ) -> None:
+        """
+        Args:
+            base_url: Embedding API 地址，默认从 LLM_EMBEDDING_BASE_URL 读取
+            model: 模型名，默认从 EMBEDDING_MODEL 读取
+            api_key: API Key，本地部署通常用 "not-needed"
+        """
+        self._base_url = base_url or EMBEDDING_BASE_URL
+        self._model = model or EMBEDDING_MODEL
+        self._api_key = api_key or OPENAI_API_KEY
+        self._client = OpenAI(base_url=self._base_url, api_key=self._api_key)
+
+    def __call__(self, input: Documents) -> Embeddings:
+        """
+        将文本列表转换为向量列表（ChromaDB EmbeddingFunction 接口）
+
+        Args:
+            input: 文本列表，每项为 str
+
+        Returns:
+            向量列表，每项为 list[float]
+        """
+        if not input:
+            return []
+
+        texts = [t if isinstance(t, str) else str(t) for t in input]
+        response = self._client.embeddings.create(model=self._model, input=texts)
+        # 按 order 排序（API 可能乱序返回）
+        sorted_data = sorted(response.data, key=lambda x: x.index)
+        return [item.embedding for item in sorted_data]

src/drone_planning/rag/ingestion.py

@@ -0,0 +1,136 @@
+"""
+RAG 数据灌入脚本
+
+从 data/knowledge/*.jsonl 读取 NDJSON，提取 document 作为文本，
+其他结构化字段作为 metadata，灌入对应的 ChromaDB Collection。
+可独立执行：python -m drone_planning.rag.ingestion
+"""
+
+from __future__ import annotations
+
+import json
+import uuid
+from pathlib import Path
+
+from drone_planning.rag.vector_store import RAGVectorStore
+
+# 知识库路径（相对于项目根）
+KNOWLEDGE_DIR = Path(__file__).resolve().parent.parent.parent.parent / "data" / "knowledge"
+MAP_DB_FILE = KNOWLEDGE_DIR / "map_db.jsonl"
+RULE_DB_FILE = KNOWLEDGE_DIR / "rule_db.jsonl"
+FEW_SHOT_DB_FILE = KNOWLEDGE_DIR / "few_shot_db.jsonl"
+
+
+def _load_jsonl(path: Path) -> list[dict]:
+    """加载 jsonl 文件，每行一个 JSON"""
+    if not path.exists():
+        return []
+    records = []
+    with open(path, "r", encoding="utf-8") as f:
+        for i, line in enumerate(f):
+            line = line.strip()
+            if not line:
+                continue
+            try:
+                records.append(json.loads(line))
+            except json.JSONDecodeError as e:
+                raise ValueError(f"{path}:{i + 1} JSON 解析失败: {e}")
+    return records
+
+
+def _prepare_map_records(records: list[dict]) -> tuple[list[str], list[str], list[dict]]:
+    """
+    准备 map_db 灌入数据
+
+    document 作为文本，location/x/y/z 等作为 metadata。
+    """
+    ids, documents, metadatas = [], [], []
+    for i, r in enumerate(records):
+        doc = r.get("document", "")
+        if not doc:
+            continue
+        meta = {k: v for k, v in r.items() if k != "document"}
+        # ChromaDB metadata 值需为 str, int, float, bool
+        meta = {k: (v if isinstance(v, (str, int, float, bool)) else str(v)) for k, v in meta.items()}
+        ids.append(r.get("id", f"map_{i}_{uuid.uuid4().hex[:8]}"))
+        documents.append(doc)
+        metadatas.append(meta)
+    return ids, documents, metadatas
+
+
+def _prepare_rule_records(records: list[dict]) -> tuple[list[str], list[str], list[dict]]:
+    """准备 rule_db 灌入数据"""
+    ids, documents, metadatas = [], [], []
+    for i, r in enumerate(records):
+        doc = r.get("document", "")
+        if not doc:
+            continue
+        meta = {k: v for k, v in r.items() if k != "document"}
+        meta = {k: (v if isinstance(v, (str, int, float, bool)) else str(v)) for k, v in meta.items()}
+        ids.append(r.get("id", f"rule_{i}_{uuid.uuid4().hex[:8]}"))
+        documents.append(doc)
+        metadatas.append(meta)
+    return ids, documents, metadatas
+
+
+def _prepare_few_shot_records(records: list[dict]) -> tuple[list[str], list[str], list[dict]]:
+    """准备 few_shot_db 灌入数据"""
+    ids, documents, metadatas = [], [], []
+    for i, r in enumerate(records):
+        doc = r.get("document", "")
+        if not doc:
+            continue
+        meta = {k: v for k, v in r.items() if k != "document"}
+        meta = {k: (v if isinstance(v, (str, int, float, bool)) else str(v)) for k, v in meta.items()}
+        ids.append(r.get("id", f"fewshot_{i}_{uuid.uuid4().hex[:8]}"))
+        documents.append(doc)
+        metadatas.append(meta)
+    return ids, documents, metadatas
+
+
+def run_ingestion(clear_first: bool = True) -> dict[str, int]:
+    """
+    执行数据灌入
+
+    Args:
+        clear_first: 是否先清空已有数据再灌入
+
+    Returns:
+        各 Collection 灌入数量 {"map_db": n, "rule_db": n, "few_shot_db": n}
+    """
+    store = RAGVectorStore()
+    if clear_first:
+        store.clear_all()
+
+    counts = {"map_db": 0, "rule_db": 0, "few_shot_db": 0}
+
+    # map_db
+    if MAP_DB_FILE.exists():
+        records = _load_jsonl(MAP_DB_FILE)
+        if records:
+            ids, docs, metas = _prepare_map_records(records)
+            store.add_map_records(ids, docs, metas)
+            counts["map_db"] = len(ids)
+
+    # rule_db
+    if RULE_DB_FILE.exists():
+        records = _load_jsonl(RULE_DB_FILE)
+        if records:
+            ids, docs, metas = _prepare_rule_records(records)
+            store.add_rule_records(ids, docs, metas)
+            counts["rule_db"] = len(ids)
+
+    # few_shot_db
+    if FEW_SHOT_DB_FILE.exists():
+        records = _load_jsonl(FEW_SHOT_DB_FILE)
+        if records:
+            ids, docs, metas = _prepare_few_shot_records(records)
+            store.add_few_shot_records(ids, docs, metas)
+            counts["few_shot_db"] = len(ids)
+
+    return counts
+
+
+if __name__ == "__main__":
+    counts = run_ingestion()
+    print("灌入完成:", counts)

src/drone_planning/rag/retriever.py

@@ -0,0 +1,203 @@
+"""
+RAG 检索核心 - 冷热双态检索
+
+- 热表（dynamic_memory）：占位，未来接入语义地图
+- 冷库（ChromaDB）：map_db、rule_db、few_shot_db
+- 仅提供基准点坐标（base_location_coords），不负责相对坐标计算；相对位置由 LLM 调用 mcp 工具计算
+"""
+
+from __future__ import annotations
+
+import re
+from typing import Any
+
+from drone_planning.rag.vector_store import RAGVectorStore
+
+
+def _parse_relative_location(loc: str) -> tuple[str | None, str | None, float | None]:
+    """
+    解析「广场东边500米」「大门北偏东30度100米」类复合地点
+
+    Returns:
+        (base_location, direction_str, distance) 或 (None, None, None)
+    """
+    s = str(loc).strip()
+    # 广场东边500米、广场东侧500米、广场东500米
+    m = re.search(r"^(.+?)([东西南北])(?:边|侧)?\s*(\d+(?:\.\d+)?)\s*米", s)
+    if m:
+        return m.group(1).strip(), m.group(2), float(m.group(3))
+    # 大门北偏东30度100米
+    m = re.search(r"^(.+?)(北偏东|东偏北|南偏东|东偏南|北偏西|西偏北|南偏西|西偏南)\s*(\d+(?:\.\d+)?)\s*度\s*(\d+(?:\.\d+)?)\s*米", s)
+    if m:
+        return m.group(1).strip(), f"{m.group(2)}{m.group(3)}度", float(m.group(4))
+    return None, None, None
+
+
+class RAGRetriever:
+    """
+    RAG 检索器
+
+    实现冷热双态检索逻辑：
+    - 地图：先查热表 dynamic_memory，没有再查 map_db（metadata 精确匹配）
+    - 规则：根据 intents 查 rule_db（向量 + metadata）
+    - 示例：用 user_text 查 few_shot_db（向量相似度，取 top 1~2）
+    """
+
+    def __init__(self, vector_store: RAGVectorStore | None = None) -> None:
+        """
+        Args:
+            vector_store: 向量存储实例，默认新建
+        """
+        self._store = vector_store or RAGVectorStore()
+        # 热数据占位：未来接入动态语义地图
+        # 格式 {"locations": {"大门": {"x": 0, "y": 0, "z": 0}, ...}, "targets": {...}}
+        self.dynamic_memory: dict[str, Any] = {"locations": {}, "targets": {}}
+
+    def retrieve_context(
+        self,
+        intents: list[str],
+        entities: dict[str, Any],
+        user_text: str,
+    ) -> dict[str, Any]:
+        """
+        检索 RAG 上下文
+
+        Args:
+            intents: 意图列表
+            entities: 实体字典（含 locations、targets 等）
+            user_text: 用户原始文本
+
+        Returns:
+            {
+                "map_context": str,
+                "rule_context": str,
+                "few_shot_examples": [...],
+                "base_location_coords": {loc: {x,y,z}},  # 基准点坐标，不计算相对位置
+                "relative_descriptions": [{target, base, direction, distance}],  # 相对描述，供 LLM 调用工具
+            }
+        """
+        result: dict[str, Any] = {
+            "map_context": "",
+            "rule_context": "",
+            "few_shot_examples": [],
+            "base_location_coords": {},  # 基准点坐标（仅从 map_db 查，不计算相对坐标）
+            "relative_descriptions": [],  # 相对描述列表，供 LLM 调用工具
+        }
+
+        locations = entities.get("locations") or []
+        if not isinstance(locations, list):
+            locations = [locations] if locations else []
+        direction = entities.get("direction")
+        distance = entities.get("distance")
+        if distance is not None:
+            try:
+                distance = float(distance)
+            except (TypeError, ValueError):
+                distance = None
+
+        # 1. 地图：热表优先，冷库兜底；仅提供基准点坐标，不调用 mcp 计算
+        map_lines: list[str] = []
+        for loc in locations:
+            loc = str(loc).strip()
+            if not loc:
+                continue
+
+            # 解析「广场东边500米」或使用 entities 的 direction/distance
+            base_loc, dir_str, dist = _parse_relative_location(loc)
+            if base_loc and (dir_str or direction) and (dist is not None or distance is not None):
+                use_relative = True
+                dir_str = dir_str or direction or "东"
+                dist = float(dist) if dist is not None else float(distance)
+            elif direction and distance is not None:
+                use_relative = True
+                base_loc = loc
+                dir_str = str(direction)
+                dist = float(distance)
+                loc = f"{loc}{dir_str}边{int(dist)}米"  # 合成目标点名称
+            else:
+                use_relative = False
+                base_loc = loc
+
+            # 先查热表
+            hot_coords = self.dynamic_memory.get("locations", {}).get(base_loc)
+            if hot_coords is not None:
+                x, y, z = hot_coords.get("x", 0), hot_coords.get("y", 0), hot_coords.get("z", 0)
+                result["base_location_coords"][base_loc] = {"x": x, "y": y, "z": z}
+                if use_relative:
+                    map_lines.append(f"{base_loc}（基准点）: x={x}, y={y}, z={z}；相对描述需 LLM 调用工具计算")
+                    result["relative_descriptions"].append({
+                        "target": loc,
+                        "base": base_loc,
+                        "direction": dir_str,
+                        "distance": dist,
+                    })
+                else:
+                    map_lines.append(f"{loc}: x={x}, y={y}, z={z} (热表)")
+                continue
+
+            # 冷库：metadata 精确查找
+            try:
+                got = self._store.map_db.get(
+                    where={"location": base_loc},
+                    include=["documents", "metadatas"],
+                )
+                if got and got["metadatas"]:
+                    m = got["metadatas"][0]
+                    x, y, z = m.get("x", 0), m.get("y", 0), m.get("z", 0)
+                    result["base_location_coords"][base_loc] = {"x": x, "y": y, "z": z}
+                    if use_relative:
+                        map_lines.append(f"{base_loc}（基准点）: x={x}, y={y}, z={z}；相对描述需 LLM 调用工具计算")
+                        result["relative_descriptions"].append({
+                            "target": loc,
+                            "base": base_loc,
+                            "direction": dir_str,
+                            "distance": dist,
+                        })
+                    else:
+                        map_lines.append(f"{loc}: x={x}, y={y}, z={z} (ChromaDB)")
+            except Exception:
+                pass
+        if map_lines:
+            result["map_context"] = "\n".join(map_lines)
+
+        # 2. 规则：根据 intents 查 rule_db
+        rule_lines: list[str] = []
+        for intent in intents:
+            try:
+                got = self._store.rule_db.get(
+                    where={"intent": intent},
+                    include=["documents"],
+                )
+                if got and got["documents"]:
+                    rule_lines.extend(got["documents"])
+            except Exception:
+                pass
+        if rule_lines:
+            result["rule_context"] = "\n".join(rule_lines)
+
+        # 3. 示例：user_text 向量检索 few_shot_db，取 top 2
+        if user_text.strip():
+            try:
+                import json as _json
+                got = self._store.few_shot_db.query(
+                    query_texts=[user_text.strip()],
+                    n_results=2,
+                    include=["documents", "metadatas"],
+                )
+                docs_list = (got.get("documents") or [[]])[0]
+                metas_list = (got.get("metadatas") or [[]])[0]
+                for i, meta in enumerate(metas_list or []):
+                    doc = docs_list[i] if i < len(docs_list) else ""
+                    tree_str = meta.get("tree_json", "{}")
+                    try:
+                        tree_obj = _json.loads(tree_str) if isinstance(tree_str, str) else tree_str
+                    except Exception:
+                        tree_obj = {}
+                    result["few_shot_examples"].append({
+                        "instruction": doc or "",
+                        "tree_json": tree_obj,
+                    })
+            except Exception:
+                pass
+
+        return result

src/drone_planning/rag/vector_store.py

@@ -0,0 +1,149 @@
+"""
+RAG 向量存储 - ChromaDB 封装
+
+管理 map_db、rule_db、few_shot_db 三个 Collection，
+提供将 jsonl 数据写入对应表的方法。
+存储路径：./data/chroma（项目根目录下）
+"""
+
+from __future__ import annotations
+
+import os
+from pathlib import Path
+from typing import Any
+
+import chromadb
+from chromadb.config import Settings
+
+from drone_planning.rag.embedding_client import QwenEmbeddingFunction
+
+# 默认存储路径（相对于项目根目录）
+DEFAULT_PERSIST_PATH = os.getenv("CHROMA_PERSIST_PATH", "./data/chroma")
+COLLECTION_MAP_DB = "map_db"
+COLLECTION_RULE_DB = "rule_db"
+COLLECTION_FEW_SHOT_DB = "few_shot_db"
+
+
+def _resolve_persist_path() -> str:
+    """解析 ChromaDB 持久化路径为绝对路径"""
+    base = Path(__file__).resolve().parent.parent.parent.parent
+    path = Path(DEFAULT_PERSIST_PATH)
+    if not path.is_absolute():
+        path = base / path
+    path.mkdir(parents=True, exist_ok=True)
+    return str(path)
+
+
+class RAGVectorStore:
+    """
+    RAG 向量存储客户端
+
+    初始化时创建/获取三个 Collection：map_db、rule_db、few_shot_db。
+    使用 Qwen Embedding 作为向量化函数。
+    """
+
+    def __init__(
+        self,
+        persist_path: str | None = None,
+        embedding_function: Any | None = None,
+    ) -> None:
+        """
+        Args:
+            persist_path: ChromaDB 持久化目录，默认 ./data/chroma
+            embedding_function: 自定义 EmbeddingFunction，默认使用 QwenEmbeddingFunction
+        """
+        self._persist_path = persist_path or _resolve_persist_path()
+        self._ef = embedding_function or QwenEmbeddingFunction()
+
+        self._client = chromadb.PersistentClient(
+            path=self._persist_path,
+            settings=Settings(anonymized_telemetry=False),
+        )
+
+        # 获取或创建三个 Collection
+        self._map_db = self._client.get_or_create_collection(
+            name=COLLECTION_MAP_DB,
+            embedding_function=self._ef,
+            metadata={"description": "地图地点坐标库"},
+        )
+        self._rule_db = self._client.get_or_create_collection(
+            name=COLLECTION_RULE_DB,
+            embedding_function=self._ef,
+            metadata={"description": "规则约束库"},
+        )
+        self._few_shot_db = self._client.get_or_create_collection(
+            name=COLLECTION_FEW_SHOT_DB,
+            embedding_function=self._ef,
+            metadata={"description": "Few-shot 示例库"},
+        )
+
+    @property
+    def map_db(self):
+        """地图 Collection"""
+        return self._map_db
+
+    @property
+    def rule_db(self):
+        """规则 Collection"""
+        return self._rule_db
+
+    @property
+    def few_shot_db(self):
+        """Few-shot 示例 Collection"""
+        return self._few_shot_db
+
+    def add_map_records(
+        self,
+        ids: list[str],
+        documents: list[str],
+        metadatas: list[dict[str, Any]],
+    ) -> None:
+        """
+        向 map_db 添加记录
+
+        Args:
+            ids: 文档 ID 列表
+            documents: 文档文本列表（用于向量化）
+            metadatas: 元数据列表，应含 location、x、y、z 等
+        """
+        self._map_db.add(ids=ids, documents=documents, metadatas=metadatas)
+
+    def add_rule_records(
+        self,
+        ids: list[str],
+        documents: list[str],
+        metadatas: list[dict[str, Any]],
+    ) -> None:
+        """向 rule_db 添加记录"""
+        self._rule_db.add(ids=ids, documents=documents, metadatas=metadatas)
+
+    def add_few_shot_records(
+        self,
+        ids: list[str],
+        documents: list[str],
+        metadatas: list[dict[str, Any]],
+    ) -> None:
+        """向 few_shot_db 添加记录"""
+        self._few_shot_db.add(ids=ids, documents=documents, metadatas=metadatas)
+
+    def clear_all(self) -> None:
+        """清空三个 Collection（用于重新灌入）"""
+        self._client.delete_collection(COLLECTION_MAP_DB)
+        self._client.delete_collection(COLLECTION_RULE_DB)
+        self._client.delete_collection(COLLECTION_FEW_SHOT_DB)
+        # 重新创建
+        self._map_db = self._client.get_or_create_collection(
+            name=COLLECTION_MAP_DB,
+            embedding_function=self._ef,
+            metadata={"description": "地图地点坐标库"},
+        )
+        self._rule_db = self._client.get_or_create_collection(
+            name=COLLECTION_RULE_DB,
+            embedding_function=self._ef,
+            metadata={"description": "规则约束库"},
+        )
+        self._few_shot_db = self._client.get_or_create_collection(
+            name=COLLECTION_FEW_SHOT_DB,
+            embedding_function=self._ef,
+            metadata={"description": "Few-shot 示例库"},
+        )

src/drone_planning/tools/__init__.py

@@ -0,0 +1 @@
+"""工具模块：坐标解析、地理信息等"""

src/drone_planning/tools/geo.py

@@ -0,0 +1,38 @@
+"""
+坐标解析工具 - 已废弃硬编码兜底
+
+架构升级：所有基准地点坐标一律只从 RAG (map_db) 中获取。
+本模块保留空壳，供可能的外部调用兼容；新逻辑不再依赖此处。
+"""
+
+from __future__ import annotations
+
+from typing import Any
+
+
+def landmark_to_enu(landmark: str) -> dict[str, float] | None:
+    """
+    已废弃：不再提供硬编码坐标。
+
+    坐标解析统一由 RAG map_db 提供，Composer 中已移除对本函数的兜底调用。
+    """
+    return None
+
+
+def landmarks_to_enu(landmarks: list[str]) -> list[dict[str, float]]:
+    """
+    已废弃：不再提供硬编码坐标。
+
+    返回空列表或 None 占位，调用方应改用 RAG 检索的 location_coords。
+    """
+    return []
+
+
+def add_landmark(name: str, x: float, y: float, z: float = 0.0) -> None:
+    """已废弃：无硬编码表可写。"""
+    pass
+
+
+def get_all_landmarks() -> dict[str, dict[str, float]]:
+    """已废弃：返回空字典。"""
+    return {}

src/drone_planning/tools/mcp_calc.py

@@ -0,0 +1,142 @@
+"""
+MCP 坐标计算工具 - 相对坐标计算
+
+根据基准点 (base_x, base_y) 和方向、距离，计算目标点的绝对 ENU 坐标。
+供大模型通过 Function Calling 自主调用，实现「广场东边500米」等相对描述。
+"""
+
+from __future__ import annotations
+
+import math
+import re
+from typing import Any
+
+# 方向别名 -> 角度（度，0=东，90=北，ENU 坐标系）
+DIRECTION_ANGLES: dict[str, float] = {
+    "east": 0,
+    "东": 0,
+    "东边": 0,
+    "东侧": 0,
+    "west": 180,
+    "西": 180,
+    "西边": 180,
+    "西侧": 180,
+    "north": 90,
+    "北": 90,
+    "北边": 90,
+    "北侧": 90,
+    "south": 270,
+    "南": 270,
+    "南边": 270,
+    "南侧": 270,
+    "northeast": 45,
+    "东北": 45,
+    "东北方": 45,
+    "northwest": 135,
+    "西北": 135,
+    "西北方": 135,
+    "southeast": 315,
+    "东南": 315,
+    "东南方": 315,
+    "southwest": 225,
+    "西南": 225,
+    "西南方": 225,
+}
+
+
+def _parse_direction_angle(direction_str: str) -> float | None:
+    """
+    解析方向字符串为角度（度）
+
+    支持：
+    - 英文/中文方向词：east, 东, northeast, 东北
+    - 角度描述：北偏东30度、东偏北45度、30度
+    """
+    s = direction_str.strip()
+    if not s:
+        return None
+    s_lower = s.lower()
+
+    # 先匹配角度描述（避免「北偏东30度」被误匹配为「东」）
+    m = re.search(r"北偏东\s*(\d+(?:\.\d+)?)\s*度?", s, re.I)
+    if m:
+        return 90 - float(m.group(1))  # 北为90°，偏东减角度
+
+    m = re.search(r"东偏北\s*(\d+(?:\.\d+)?)\s*度?", s, re.I)
+    if m:
+        return float(m.group(1))  # 东为0°，偏北加角度
+
+    m = re.search(r"南偏东\s*(\d+(?:\.\d+)?)\s*度?", s, re.I)
+    if m:
+        return 270 + float(m.group(1))  # 南270°，偏东加
+
+    m = re.search(r"东偏南\s*(\d+(?:\.\d+)?)\s*度?", s, re.I)
+    if m:
+        return 360 - float(m.group(1))
+
+    m = re.search(r"北偏西\s*(\d+(?:\.\d+)?)\s*度?", s, re.I)
+    if m:
+        return 90 + float(m.group(1))
+
+    m = re.search(r"南偏西\s*(\d+(?:\.\d+)?)\s*度?", s, re.I)
+    if m:
+        return 270 - float(m.group(1))
+
+    # 纯数字角度
+    m = re.search(r"(\d+(?:\.\d+)?)\s*度?", s_lower)
+    if m:
+        return float(m.group(1)) % 360
+
+    # 最后查简单方向别名（精确匹配优先）
+    for key, angle in DIRECTION_ANGLES.items():
+        if s_lower == key or (len(s_lower) <= 4 and key in s_lower and key not in ("东", "西", "南", "北")):
+            return angle
+    for short in ("东", "西", "南", "北"):
+        if s_lower == short or s == short:
+            return DIRECTION_ANGLES.get(short, 0)
+
+    return None
+
+
+def calculate_relative_coordinate(
+    base_x: float,
+    base_y: float,
+    direction_str: str,
+    distance: float,
+    base_z: float = 0.0,
+) -> dict[str, Any]:
+    """
+    计算相对坐标
+
+    ENU 坐标系：东=x+，北=y+，上=z+。
+    角度：0°=东，90°=北，180°=西，270°=南。
+
+    Args:
+        base_x: 基准点东向坐标（米）
+        base_y: 基准点北向坐标（米）
+        direction_str: 方向描述，如 "east", "东", "northeast", "北偏东30度"
+        distance: 距离（米）
+        base_z: 基准点高度，默认 0（输出时保持）
+
+    Returns:
+        {"x": float, "y": float, "z": float, "angle_deg": float}
+    """
+    angle = _parse_direction_angle(direction_str)
+    if angle is None:
+        return {
+            "x": base_x,
+            "y": base_y,
+            "z": base_z,
+            "angle_deg": None,
+            "error": f"无法解析方向: {direction_str}",
+        }
+
+    rad = math.radians(angle)
+    dx = distance * math.cos(rad)
+    dy = distance * math.sin(rad)
+    return {
+        "x": round(base_x + dx, 2),
+        "y": round(base_y + dy, 2),
+        "z": base_z,
+        "angle_deg": angle,
+    }