huangfu/DronePlanningV2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

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

Browse Source 
Made-with: Cursor
This commit is contained in:
huangfu
2026-03-17 10:51:32 +08:00
commit 3ddc2e1ff4
35 changed files with 3219 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

265
# 无人机行为规划后端系统（重建版）需求说明.md Normal file
View File

@@ -0,0 +1,265 @@
# 无人机行为规划后端系统（重建版）需求说明
> 目标：基于端侧 Qwen3-4B（llama-server）和行为树，重建一个从自然语言到无人机行为树 JSON 的后端系统，支持未来扩展 RAG 和动态语义地图。
## 一、总体架构理念（务必遵守）
1. **大小脑解耦**
- 大模型（4B，小模型） = 「大脑」：只做语义理解、意图拆解、行为树结构生成，不直接管起飞/降落/避障。
- Python + py_trees + PX4/仿真 = 「小脑」：负责状态机管理、物理安全、控制执行（起飞、悬停、返航等）。
2. **四层流水线结构**
- 🔴 Layer 0：感知层（Perception & State）
纯代码。维护一个全局状态黑板 `DroneStateBlackboard`，至少包含：
- `is_in_air: bool`
- `position: {x, y, z}`（ENU）
- 后续可扩展电量、模式等。
- 🟡 Layer 1：意图路由层（Stage 1 Intent Router）
调用 LLM 做**极简意图分类和实体抽取**，只输出：
- `intents: list[str]`
- `entities: dict`
并实现 **Fast-Path 短路机制**：
- 若指令是纯原子控制（如“起飞”、“降落”、“悬停”），直接由代码执行，不进入规划阶段。
- 🟢 Layer 2：动态组装与外部计算（Dynamic Composer & MCP/RAG）
纯代码。
- 根据 `intents` 和 `entities`，从本地 `node_schema.json` 裁剪出**最小必要的节点定义**（行为树节点）。
- 使用工具（后续接 RAG）解析地点到坐标（ENU），大模型不做算术。
- 生成发给 Stage 2 LLM 的 **精简 System Prompt**。
- 🔵 Layer 3：宏观行为规划（Stage 2 Macro Planner）
调用 LLM（Qwen3-4B）：
- 输入：Layer 2 生成的 System Prompt + 用户原文（含 RAG 上下文）。
- 输出：业务行为树 JSON（**不包含起飞/降落**）。
- 🟣 Layer 4：执行包装与安全逻辑（Execution Wrapper）
纯代码 + py_trees：
- 将业务树解析为 py_trees 对象。
- 如果 `is_in_air == False`，自动在业务树前面**插入 `[SystemCheck -> Takeoff]` 子树**。
- 提供 tick 循环和未来的中断（用户打断、动态重规划）预留。
---
## 二、节点定义（给 LLM 的行为树节点池）
请在 `config/node_schema.json` 定义一个简洁的行为节点池（LLM 可见的动作），结构类似：
```json
{
"_instruction": "作为行为树规划器，你只能使用以下定义的节点和参数。",
"actions": {
"fly_to_waypoint": { "desc": "...", "params": { "x": "float", "y": "float", "z": "float" } },
"fly_sequence": { "desc": "...", "params": { "waypoints": "array of {x,y,z}" } },
"move_direction": { "desc": "...", "params": { "direction": "front|back|left|right|up|down", "distance": "float" } },
"search_pattern": { "desc": "...", "params": { "pattern_type": "spiral|grid", "radius": "float", "target_class": "string" } },
"rotate_search": { "desc": "...", "params": { "target_class": "string" } },
"track_object": { "desc": "...", "params": { "target_class": "string", "track_time": "float" } },
"take_photos": { "desc": "...", "params": { "target_class": "string", "count": "int" } },
"report_message": { "desc": "...", "params": { "message": "string" } },
"manual_confirmation": { "desc": "...", "params": { "prompt_message": "string" } }
},
"conditions": {
"object_detected": { "desc": "...", "params": { "target_class": "string" } }
},
"control_flow": {
"Sequence": { "desc": "...", "params": {}, "requires_children": true },
"Selector": { "desc": "...", "params": {}, "requires_children": true },
"Parallel": { "desc": "...", "params": { "policy": "success_on_one|success_on_all" }, "requires_children": true }
},
"decorators": {
"Timeout": { "desc": "...", "params": { "max_time": "float" }, "requires_child": true },
"Repeat": { "desc": "...", "params": { "times": "int" }, "requires_child": true }
}
}
```
> 注意：**起飞/降落等底层动作不要出现在这里**，由 Layer 4 包装硬编码处理。
---
## 三、Stage 1 Intent Router 的设计（重点）
### 1. LLM 提示词（Router System Prompt）
要求使用简洁、正交的 Intent 集合（避免同义词过多）：
- 原子意图（Atomic Intents）：仅用于 Fast-Path
- `atomic_takeoff`
- `atomic_land`
- `atomic_hover`
- 业务意图（Business Intents）
- `fly_task`：所有涉及空间移动/路径/巡逻
- `search_task`：搜索/侦查
- `track_task`：跟踪
- `photo_task`：拍照
- `interact_task`：上报/请求确认
**实体提取要求：**
- 地点实体统一用 `"locations"`，且必须为列表：
- `{"locations": ["大门"]}`
- `{"locations": ["大门", "广场"]}`（支持“先去A再去B”）
- 目标实体用 `"targets"` 列表：
- `{"targets": ["汽车", "行人"]}`
- 其他参数如 `"direction"`, `"distance"` 直接提取。
### 2. Python 层的“硬编码冲突处理”逻辑
在 `pipeline/router.py` 中：
- 定义集合：
```python
ATOMIC_INTENTS = {"atomic_takeoff", "atomic_land", "atomic_hover"}
BUSINESS_INTENTS = {"fly_task", "search_task", "track_task", "photo_task", "interact_task"}
```
- 处理流程：
- 若 `intents` 非空且 **全部属于 ATOMIC_INTENTS**：
→ 允许 Fast-Path（直接执行原子命令）。
- 若 `intents` 同时包含 atomic 和 business（如 `["atomic_takeoff", "fly_task"]`）：
→ **删除所有 atomic**，仅保留 business，走正常规划（起飞由 Layer 4 自动包装）。
- 若 `intents` 为空或包含未识别标签（小模型幻觉）：
→ 兜底为 `["fly_task", "search_task"]`，保证系统不崩。
---
## 四、RAG 模块设计（Layer 2 可选扩展）
### 1. 目录结构
`src/drone_planning/rag/`：
- `embedding_client.py`：封装 Qwen Embedding（llama-server 8090）。
- `vector_store.py`：ChromaDB 客户端，管理三个 Collection：
- `map_db`
- `rule_db`
- `few_shot_db`
- `retriever.py`：`RAGRetriever`：
- `dynamic_memory`：热数据表占位（未来语义地图）。
- `retrieve_context(intents, entities, user_text) -> dict`
- `ingestion.py`：从 `data/knowledge/*.jsonl` 读 NDJSON 灌入 ChromaDB。
- `schemas.py`（可选）：约定 JSONL 字段结构。
### 2. JSONL 格式约定（统一用 `.jsonl`，一行一 JSON）
- `data/knowledge/map_db.jsonl`：
```json
{"document": "喷泉在广场正中央，坐标 x=10, y=20", "location": "喷泉", "x": 10, "y": 20, "z": 0}
{"document": "A区是禁飞区，不得进入", "location": "A区", "zone_type": "no_fly"}
```
- `data/knowledge/rule_db.jsonl`：
```json
{"document": "夜间巡逻必须开启热成像", "intent": "search_task", "scene": "night"}
{"document": "起飞必须先做系统检查", "intent": "fly_task", "priority": "high"}
```
- `data/knowledge/few_shot_db.jsonl`：
```json
{"document": "飞到大门然后拍照", "intent": "fly_task", "tree_json": "{\"root\": {...}}"}
```
### 3. 检索策略（在 `retriever.py`）
- 热数据优先（Dynamic Semantic Map）：
- `dynamic_memory = {"locations": {...}, "targets": {...}}`
- 若某 location 在热内存中有坐标，直接返回，**不要查 ChromaDB**。
- 冷库次之（ChromaDB）：
- 根据 `entities["locations"]` 和 `intents` 用 metadata + 向量检索，从 `map_db` 和 `rule_db` 获取上下文。
- 使用 `user_text` 在 `few_shot_db` 中查 1–2 个相似任务的行为树，作为 few-shot 示例。
RAG 返回统一结构：
```python
{
"map_context": "若干行文本或结构化信息",
"rule_context": "规则文本汇总",
"few_shot_examples": [
{"instruction": "...", "tree_json": {...}},
...
]
}
```
### 4. 与 Composer 集成（`pipeline/composer.py`）
- 为 `build_system_prompt(...)` 增加参数 `rag_context: dict | None`。
- 在 Prompt 中插入三个区块（若非空）：
- `## RAG 地图上下文`
- `## RAG 规则约束`
- `## RAG 示例行为树`
- 坐标解析优先级：
1. 动态内存（语义地图，未来接入）。
2. RAG 地图（map_db 的 x,y,z）。
3. geo.py 的硬编码 `landmark_to_enu()`（兜底，不删除，作为最后一层 fallback）。
---
## 五、执行包装层（Layer 4，`execution`）
### 1. 行为节点实现（`execution/nodes.py`）
实现继承自 `py_trees.behaviour.Behaviour` 的 Mock 节点：
- `SystemCheckCondition`
- `TakeoffAction`
- `LandAction`
- `FlyToWaypointAction`
- `GenericAction`（兜底）
目前只打印日志并返回 `SUCCESS`，后续预留 ROS2/PX4 接口。
### 2. 树包装与 Tick 循环（`execution/tree_wrapper.py`）
- `parse_json_to_tree(node_dict)`：递归将 Planner 的 JSON 转换为 py_trees 树。
- `wrap_and_build_tree(business_tree)`：
- 读取 `DroneStateBlackboard.is_in_air`：
- 若 `False`：创建 `Sequence("Safe_Execution")`，依次添加：
- `SystemCheckCondition()`
- `TakeoffAction()`
- `business_tree`
- 若 `True`：直接返回 `business_tree`。
---
## 六、API 接口与 Orchestrator
- 使用 FastAPI 暴露 `POST /api/plan`：
- 输入：`{"text": "用户自然语言指令"}`。
- 调用顺序：
1. Layer 1 Router：若 Fast-Path，直接返回执行结果。
2. 否则，调用 RAG → Composer → Planner → Tree Wrapper，返回行为树 JSON 和日志。
---
## 七、请 Cursor 的工作顺序建议
1. 生成项目目录和基础依赖（`requirements.txt`，`pyproject.toml`）。
2. 实现：
- `core/blackboard.py`
- `llm_client/client.py`（指向 `http://localhost:8080/v1` for chat，`http://localhost:8090/v1` for embeddings 可在 embedding_client 中配置）
3. 实现 `config/node_schema.json`。
4. 实现 `pipeline/router.py`（含 intents/entities 提取与 Fast-Path 短路 + 冲突处理）。
5. 实现 `tools/geo.py`（仅作为 fallback 静态坐标表）。
6. 实现 `rag/*`（embedding_client, vector_store, ingestion, retriever）。
7. 实现 `pipeline/composer.py` 和 `pipeline/planner.py`（加入 rag_context）。
8. 实现 `execution/nodes.py` 和 `execution/tree_wrapper.py`。
9. 实现 `api/routes.py` 和 `main.py`。
10. 最后补充简单的 `tests/` 或手动 curl 示例。
请从目录结构 + 模块职责开始规划，确认后再逐步生成代码。
```

43
.gitignore vendored Normal file
View File

@@ -0,0 +1,43 @@
# Byte-compiled / optimized / DLL files
__pycache__/
*.py[cod]
*$py.class
*.so
.Python
build/
develop-eggs/
dist/
downloads/
eggs/
.eggs/
lib/
lib64/
parts/
sdist/
var/
wheels/
*.egg-info/
*.egg
# Virtual environments
.venv/
venv/
ENV/
env/
# IDE
.idea/
.vscode/
.cursor/
# ChromaDB / data
data/chroma/
# Local config
.env
.env.local
*.local
# OS
.DS_Store
Thumbs.db

181
README.md Normal file
View File

@@ -0,0 +1,181 @@
# 无人机行为规划后端系统
从自然语言指令到行为树 JSON 的四层流水线系统，支持 RAG 检索、Function Calling 坐标计算、Fast-Path 短路等能力。
## 功能概览
- **Layer 1 (Router)**：意图分类与实体抽取，支持原子指令（起飞/降落/悬停）Fast-Path 短路
- **RAG 检索**：地图基准点坐标、规则约束、Few-shot 示例（不负责相对坐标计算）
- **Layer 2 (Composer)**：根据意图裁剪 schema，组装精简 System Prompt
- **Layer 3 (Planner)**：LLM 生成行为树 JSON，**仅 LLM 通过 mcp 工具计算坐标**（绝对地点用 base_location_coords，相对位置必须调用工具，禁止心算）
- **Layer 4 (Execution)**：解析 JSON 为 py_trees，根据 `is_in_air` 自动插入起飞逻辑
## 环境要求
- Python 3.10+
- 本地 LLM 服务（llama-server 或兼容 OpenAI API 的服务）：
- **Chat**：默认 `http://localhost:8081/v1`
- **Embedding**：默认 `http://localhost:8090/v1`（RAG 用）
## 快速部署
### 1. 克隆与依赖
```bash
cd DronePlanningV2
pip install -e .
# 或
pip install -r requirements.txt
```
### 2. 配置 LLM 服务
确保 Chat 和 Embedding 服务已启动。可通过环境变量覆盖默认地址：
```bash
export LLM_CHAT_BASE_URL="http://localhost:8081/v1"
export LLM_EMBEDDING_BASE_URL="http://localhost:8090/v1"
export OPENAI_API_KEY="not-needed" # 本地部署通常不需要
```
可选：开启模型思考模式（默认关闭以降低延迟）：
```bash
export ENABLE_THINKING=true
```
### 3. RAG 知识库灌入
首次使用或更新知识库后需执行灌入：
```bash
python -m drone_planning.rag.ingestion
```
知识库文件位于 `data/knowledge/`：
- `map_db.jsonl`：地点坐标（location, x, y, z）
- `rule_db.jsonl`：规则约束（intent, document）
- `few_shot_db.jsonl`：Few-shot 示例（instruction, tree_json）
### 4. 启动 API 服务
```bash
python -m uvicorn main:app --reload --host 0.0.0.0 --port 8000
```
### 5. 启动 Playground 测试台
```bash
streamlit run playground.py
```
## 使用方式
### API 调用
```bash
# 复杂任务（走完整流水线）
curl -X POST http://localhost:8000/api/plan \
-H "Content-Type: application/json" \
-d '{"text": "飞到大门然后拍照"}'
# 原子指令（Fast-Path 短路）
curl -X POST http://localhost:8000/api/plan \
-H "Content-Type: application/json" \
-d '{"text": "起飞"}'
# 相对描述（RAG 提供基准点，LLM 调用 mcp 计算）
curl -X POST http://localhost:8000/api/plan \
-H "Content-Type: application/json" \
-d '{"text": "飞到广场东边500米"}'
```
响应示例（含各环节耗时 `timing_ms`）：
```json
{
"success": true,
"fast_path": false,
"intents": ["fly_task", "photo_task"],
"entities": {"locations": ["大门"], "targets": []},
"rag_context": {...},
"tool_call_log": [...],
"tree_json": {"root": {...}},
"tree_ascii": "...",
"timing_ms": {
"Layer1_Router": 120,
"RAG_检索": 45,
"Layer2_Composer": 2,
"Layer3_Planner": 3500,
"Layer4_Execution": 1
}
}
```
### Playground 测试
1. 打开 `http://localhost:8501`
2. 左侧边栏：切换「是否在空中」模拟起飞状态
3. 主界面：输入自然语言指令，点击「执行规划」
4. 查看：各环节耗时、Layer 1~4 输出、RAG 检索结果、LLM Tool Call 日志（坐标仅由 LLM 通过 mcp 计算）
### 命令行测试 Router
```bash
python -m drone_planning.pipeline.router "飞到大门然后拍照"
```
## 项目结构
```
DronePlanningV2/
├── config/
│ └── node_schema.json # 行为树节点定义
├── data/
│ └── knowledge/ # RAG 知识库 jsonl
│ ├── map_db.jsonl
│ ├── rule_db.jsonl
│ └── few_shot_db.jsonl
├── src/drone_planning/
│ ├── api/ # FastAPI 路由
│ ├── core/ # Blackboard 黑板
│ ├── execution/ # py_trees 解析与包装
│ ├── llm_client/ # Chat / Embedding 客户端
│ ├── pipeline/ # Router / Composer / Planner
│ ├── rag/ # 向量存储、检索、灌入
│ └── tools/ # mcp_calc 坐标计算
├── main.py # API 入口
├── playground.py # Streamlit 测试台
├── requirements.txt
└── pyproject.toml
```
## 环境变量汇总
| 变量 | 默认值 | 说明 |
|------|--------|------|
| `LLM_CHAT_BASE_URL` | `http://localhost:8081/v1` | Chat API 地址 |
| `LLM_EMBEDDING_BASE_URL` | `http://localhost:8090/v1` | Embedding API 地址 |
| `OPENAI_API_KEY` | `not-needed` | API Key |
| `ENABLE_THINKING` | `false` | 是否开启模型思考模式 |
| `CHROMA_PERSIST_PATH` | `./data/chroma` | ChromaDB 持久化路径 |
| `EMBEDDING_MODEL` | `qwen3-embedding` | Embedding 模型名 |
## 常见问题
**Q: ChromaDB 报错？**
A: 确保已安装 `chromadb>=0.4.0`，首次运行需执行 `python -m drone_planning.rag.ingestion`。
**Q: LLM 调用超时？**
A: 检查 `LLM_CHAT_BASE_URL` 是否可达，模型是否支持 `json_schema` / `tools`。
**Q: 相对坐标未计算？**
A: 确保 `map_db.jsonl` 中有基准点（如「广场」），Router 正确抽取 `direction`、`distance`，且 LLM 支持 Function Calling 调用 `calculate_relative_coordinate`。
**Q: 如何扩展知识库？**
A: 编辑 `data/knowledge/*.jsonl` 后重新执行 `python -m drone_planning.rag.ingestion`。
## 详细流程文档
完整项目流程、各 Stage 对应代码、RAG 调用、工具调用、提示词组织等详见：[docs/项目流程详解.md](docs/项目流程详解.md)

