3、行为树基础：行为树节点类型与Nav2默认行为树解析

各位同学，欢迎来到第三章。前两章我们聊了Nav2的整体架构和导航栈的搭建。从这章开始，我们要深入一个非常核心的概念——行为树。

说实话，我第一次接触行为树的时候，觉得它不就是个高级点的状态机吗？后来在项目中踩了不少坑，才真正体会到它的威力。尤其是做机器人导航这种需要处理大量异常情况的系统，行为树的灵活性和可维护性，是传统状态机完全没法比的。

3.1 行为树节点类型：四种基本角色

行为树的节点，说白了就四种类型。我习惯把它们比作一个团队里的不同角色：

• Control Node（控制节点）：项目经理，决定下一步谁干活
• Decorator Node（装饰节点）：质检员，给任务加条件限制
• Action Node（动作节点）：执行者，真正干活的
• Condition Node（条件节点）：侦察兵，只负责打探消息

每种节点都有固定的执行逻辑，咱们一个一个来看。

3.1.1 Control Node（控制节点）

控制节点是行为树的骨架。它不干具体活，但决定子节点的执行顺序和策略。最常用的有这几个：

这里有个细节要注意：Sequence 和 Fallback 都有带记忆和不带记忆的版本。带记忆的版本（如 SequenceWithMemory）在子节点失败后，下次重跑时会从失败的地方继续，而不是从头开始。我在做多楼层导航时就用过这个特性，省了不少重复计算。

3.1.2 Decorator Node（装饰节点）

装饰节点，顾名思义，就是给子节点「装饰」一层额外的逻辑。它只有一个子节点，负责修改子节点的行为。

常用的装饰节点：

• Repeat：重复执行子节点N次
• Retry：子节点失败时重试N次
• Inverter：反转子节点的结果（成功变失败，失败变成功）
• Timeout：给子节点设置超时时间
• ForceSuccess：不管子节点返回什么，都返回成功

3.1.3 Action Node（动作节点）

动作节点是真正干活的。在Nav2里，动作节点通常对应一个ROS2 Action。它有三个状态：RUNNING（正在执行）、SUCCESS（成功）、FAILURE（失败）。

Nav2中常见的动作节点：

• ComputePathToPose：计算路径
• FollowPath：跟踪路径
• Spin：原地旋转
• BackUp：后退
• Wait：等待指定时间

动作节点可以持续返回 RUNNING 状态，这意味着行为树不会卡住。比如机器人正在导航，FollowPath 会一直返回 RUNNING，直到到达目标或遇到障碍。

3.1.4 Condition Node（条件节点）

条件节点只做一件事：检查某个条件是否满足。它返回 SUCCESS 或 FAILURE，不会返回 RUNNING，也不会修改任何状态。

Nav2中的条件节点：

• IsGoalReached：检查是否到达目标
• IsStuck：检查机器人是否卡住
• GoalUpdated：检查是否有新目标
• IsPathValid：检查路径是否有效

3.2 使用Groot2可视化编辑行为树

光说理论太枯燥。咱们来点实际的——用Groot2来编辑行为树。

Groot2是BehaviorTree.CPP官方推出的可视化编辑器。我个人觉得，它最大的价值不是「画图」，而是让你能直观地看到行为树的执行过程。

3.2.1 安装与启动

安装很简单：

sudo apt install ros-humble-groot2
# 或者从源码编译
git clone https://github.com/BehaviorTree/Groot2.git
cd Groot2
mkdir build && cd build
cmake ..
make -j4

启动后，你会看到一个空白的画布。左侧是节点面板，右侧是属性编辑器。

3.2.2 创建你的第一个行为树

咱们来创建一个简单的导航行为树：

1. 从左侧拖一个 Sequence 到画布
2. 在Sequence下添加三个子节点：ComputePathToPose、FollowPath、IsGoalReached
3. 给 ComputePathToPose 设置参数：goal、planner_id
4. 给 FollowPath 设置参数：controller_id

保存为 navigate.xml。这就是一个最简的导航行为树。

3.3 Nav2默认行为树解析

Nav2自带了好几个行为树模板。咱们来拆解最常用的那个——navigate_to_pose_w_replanning_and_recovery.xml。

先看整体结构：

这个行为树的核心逻辑是：

1. 根节点是 Fallback：先尝试主任务，主任务失败就尝试恢复策略
2. 主任务是一个 Sequence：RateController → ComputePath → FollowPath → CheckGoal
3. 恢复策略也是一个 Fallback：Spin → BackUp → Wait → ClearCostmap，逐个尝试

你想想看，这个设计妙在哪里？

妙在「失败」不是终点。当FollowPath失败时，行为树不会直接放弃，而是进入恢复分支。Spin转一下、BackUp退一下、Wait等一等、ClearCostmap清一下地图——总有一个能解决问题。如果所有恢复策略都失败了，那才真正返回失败。

3.4 行为树的执行流程

理解了节点类型和默认行为树的结构，咱们来看看行为树到底是怎么「跑」起来的。

行为树的执行是深度优先的。每次tick（心跳）从根节点开始，逐层向下传递。每个节点执行完后，把结果返回给父节点。

举个例子，假设机器人正在导航：

1. 根节点 Fallback tick 第一个子节点（Sequence）
2. Sequence tick 第一个子节点（RateController）
3. RateController 检查是否到了重新规划的时间，返回 SUCCESS
4. Sequence 继续 tick 第二个子节点（ComputePathToPose）
5. ComputePathToPose 计算路径，返回 SUCCESS
6. Sequence 继续 tick 第三个子节点（FollowPath）
7. FollowPath 开始控制机器人移动，返回 RUNNING
8. Sequence 因为子节点返回 RUNNING，自己也返回 RUNNING
9. 根节点 Fallback 收到 RUNNING，继续等待

这个过程每秒会重复很多次（取决于你的 tick 频率）。每次 tick 都是一次完整的深度优先遍历。

3.5 自定义节点：让行为树听懂你的话

Nav2自带的节点能满足大部分场景，但总有些特殊需求需要自己写节点。比如，我做过一个项目，需要在导航前先检查电量，电量低于20%就先去充电。

自定义一个条件节点很简单：

#include "behaviortree_cpp/condition_node.h"

class CheckBattery : public BT::ConditionNode
{
public:
    CheckBattery(const std::string& name, const BT::NodeConfig& config)
        : BT::ConditionNode(name, config)
    {
        // 初始化
    }

    static BT::PortsList providedPorts()
    {
        return {
            BT::InputPort<float>("threshold", 0.2, "电量阈值")
        };
    }

    BT::NodeStatus tick() override
    {
        float threshold;
        getInput("threshold", threshold);
        
        float battery = getBatteryLevel(); // 假设有这个函数
        
        if (battery > threshold) {
            return BT::NodeStatus::SUCCESS;
        } else {
            return BT::NodeStatus::FAILURE;
        }
    }
};

<root main_tree_to_execute="MainTree">
    <BehaviorTree ID="MainTree">
        <Fallback>
            <Sequence>
                <CheckBattery threshold="0.2"/>
                <NavigateToPose goal="{goal}"/>
            </Sequence>
            <NavigateToPose goal="{charging_station}"/>
        </Fallback>
    </BehaviorTree>
</root>