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

Made-with: Cursor

src/drone_planning/tools/__init__.py

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

src/drone_planning/tools/geo.py

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

src/drone_planning/tools/mcp_calc.py

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