109
config/node_schema.json Normal file
View File

@@ -0,0 +1,109 @@
{
"_instruction": "作为行为树规划器，你只能使用以下定义的节点和参数。不要使用起飞、降落等底层动作，它们由系统自动包装。",
"actions": {
"fly_to_waypoint": {
"desc": "飞往指定坐标点（ENU 坐标系）",
"params": {
"x": "float",
"y": "float",
"z": "float"
}
},
"fly_sequence": {
"desc": "按顺序飞往多个航点",
"params": {
"waypoints": "array of {x,y,z}"
}
},
"move_direction": {
"desc": "沿指定方向移动固定距离",
"params": {
"direction": "front|back|left|right|up|down",
"distance": "float"
}
},
"search_pattern": {
"desc": "按螺旋或网格模式搜索目标",
"params": {
"pattern_type": "spiral|grid",
"radius": "float",
"target_class": "string"
}
},
"rotate_search": {
"desc": "原地旋转搜索目标",
"params": {
"target_class": "string"
}
},
"track_object": {
"desc": "跟踪指定类型的目标",
"params": {
"target_class": "string",
"track_time": "float"
}
},
"take_photos": {
"desc": "对目标拍照",
"params": {
"target_class": "string",
"count": "int"
}
},
"report_message": {
"desc": "上报消息",
"params": {
"message": "string"
}
},
"manual_confirmation": {
"desc": "请求用户手动确认",
"params": {
"prompt_message": "string"
}
}
},
"conditions": {
"object_detected": {
"desc": "检测到指定类型的目标",
"params": {
"target_class": "string"
}
}
},
"control_flow": {
"Sequence": {
"desc": "顺序执行子节点，全部成功才成功",
"params": {},
"requires_children": true
},
"Selector": {
"desc": "选择执行子节点，有一个成功即成功",
"params": {},
"requires_children": true
},
"Parallel": {
"desc": "并行执行子节点",
"params": {
"policy": "success_on_one|success_on_all"
},
"requires_children": true
}
},
"decorators": {
"Timeout": {
"desc": "限制子节点最大执行时间",
"params": {
"max_time": "float"
},
"requires_child": true
},
"Repeat": {
"desc": "重复执行子节点指定次数",
"params": {
"times": "int"
},
"requires_child": true
}
}
}

