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

Made-with: Cursor

src/drone_planning/core/__init__.py

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

src/drone_planning/core/blackboard.py

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