diff --git a/backend_service/src/prompts/scene4_prompt.txt b/backend_service/src/prompts/scene4_prompt.txt index 87479e8c..fd88d7e8 100644 --- a/backend_service/src/prompts/scene4_prompt.txt +++ b/backend_service/src/prompts/scene4_prompt.txt @@ -299,10 +299,20 @@ **仅当指令涉及前往“具体地点”（如广场、大门）的偏移位置时，才需计算绝对坐标并使用`fly_to_waypoint`：** -当指令包含“具体地点 + 方向 + 距离”的偏移（如“广场西边200米”）时，**必须**先调用工具`calc_offset_enu`计算绝对坐标，再使用`fly_to_waypoint`。工具参数： -- `base`: 参考地点的ENU坐标（含x/y/z） -- `direction`: east/west/north/south/up/down -- `distance`: 偏移距离（米） +当指令包含“具体地点 + 方向 + 距离”的偏移时，按方向类型选择工具： +- **单一方向**（东/西/南/北/上/下，如“广场西边200米”）：**必须**先调用工具`calc_offset_enu`计算绝对坐标，再使用`fly_to_waypoint`。工具参数： + - `base`: 参考地点的ENU坐标（含x/y/z） + - `direction`: east/west/north/south/up/down + - `distance`: 偏移距离（米） +- **中文复合方位**（东南/西北/东北/西南/南偏东10度/北偏西15度等）：**必须**先调用工具`calc_offset_esu_direction_text`计算绝对坐标，再使用`fly_to_waypoint`。工具参数： + - `base`: 参考地点的ENU坐标（含x/y/z） + - `direction_text`: 中文方位原文（如“东南”“南偏东10度”） + - `distance`: 偏移距离（米） +- **禁止简化**：当出现“东南/西北/东北/西南/南偏东/北偏西”等复合方位时，禁止将其简化为单一方向（如仅“东”或仅“南”）。 + +示例（必须遵守）： +- “飞到广场东南方向100米” → 必须调用 `calc_offset_esu_direction_text`，参数 `direction_text` 为 `"东南"`，`distance` 为 `100`，再使用 `fly_to_waypoint`。 +- “飞到广场南偏东10度100米” → 必须调用 `calc_offset_esu_direction_text`，参数 `direction_text` 为 `"南偏东10度"`，`distance` 为 `100`。 当指令只有“方向 + 距离”且**没有具体地点名词**时，**禁止**调用`calc_offset_enu`，必须使用`move_direction`。 当指令描述“附近/边上/区域内”等模糊位置且**无方向+距离**时，视为到该地点本身，不做偏移计算。 diff --git a/backend_service/src/prompts/system_prompt.txt b/backend_service/src/prompts/system_prompt.txt index fe9aebbd..b4869af0 100644 --- a/backend_service/src/prompts/system_prompt.txt +++ b/backend_service/src/prompts/system_prompt.txt @@ -789,10 +789,20 @@ **仅当指令涉及前往“具体地点”（如广场、大门）的偏移位置时，才需计算绝对坐标并使用`fly_to_waypoint`：** -当指令包含“具体地点 + 方向 + 距离”的偏移（如“广场西边200米”）时，**必须**先调用工具`calc_offset_enu`计算绝对坐标，再使用`fly_to_waypoint`。工具参数： -- `base`: 参考地点的ENU坐标（含x/y/z） -- `direction`: east/west/north/south/up/down -- `distance`: 偏移距离（米） +当指令包含“具体地点 + 方向 + 距离”的偏移时，按方向类型选择工具： +- **单一方向**（东/西/南/北/上/下，如“广场西边200米”）：**必须**先调用工具`calc_offset_enu`计算绝对坐标，再使用`fly_to_waypoint`。工具参数： + - `base`: 参考地点的ENU坐标（含x/y/z） + - `direction`: east/west/north/south/up/down + - `distance`: 偏移距离（米） +- **中文复合方位**（东南/西北/东北/西南/南偏东10度/北偏西15度等）：**必须**先调用工具`calc_offset_esu_direction_text`计算绝对坐标，再使用`fly_to_waypoint`。工具参数： + - `base`: 参考地点的ENU坐标（含x/y/z） + - `direction_text`: 中文方位原文（如“东南”“南偏东10度”） + - `distance`: 偏移距离（米） +- **禁止简化**：当出现“东南/西北/东北/西南/南偏东/北偏西”等复合方位时，禁止将其简化为单一方向（如仅“东”或仅“南”）。 + +示例（必须遵守）： +- “飞到广场东南方向100米” → 必须调用 `calc_offset_esu_direction_text`，参数 `direction_text` 为 `"东南"`，`distance` 为 `100`，再使用 `fly_to_waypoint`。 +- “飞到广场南偏东10度100米” → 必须调用 `calc_offset_esu_direction_text`，参数 `direction_text` 为 `"南偏东10度"`，`distance` 为 `100`。 当指令只有“方向 + 距离”且**没有具体地点名词**时，**禁止**调用`calc_offset_enu`，必须使用`move_direction`。 当指令描述“附近/边上/区域内”等模糊位置且**无方向+距离**时，视为到该地点本身，不做偏移计算。 diff --git a/backend_service/src/tools/__pycache__/__init__.cpython-310.pyc b/backend_service/src/tools/__pycache__/__init__.cpython-310.pyc new file mode 100644 index 00000000..2f6d5541 Binary files /dev/null and b/backend_service/src/tools/__pycache__/__init__.cpython-310.pyc differ diff --git a/backend_service/src/tools/__pycache__/coordinate_tools.cpython-310.pyc b/backend_service/src/tools/__pycache__/coordinate_tools.cpython-310.pyc new file mode 100644 index 00000000..d438c821 Binary files /dev/null and b/backend_service/src/tools/__pycache__/coordinate_tools.cpython-310.pyc differ diff --git a/backend_service/src/tools/coordinate_tools.py b/backend_service/src/tools/coordinate_tools.py index 41c9aa0a..10056092 100644 --- a/backend_service/src/tools/coordinate_tools.py +++ b/backend_service/src/tools/coordinate_tools.py @@ -1,5 +1,7 @@ from __future__ import annotations +import math +import re from typing import Dict, Tuple @@ -37,3 +39,72 @@ def calc_offset_enu(base: Dict[str, float], direction: str, distance: float) -> offset_z = z + dz * distance return {"x": offset_x, "y": offset_y, "z": offset_z} + + +_DIRECTION_TEXT_DIAGONALS_ESU: Dict[str, Tuple[float, float]] = { + "东南": (1.0, 1.0), + "西南": (-1.0, 1.0), + "东北": (1.0, -1.0), + "西北": (-1.0, -1.0), +} + + +def _parse_direction_text_esu(direction_text: str) -> Tuple[float, float]: + text = (direction_text or "").strip().replace(" ", "") + if not text: + raise ValueError("direction_text must be a non-empty string") + + for prefix in ("向", "正"): + if text.startswith(prefix): + text = text[len(prefix):] + + if text in _DIRECTION_TEXT_DIAGONALS_ESU: + x, y = _DIRECTION_TEXT_DIAGONALS_ESU[text] + length = math.hypot(x, y) + return x / length, y / length + + if text in ("东", "西", "南", "北"): + if text == "东": + return 1.0, 0.0 + if text == "西": + return -1.0, 0.0 + if text == "南": + return 0.0, 1.0 + return 0.0, -1.0 + + match = re.match(r"^(南|北)偏(东|西)(\d+(?:\.\d+)?)度?$", text) + if match: + base_dir, toward_dir, angle_str = match.groups() + angle_deg = float(angle_str) + if angle_deg < 0 or angle_deg > 90: + raise ValueError("angle must be between 0 and 90 degrees") + angle_rad = math.radians(angle_deg) + x_sign = 1.0 if toward_dir == "东" else -1.0 + if base_dir == "南": + return math.sin(angle_rad) * x_sign, math.cos(angle_rad) + return math.sin(angle_rad) * x_sign, -math.cos(angle_rad) + + raise ValueError(f"unsupported direction_text: {direction_text}") + + +def calc_offset_esu_direction_text( + base: Dict[str, float], direction_text: str, distance: float +) -> Dict[str, float]: + """在ESU坐标系下按中文方位偏移固定距离。""" + if distance < 0: + raise ValueError("distance must be non-negative") + if not isinstance(base, dict): + raise ValueError("base must be an object with x/y/z") + + try: + x = float(base["x"]) + y = float(base["y"]) + z = float(base["z"]) + except Exception as exc: + raise ValueError("base must contain numeric x/y/z") from exc + + dx_unit, dy_unit = _parse_direction_text_esu(direction_text) + offset_x = x + dx_unit * distance + offset_y = y + dy_unit * distance + + return {"x": offset_x, "y": offset_y, "z": z}