3
data/knowledge/few_shot_db.jsonl Normal file
View File

@@ -0,0 +1,3 @@
{"document": "飞到大门然后拍照", "intent": "fly_task", "tree_json": "{\"root\": {\"type\": \"Sequence\", \"children\": [{\"type\": \"fly_to_waypoint\", \"params\": {\"x\": 0, \"y\": 0, \"z\": 5}}, {\"type\": \"take_photos\", \"params\": {\"target_class\": \"unknown\", \"count\": 1}}]}}"}
{"document": "先去广场再去喷泉", "intent": "fly_task", "tree_json": "{\"root\": {\"type\": \"Sequence\", \"children\": [{\"type\": \"fly_to_waypoint\", \"params\": {\"x\": 10, \"y\": 15, \"z\": 5}}, {\"type\": \"fly_to_waypoint\", \"params\": {\"x\": 10, \"y\": 20, \"z\": 5}}]}}"}
{"document": "在A区搜索汽车", "intent": "search_task", "tree_json": "{\"root\": {\"type\": \"Sequence\", \"children\": [{\"type\": \"fly_to_waypoint\", \"params\": {\"x\": 5, \"y\": 5, \"z\": 5}}, {\"type\": \"search_pattern\", \"params\": {\"pattern_type\": \"spiral\", \"radius\": 10, \"target_class\": \"汽车\"}}]}}"}

