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

Made-with: Cursor

src/drone_planning/execution/__init__.py

@@ -0,0 +1 @@
+"""执行层：py_trees 节点与树包装"""

src/drone_planning/execution/nodes.py

@@ -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

src/drone_planning/execution/tree_wrapper.py

@@ -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)