Initial commit: 无人机行为规划后端系统
Made-with: Cursor
This commit is contained in:
265
# 无人机行为规划后端系统（重建版）需求说明.md
Normal file
265
# 无人机行为规划后端系统（重建版）需求说明.md
Normal 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
43
.gitignore
vendored
Normal 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
181
README.md
Normal 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
109
config/node_schema.json
Normal 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
3
data/knowledge/few_shot_db.jsonl
Normal 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
5
data/knowledge/map_db.jsonl
Normal 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
5
data/knowledge/rule_db.jsonl
Normal 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
391
docs/项目流程详解.md
Normal 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
43
main.py
Normal 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
235
playground.py
Normal 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
26
pyproject.toml
Normal 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
11
requirements.txt
Normal 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
36
run_api.sh
Executable 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
6
src/drone_planning/__init__.py
Normal file
@@ -0,0 +1,6 @@
|
||||
"""
|
||||
无人机行为规划后端系统
|
||||
从自然语言到无人机行为树 JSON 的四层流水线
|
||||
"""
|
||||
|
||||
__version__ = "0.1.0"
|
||||
1
src/drone_planning/api/__init__.py
Normal file
1
src/drone_planning/api/__init__.py
Normal file
@@ -0,0 +1 @@
|
||||
"""API 模块"""
|
||||
115
src/drone_planning/api/routes.py
Normal file
115
src/drone_planning/api/routes.py
Normal 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
1
src/drone_planning/core/__init__.py
Normal file
@@ -0,0 +1 @@
|
||||
"""核心模块：黑板、状态等"""
|
||||
97
src/drone_planning/core/blackboard.py
Normal file
97
src/drone_planning/core/blackboard.py
Normal 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
1
src/drone_planning/execution/__init__.py
Normal file
@@ -0,0 +1 @@
|
||||
"""执行层：py_trees 节点与树包装"""
|
||||
71
src/drone_planning/execution/nodes.py
Normal file
71
src/drone_planning/execution/nodes.py
Normal 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
107
src/drone_planning/execution/tree_wrapper.py
Normal 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
1
src/drone_planning/llm_client/__init__.py
Normal file
@@ -0,0 +1 @@
|
||||
"""LLM 客户端：对接 llama-server（OpenAI 兼容 API）"""
|
||||
152
src/drone_planning/llm_client/client.py
Normal file
152
src/drone_planning/llm_client/client.py
Normal 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
1
src/drone_planning/pipeline/__init__.py
Normal file
@@ -0,0 +1 @@
|
||||
"""流水线模块：意图路由、动态组装、宏观规划"""
|
||||
171
src/drone_planning/pipeline/composer.py
Normal file
171
src/drone_planning/pipeline/composer.py
Normal 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
204
src/drone_planning/pipeline/planner.py
Normal 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
205
src/drone_planning/pipeline/router.py
Normal 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
1
src/drone_planning/rag/__init__.py
Normal file
@@ -0,0 +1 @@
|
||||
"""RAG 模块：Embedding、向量存储、检索、灌入"""
|
||||
63
src/drone_planning/rag/embedding_client.py
Normal file
63
src/drone_planning/rag/embedding_client.py
Normal 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
136
src/drone_planning/rag/ingestion.py
Normal 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
203
src/drone_planning/rag/retriever.py
Normal 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
149
src/drone_planning/rag/vector_store.py
Normal 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
1
src/drone_planning/tools/__init__.py
Normal file
@@ -0,0 +1 @@
|
||||
"""工具模块：坐标解析、地理信息等"""
|
||||
38
src/drone_planning/tools/geo.py
Normal file
38
src/drone_planning/tools/geo.py
Normal 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
142
src/drone_planning/tools/mcp_calc.py
Normal 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,
|
||||
}
|
||||
Reference in New Issue
Block a user