5
data/knowledge/map_db.jsonl Normal file
View File

@@ -0,0 +1,5 @@
{"document": "喷泉在广场正中央，坐标 x=10, y=20", "location": "喷泉", "x": 10, "y": 20, "z": 0}
{"document": "大门位于入口处，坐标 x=0, y=0", "location": "大门", "x": 0, "y": 0, "z": 0}
{"document": "广场在喷泉西侧，坐标 x=10, y=15", "location": "广场", "x": 10, "y": 15, "z": 0}
{"document": "A区是工作区，坐标 x=5, y=5", "location": "A区", "x": 5, "y": 5, "z": 0}
{"document": "B区在东北角，坐标 x=20, y=10", "location": "B区", "x": 20, "y": 10, "z": 0}

5
data/knowledge/rule_db.jsonl Normal file
View File

@@ -0,0 +1,5 @@
{"document": "夜间巡逻必须开启热成像", "intent": "search_task", "scene": "night"}
{"document": "起飞必须先做系统检查", "intent": "fly_task", "priority": "high"}
{"document": "搜索任务需保持安全高度 5 米以上", "intent": "search_task", "min_altitude": 5}
{"document": "拍照任务需先悬停稳定", "intent": "photo_task", "pre_stabilize": true}
{"document": "跟踪移动目标时保持 3 米距离", "intent": "track_task", "safe_distance": 3}

391
docs/项目流程详解.md Normal file
View File

