98 lines
2.8 KiB
Python
98 lines
2.8 KiB
Python
"""
|
||
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()
|