@@ -0,0 +1,391 @@
# 无人机行为规划系统 - 项目流程详解
---
## 1. 整体架构
```mermaid
flowchart TB
subgraph Input [输入]
UserText[用户自然语言指令]
end
subgraph Layer1 [Layer 1 - Router]
Router[意图路由]
FastPath{Fast-Path?}
end
subgraph RAG [RAG 检索]
MapDB[map_db]
RuleDB[rule_db]
FewShotDB[few_shot_db]
Retriever[RAGRetriever]
end
subgraph Layer2 [Layer 2 - Composer]
Composer[动态组装 Prompt]
end
subgraph Layer3 [Layer 3 - Planner]
Planner[LLM 规划]
ToolCall[Function Calling]
MCPCalc[calculate_relative_coordinate]
end
subgraph Layer4 [Layer 4 - Execution]
Parse[parse_json_to_tree]
Wrap[wrap_and_build_tree]
Tree[py_trees 行为树]
end
UserText --> Router
Router --> FastPath
FastPath -->|是| Output1[直接返回原子意图]
FastPath -->|否| Retriever
Retriever --> MapDB
Retriever --> RuleDB
Retriever --> FewShotDB
Retriever --> Composer
Composer --> Planner
Planner --> ToolCall
ToolCall --> MCPCalc
MCPCalc --> Planner
Planner --> Parse
Parse --> Wrap
Wrap --> Tree
```
---
## 2. 数据流与各 Stage 总览
| Stage | 模块 | 对应代码 | 是否调用 RAG | 是否调用工具 | 是否调用 LLM |
|-------|------|----------|--------------|--------------|--------------|
| Layer 1 | Router | `pipeline/router.py` | 否 | 否 | 是 |
| RAG | Retriever | `rag/retriever.py` | 是（map/rule/few_shot） | 否 | 否 |
| Layer 2 | Composer | `pipeline/composer.py` | 否（消费 RAG 结果） | 否 | 否 |
| Layer 3 | Planner | `pipeline/planner.py` | 否（消费 Composer 输出） | 是（LLM 调用 mcp） | 是 |
| Layer 4 | Execution | `execution/tree_wrapper.py` | 否 | 否 | 否 |
---
## 3. Stage 1：Layer 1 - Router（意图路由）
### 3.1 职责
- 对用户自然语言做意图分类和实体抽取
- 判断是否为 Fast-Path（仅原子意图：起飞/降落/悬停）
- 冲突处理：同时含原子+业务意图时，保留业务意图
### 3.2 对应代码
- **入口函数**：`route(user_text: str) -> RouterResult`
- **文件**：[`src/drone_planning/pipeline/router.py`](../src/drone_planning/pipeline/router.py)
### 3.3 是否调用 RAG
**否**。Router 仅依赖 LLM，不访问 RAG。
### 3.4 是否调用工具
**否**。仅调用 LLM 的 `chat_completion_json`，无 Function Calling。
### 3.5 提示词组织
**System Prompt**（`ROUTER_SYSTEM_PROMPT`）：
- 角色：指令意图分类器
- 意图集合：原子意图（atomic_takeoff/atomic_land/atomic_hover）、业务意图（fly_task/search_task/track_task/photo_task/interact_task）
- 实体要求：locations、targets、direction、distance
- 规则：相对描述（如「广场东边500米」）必须拆分为 `locations=["广场"]`, `direction="东"`, `distance=500`
**User Message**：用户原始指令。
**输出 Schema**（`ROUTER_JSON_SCHEMA`）：
```json
{
"intents": ["string"],
"entities": {
"locations": ["string"],
"targets": ["string"],
"direction": "string",
"distance": number
}
}
```
### 3.6 输出与短路逻辑
- `RouterResult.intents`：解析后的意图列表
- `RouterResult.entities`：实体字典
- `RouterResult.is_fast_path`：若 `intents` 非空且全部属于 `ATOMIC_INTENTS`，则为 `True`，后续 RAG/Composer/Planner/Execution 不执行
---
## 4. Stage 2：RAG 检索
### 4.1 职责
- 根据 Router 的 `intents`、`entities`、`user_text` 检索上下文
- 地图坐标：热表优先，冷库（ChromaDB map_db）兜底
- 规则约束：按 intent 查 rule_db
- Few-shot 示例：用 user_text 向量检索 few_shot_db
- **仅提供基准点坐标**（`base_location_coords`），不负责相对坐标计算；相对描述由 LLM 通过 mcp 工具计算
### 4.2 对应代码
- **类**：`RAGRetriever`
- **方法**：`retrieve_context(intents, entities, user_text) -> dict`
- **文件**：[`src/drone_planning/rag/retriever.py`](../src/drone_planning/rag/retriever.py)
### 4.3 调用的 RAG 库
| 库名 | 用途 | 检索方式 |
|------|------|----------|
| **map_db** | 基准点坐标 | metadata 精确匹配 `where={"location": base_loc}` |
| **rule_db** | 规则约束 | metadata 精确匹配 `where={"intent": intent}` |
| **few_shot_db** | Few-shot 示例 | 向量相似度 `query(query_texts=[user_text], n_results=2)` |
### 4.4 是否调用工具
**否**。RAG 不调用 mcp_calc，仅提供 `base_location_coords` 和 `relative_descriptions`，坐标计算由 LLM 通过 Function Calling 完成。
### 4.5 输出结构
```python
{
"map_context": str,
"rule_context": str,
"few_shot_examples": [...],
"base_location_coords": {loc: {x,y,z}}, # 基准点坐标，不计算相对位置
"relative_descriptions": [ # 相对描述，供 LLM 调用工具
{"target": "广场东边500米", "base": "广场", "direction": "东", "distance": 500},
...
]
}
```
---
## 5. Stage 3：Layer 2 - Composer（动态组装）
### 5.1 职责
- 根据 `intents` 裁剪 `node_schema.json`，只保留相关 actions
- 将 RAG 的 `base_location_coords`、`relative_descriptions`、`map_context`、`rule_context`、`few_shot_examples` 拼入 System Prompt
- 生成发给 Planner LLM 的精简 Prompt
### 5.2 对应代码
- **函数**：`build_system_prompt(intents, entities, schema_json, rag_context) -> (str, dict)`
- **文件**：[`src/drone_planning/pipeline/composer.py`](../src/drone_planning/pipeline/composer.py)
### 5.3 是否调用 RAG
**否**。Composer 不直接调用 RAG，只消费 Retriever 返回的 `rag_context`。
### 5.4 是否调用工具
**否**。
### 5.5 提示词组织
Prompt 由以下部分顺序拼接：
1. **指令**：`_instruction`（来自 node_schema）
2. **基准点坐标**：若 `base_location_coords` 非空，输出 `## 基准点坐标 base_location_coords（ENU，单位米）`
3. **可用节点**：裁剪后的 schema JSON（actions、conditions、control_flow、decorators）
4. **RAG 地图上下文**：`rag_context["map_context"]`
5. **RAG 规则约束**：`rag_context["rule_context"]`
6. **RAG 示例行为树**：`rag_context["few_shot_examples"]` 前 2 条
7. **坐标计算规则**：若用户只说绝对地点，可直接使用 base_location_coords；若用户说相对位置，必须调用 calculate_relative_coordinate，禁止心算
### 5.6 意图到 Actions 映射
```python
INTENT_TO_ACTIONS = {
"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"],
}
```
---
## 6. Stage 4：Layer 3 - Planner（宏观规划）
### 6.1 职责
- 根据 Composer 的 System Prompt 和用户文本，生成行为树 JSON
- 支持 Function Calling：LLM 可自主调用 `calculate_relative_coordinate` 计算相对坐标
- 最多 5 轮 Tool 循环，无工具调用时解析最终 JSON 返回
### 6.2 对应代码
- **函数**：`plan(system_prompt, user_text, tool_call_log) -> dict`
- **文件**：[`src/drone_planning/pipeline/planner.py`](../src/drone_planning/pipeline/planner.py)
### 6.3 是否调用 RAG
**否**。Planner 只消费 Composer 输出的 `system_prompt`，其中已包含 RAG 上下文。
### 6.4 是否调用工具
**是**。坐标计算**仅由 LLM 负责**，通过 OpenAI Function Calling 调用 `calculate_relative_coordinate`（mcp 工具）。Planner 执行工具并追加结果到 messages，继续下一轮推理。
**工具定义**（`CALC_COORDINATE_TOOL`）：
```json
{
"type": "function",
"function": {
"name": "calculate_relative_coordinate",
"description": "根据基准点坐标和方向、距离，计算目标点的绝对 ENU 坐标...",
"parameters": {
"base_x": "number",
"base_y": "number",
"direction_str": "string",
"distance": "number"
}
}
}
```
**工具实现**：[`src/drone_planning/tools/mcp_calc.py`](../src/drone_planning/tools/mcp_calc.py) 中的 `calculate_relative_coordinate`
### 6.5 提示词组织
- **System**：Composer 生成的完整 Prompt（含 schema、坐标、RAG 上下文、工具说明）
- **User**：用户原始指令
- 若 LLM 返回 `tool_calls`：追加 assistant message + 各 tool 结果，继续调用 `chat_completion_with_tools`
- 若 LLM 返回纯文本：解析 JSON 作为行为树
### 6.6 回退逻辑
- 若模型不支持 tools：使用 `chat_completion_json` + `PLANNER_JSON_SCHEMA` 无工具模式
- 若 JSON 解析失败：使用 `chat_completion` + `response_format={"type": "json_object"}`
---
## 7. Stage 5：Layer 4 - Execution（执行包装）
### 7.1 职责
- 将 Planner 的 JSON 递归解析为 py_trees 对象
- 根据 Blackboard `is_in_air` 判断：若在地面，自动在业务树前插入 `[SystemCheck -> Takeoff]` 子树
- 输出 ASCII 树结构供展示
### 7.2 对应代码
- **函数**：`parse_json_to_tree`, `wrap_and_build_tree`, `tree_to_ascii`
- **文件**：[`src/drone_planning/execution/tree_wrapper.py`](../src/drone_planning/execution/tree_wrapper.py)
- **依赖**：[`core/blackboard.py`](../src/drone_planning/core/blackboard.py)、[`execution/nodes.py`](../src/drone_planning/execution/nodes.py)
### 7.3 是否调用 RAG
**否**。
### 7.4 是否调用工具
**否**。
### 7.5 节点映射
| JSON type | py_trees 实现 |
|-----------|---------------|
| Sequence | `py_trees.composites.Sequence` |
| Selector | `py_trees.composites.Selector` |
| Parallel | `py_trees.composites.Parallel` |
| fly_to_waypoint | `FlyToWaypointAction` |
| 其他 action | `GenericAction` |
---
## 8. 坐标计算流程（仅 LLM 负责）
```mermaid
flowchart LR
subgraph RAG [RAG 仅提供基准点]
Router1[Router 抽取 direction+distance]
Retriever1[Retriever 解析复合地点]
MapDB1[查 map_db 得基准坐标]
Composer1[Composer 将 base_location_coords 写入 Prompt]
end
subgraph LLM [LLM 调用 mcp 计算]
Planner1[Planner 收到 Prompt]
LLM1[LLM 调用 calculate_relative_coordinate]
Tool1[mcp 工具]
Result1[工具结果写回 messages]
end
Router1 --> Retriever1
Retriever1 --> MapDB1
MapDB1 --> Composer1
Composer1 --> Planner1
Planner1 --> LLM1
LLM1 --> Tool1
Tool1 --> Result1
```
- **RAG**：仅提供 `base_location_coords`（基准点坐标）和 `relative_descriptions`（相对描述列表），不调用 mcp_calc
- **LLM**：若用户说绝对地点，直接使用 base_location_coords；若用户说相对位置，必须调用 calculate_relative_coordinate，禁止心算
---
## 9. RAG 数据格式
### 9.1 map_db.jsonl
每行一个 JSON，字段示例：
```json
{"document": "大门位于入口处，坐标 x=0, y=0", "location": "大门", "x": 0, "y": 0, "z": 0}
```
### 9.2 rule_db.jsonl
```json
{"document": "起飞必须先做系统检查", "intent": "fly_task", "priority": "high"}
```
### 9.3 few_shot_db.jsonl
```json
{"document": "飞到大门然后拍照", "intent": "fly_task", "tree_json": "{\"root\": {...}}"}
```
---
## 10. 环境变量与配置
| 变量 | 默认值 | 说明 |
|------|--------|------|
| `LLM_CHAT_BASE_URL` | `http://localhost:8081/v1` | Chat API |
| `LLM_EMBEDDING_BASE_URL` | `http://localhost:8090/v1` | Embedding API（RAG） |
| `ENABLE_THINKING` | `false` | 模型思考模式 |
| `CHROMA_PERSIST_PATH` | `./data/chroma` | ChromaDB 存储路径 |
---
## 11. 文件索引
| 功能 | 文件路径 |
|------|----------|
| Router | `src/drone_planning/pipeline/router.py` |
| Composer | `src/drone_planning/pipeline/composer.py` |
| Planner | `src/drone_planning/pipeline/planner.py` |
| RAG Retriever | `src/drone_planning/rag/retriever.py` |
| RAG 向量存储 | `src/drone_planning/rag/vector_store.py` |
| RAG 灌入 | `src/drone_planning/rag/ingestion.py` |
| 坐标计算工具 | `src/drone_planning/tools/mcp_calc.py` |
| 执行包装 | `src/drone_planning/execution/tree_wrapper.py` |
| 节点实现 | `src/drone_planning/execution/nodes.py` |
| 黑板 | `src/drone_planning/core/blackboard.py` |
| LLM 客户端 | `src/drone_planning/llm_client/client.py` |
| API 路由 | `src/drone_planning/api/routes.py` |
| 节点 Schema | `config/node_schema.json` |
| 知识库 | `data/knowledge/*.jsonl` |

43
main.py Normal file
View File

@@ -0,0 +1,43 @@
"""
无人机行为规划后端 - FastAPI 入口
启动（请用 python -m 确保使用当前 conda 环境的依赖）:
python -m uvicorn main:app --reload --host 0.0.0.0 --port 8000
RAG 数据灌入（首次使用或更新知识库后需执行）:
python -m drone_planning.rag.ingestion
curl 示例:
curl -X POST http://localhost:8000/api/plan -H "Content-Type: application/json" -d '{"text": "飞到大门然后拍照"}'
curl -X POST http://localhost:8000/api/plan -H "Content-Type: application/json" -d '{"text": "起飞"}'
"""
from fastapi import FastAPI
from fastapi.middleware.cors import CORSMiddleware
from drone_planning.api.routes import router
app = FastAPI(
title="无人机行为规划后端",
description="从自然语言到行为树 JSON 的四层流水线",
version="0.1.0",
)
app.add_middleware(
CORSMiddleware,
allow_origins=["*"],
allow_credentials=True,
allow_methods=["*"],
allow_headers=["*"],
)
app.include_router(router)
@app.get("/")
def root():
return {"service": "drone-planning", "status": "ok"}
@app.get("/health")
def health():
return {"status": "healthy"}

235
playground.py Normal file
View File

@@ -0,0 +1,235 @@
"""
可视化交互测试台 - Streamlit
左侧边栏：切换 Blackboard.is_in_air（模拟在地面/空中）
主界面：输入自然语言指令，分步展示四层流水线输出。
"""
from __future__ import annotations
import json
import sys
import time
from pathlib import Path
# 确保 src 在路径中
sys.path.insert(0, str(Path(__file__).resolve().parent / "src"))
import streamlit as st
from drone_planning.core.blackboard import DroneStateBlackboard
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
st.set_page_config(page_title="无人机行为规划测试台", page_icon="🚁", layout="wide")
# 缩小环节耗时、intents、entities 等数据的字体
st.markdown("""
<style>
/* 环节耗时、intents、entities 等 metric 字体 */
div[data-testid="stMetric"] { font-size: 0.8rem !important; }
div[data-testid="stMetric"] label { font-size: 0.75rem !important; }
div[data-testid="stMetric"] [data-testid="stMetricValue"] { font-size: 0.85rem !important; }
/* JSON、code 块字体 */
div[data-testid="stJson"], .stJson { font-size: 0.8rem !important; }
div[data-testid="stCode"] code, .stCode code { font-size: 0.8rem !important; }
/* expander 内标题与内容 */
.streamlit-expanderHeader { font-size: 0.9rem !important; }
.streamlit-expanderContent { font-size: 0.85rem !important; }
/* caption 字体 */
.stCaptionContainer { font-size: 0.8rem !important; }
/* subheader 略小 */
h3 { font-size: 1rem !important; }
</style>
""", unsafe_allow_html=True)
st.title("🚁 无人机行为规划 - 四层流水线测试台")
st.caption("输入自然语言指令，观察 Layer 1~4 的逐步输出。切换「是否在空中」可验证 Layer 4 的起飞包装逻辑。")
# ---------------------------------------------------------------------------
# 左侧边栏：Blackboard 状态
# ---------------------------------------------------------------------------
with st.sidebar:
st.header("⚙️ 黑板状态")
is_in_air = st.toggle("是否在空中 (is_in_air)", value=False, help="关=在地面，开=已起飞。关掉时 Layer 4 会在业务树前插入 SystemCheck + Takeoff")
bb = DroneStateBlackboard()
bb.is_in_air = is_in_air
st.info(f"当前: {'已起飞' if is_in_air else '在地面'}")
# ---------------------------------------------------------------------------
# 主界面：输入与执行
# ---------------------------------------------------------------------------
instruction = st.text_area("输入自然语言指令", height=80, placeholder="例如：飞到大门然后拍照、起飞、飞到广场然后搜索汽车")
run_text = instruction.strip() if instruction else ""
if run_text:
st.divider()
if st.button("执行规划", type="primary", key="run_plan"):
# 再次同步 blackboard（用户可能在点击前改了侧边栏）
bb = DroneStateBlackboard()
bb.is_in_air = is_in_air
layer1_result = None
rag_context = None
layer2_prompt = None
layer2_meta = None
tool_call_log: list[dict] = []
layer3_json = None
layer4_ascii = None
fast_path = False
error_msg = None
timing: dict[str, float] = {}
try:
# Layer 1: Router
t0 = time.perf_counter()
layer1_result = route(run_text)
timing["Layer1_Router"] = (time.perf_counter() - t0) * 1000
fast_path = layer1_result.is_fast_path
if fast_path:
st.success("✅ Layer 1 触发 Fast-Path 短路，后续层不执行")
else:
# RAG 检索（阶段二）
t0 = time.perf_counter()
retriever = RAGRetriever()
rag_context = retriever.retrieve_context(
intents=layer1_result.intents,
entities=layer1_result.entities,
user_text=run_text,
)
timing["RAG_检索"] = (time.perf_counter() - t0) * 1000
# Layer 2: Composer
t0 = time.perf_counter()
schema_json = _load_node_schema()
layer2_prompt, layer2_meta = build_system_prompt(
intents=layer1_result.intents,
entities=layer1_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()
layer3_json = plan(
system_prompt=layer2_prompt,
user_text=run_text,
tool_call_log=tool_call_log,
)
timing["Layer3_Planner"] = (time.perf_counter() - t0) * 1000
# Layer 4: 解析 + 包装（会读取 blackboard.is_in_air）
t0 = time.perf_counter()
root_node = layer3_json.get("root") or layer3_json
business_tree = parse_json_to_tree(root_node)
final_tree = wrap_and_build_tree(business_tree)
layer4_ascii = tree_to_ascii(final_tree)
timing["Layer4_Execution"] = (time.perf_counter() - t0) * 1000
except Exception as e:
error_msg = str(e)
import traceback
st.error(f"❌ 执行出错: {error_msg}")
st.code(traceback.format_exc(), language="text")
# -----------------------------------------------------------------------
# 各环节耗时
# -----------------------------------------------------------------------
if timing:
st.subheader("⏱️ 各环节耗时 (ms)")
cols = st.columns(len(timing))
for i, (name, ms) in enumerate(timing.items()):
with cols[i]:
st.metric(name, f"{ms:.0f} ms")
st.caption(f"总计: {sum(timing.values()):.0f} ms")
# -----------------------------------------------------------------------
# 分步展示
# -----------------------------------------------------------------------
st.subheader("📋 各层输出")
# Layer 1
with st.expander("**Layer 1 (Router)** - 意图与实体", expanded=True):
if layer1_result:
col1, col2, col3 = st.columns(3)
with col1:
st.metric("intents", ", ".join(layer1_result.intents) or "-")
with col2:
ent_str = json.dumps(layer1_result.entities, ensure_ascii=False)
st.metric("entities", ent_str[:60] + "..." if len(ent_str) > 60 else ent_str or "-")
with col3:
st.metric("Fast-Path", "是" if fast_path else "否")
st.json(layer1_result.entities)
else:
st.warning("Layer 1 未执行或出错")
if not fast_path:
# RAG 检索结果（Layer 1 与 Layer 2 之间）
with st.expander("**RAG 检索结果** - 地图坐标、规则、Few-shot 示例", expanded=True):
if rag_context:
col1, col2 = st.columns(2)
with col1:
st.markdown("**地图坐标 (map_context)**")
if rag_context.get("map_context"):
st.code(rag_context["map_context"], language=None)
else:
st.caption("无匹配")
with col2:
st.markdown("**规则约束 (rule_context)**")
if rag_context.get("rule_context"):
st.code(rag_context["rule_context"], language=None)
else:
st.caption("无匹配")
st.markdown("**Few-shot 示例**")
if rag_context.get("few_shot_examples"):
for i, ex in enumerate(rag_context["few_shot_examples"]):
st.json({"instruction": ex.get("instruction"), "tree_json": ex.get("tree_json")})
else:
st.caption("无相似示例")
else:
st.caption("RAG 未执行")
# Layer 2
with st.expander("**Layer 2 (Composer)** - 精简 Prompt 与选中节点", expanded=True):
if layer2_meta:
st.metric("Prompt 长度", f"{layer2_meta.get('prompt_length', 0)} 字符")
st.metric("选中节点", ", ".join(layer2_meta.get("selected_actions", [])))
st.metric("基准点坐标", json.dumps(layer2_meta.get("base_location_coords", {}), ensure_ascii=False))
if layer2_prompt:
preview = layer2_prompt[:2000] + ("..." if len(layer2_prompt) > 2000 else "")
st.code(preview, language="text")
# 坐标计算与 Tool Call 日志（仅 LLM 通过 mcp 工具计算）
rel_descs = (rag_context or {}).get("relative_descriptions") or []
has_any = bool(rel_descs) or bool(tool_call_log)
with st.expander("**坐标计算与 Tool Call 日志**", expanded=has_any):
if rel_descs:
st.markdown("**RAG 提供的相对描述**（需 LLM 调用 calculate_relative_coordinate）")
for rd in rel_descs:
st.json(rd)
if tool_call_log:
st.markdown("**LLM Tool Call**（大模型调用 mcp 计算）")
for entry in tool_call_log:
st.markdown(f"第 {entry.get('round', '?')} 次 `{entry.get('tool', '')}`")
st.json({"arguments": entry.get("arguments"), "result": entry.get("result")})
if not rel_descs and not tool_call_log:
st.caption("无相对描述（指令仅含绝对地点时，直接使用 base_location_coords）")
# Layer 3
with st.expander("**Layer 3 (Planner)** - 原生业务树 JSON", expanded=True):
if layer3_json:
st.json(layer3_json)
else:
st.warning("Layer 3 未执行")
# Layer 4
with st.expander("**Layer 4 (Execution)** - 安全包装后的 py_trees ASCII 结构", expanded=True):
if layer4_ascii:
st.text("当 is_in_air=False 时，此处应看到 [Safe_Execution] -> [SystemCheck] -> [Takeoff] -> 业务树")
st.code(layer4_ascii, language=None)
else:
st.warning("Layer 4 未执行")

26
pyproject.toml Normal file
View File

@@ -0,0 +1,26 @@
[build-system]
requires = ["setuptools>=61.0"]
build-backend = "setuptools.build_meta"
[project]
name = "drone-planning"
version = "0.1.0"
description = "无人机行为规划后端系统 - 从自然语言到行为树"
readme = "README.md"
requires-python = ">=3.10"
dependencies = [
"fastapi>=0.109.0",
"uvicorn>=0.27.0",
"httpx>=0.26.0",
"py_trees>=2.2.0",
"openai>=1.0.0",
"pydantic>=2.5.0",
"streamlit>=1.29.0",
]
[project.optional-dependencies]
dev = ["pytest>=7.0"]
[tool.setuptools.packages.find]
where = ["src"]
include = ["drone_planning*"]

11
requirements.txt Normal file
View File

@@ -0,0 +1,11 @@
# 无人机行为规划后端系统 - 依赖清单
# 阶段一 + 阶段二 RAG
fastapi>=0.109.0
uvicorn>=0.27.0
httpx>=0.26.0
py_trees>=2.2.0
openai>=1.0.0
pydantic>=2.5.0
streamlit>=1.29.0
chromadb>=0.4.0

36
run_api.sh Executable file
View File

@@ -0,0 +1,36 @@
#!/bin/bash
# 启动 LLM 服务 + DronePlanning API
# - Chat 模型：8081
# - Embedding 模型：8090
# - API：8000
cd "$(dirname "$0")"
# llama-server 所在目录（可在该目录下启动模型）
LLAMA_BIN_DIR="${LLAMA_BIN_DIR:-$HOME/llama.cpp/build/bin}"
cleanup() {
echo ""
echo "正在停止服务..."
kill $LLAMA_CHAT_PID $LLAMA_EMBED_PID 2>/dev/null
exit 0
}
trap cleanup SIGINT SIGTERM
# 启动 Chat 模型 (8081)
echo "=== 启动 LLM Chat 服务 (端口 8081) ==="
(cd "$LLAMA_BIN_DIR" && ./llama-server -m ~/models/gguf/Qwen3/Qwen3-4B/Qwen3-4B-Q5_K_M.gguf --port 8081 --gpu_layers 36 --host 0.0.0.0) &
LLAMA_CHAT_PID=$!
# 启动 Embedding 模型 (8090)
echo "=== 启动 LLM Embedding 服务 (端口 8090) ==="
(cd "$LLAMA_BIN_DIR" && ./llama-server -m ~/models/gguf/Qwen3/Qwen3-Embedding-4B/Qwen3-Embedding-4B-Q5_K_M.gguf --gpu_layers 36 --embeddings --port 8090 --host 0.0.0.0) &
LLAMA_EMBED_PID=$!
# 等待 LLM 服务就绪
echo "等待 LLM 服务启动..."
sleep 8
# 启动 DronePlanning API (8000)
echo "=== 启动 DronePlanning API (端口 8000) ==="
python -m uvicorn main:app --reload --host 0.0.0.0 --port 8000

6
src/drone_planning/__init__.py Normal file
View File

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

1
src/drone_planning/api/__init__.py Normal file
View File

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

115
src/drone_planning/api/routes.py Normal file
View File

@@ -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}")

1
src/drone_planning/core/__init__.py Normal file
View File

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

97
src/drone_planning/core/blackboard.py Normal file
View File

@@ -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()

1
src/drone_planning/execution/__init__.py Normal file
View File

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

71
src/drone_planning/execution/nodes.py Normal file
View File

@@ -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

107
src/drone_planning/execution/tree_wrapper.py Normal file
View File

@@ -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)

1
src/drone_planning/llm_client/__init__.py Normal file
View File

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

152
src/drone_planning/llm_client/client.py Normal file
View File

@@ -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)

1
src/drone_planning/pipeline/__init__.py Normal file
View File

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

171
src/drone_planning/pipeline/composer.py Normal file
View File

@@ -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

204
src/drone_planning/pipeline/planner.py Normal file
View File

@@ -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)

205
src/drone_planning/pipeline/router.py Normal file
View File

@@ -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)

1
src/drone_planning/rag/__init__.py Normal file
View File

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

63
src/drone_planning/rag/embedding_client.py Normal file
View File

@@ -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]

136
src/drone_planning/rag/ingestion.py Normal file
View File

@@ -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)

203
src/drone_planning/rag/retriever.py Normal file
View File

@@ -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

149
src/drone_planning/rag/vector_store.py Normal file
View File

@@ -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 示例库"},
)

1
src/drone_planning/tools/__init__.py Normal file
View File

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

38
src/drone_planning/tools/geo.py Normal file
View File

@@ -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 {}

142
src/drone_planning/tools/mcp_calc.py Normal file
View File

@@ -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,
}