4、行为树（BT）调试：使用Groot2可视化行为树、自定义BT节点调试、常见树结构错误修复

行为树这东西，刚接触时觉得挺玄乎。说白了，它就是一套控制机器人行为的“决策流程图”。但树一旦复杂起来，节点一多，光靠看代码和日志，你根本不知道机器人为什么突然“抽风”。

我个人习惯，调试BT必须上Groot2。这工具就像给行为树装了X光，树怎么跑的、哪个节点卡住了、返回值是什么，一眼就能看穿。

4.1 用Groot2把行为树“可视化”

Groot2是Navigation2官方推荐的可视化调试工具。它不仅能看树结构，还能实时回放节点状态。

安装很简单：

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

启动后，点击“Load Tree”加载你的行为树XML文件。我建议你同时打开ROS2话题监控，这样Groot2能订阅/bt_events话题，实时显示节点切换。

4.2 自定义BT节点的调试方法

写自定义节点是家常便饭。但节点写错了，树就跑偏。我遇到过最坑的一次，是自己写了个CheckBattery节点，返回值逻辑写反了——电量低时返回SUCCESS，结果机器人一直去充电桩“休息”。

调试自定义节点，我总结了三个步骤：

1. 加日志，别省：在节点的tick()函数里，把输入端口值、内部状态、返回值都打印出来。
2. 用Groot2的“Monitor”模式：右键节点选择“Monitor”，可以实时查看该节点的黑板变量和端口值。
3. 写单元测试：单独测试节点的逻辑，别等集成到树上再查。

举个例子，一个简单的自定义节点：

#include "behaviortree_cpp/action_node.h"

class CheckObstacle : public BT::SyncActionNode
{
public:
  CheckObstacle(const std::string& name, const BT::NodeConfig& config)
    : BT::SyncActionNode(name, config)
  {
    // 构造函数里初始化
  }

  static BT::PortsList providedPorts()
  {
    return { BT::InputPort<double>("distance") };
  }

  BT::NodeStatus tick() override
  {
    double dist;
    if (!getInput<double>("distance", dist)) {
      // 输入端口读取失败，返回FAILURE
      BT_LOG_ERROR("Failed to read input port 'distance'");
      return BT::NodeStatus::FAILURE;
    }

    BT_LOG_INFO("CheckObstacle: distance = %.2f", dist);

    if (dist < 0.5) {
      return BT::NodeStatus::FAILURE;  // 有障碍物
    }
    return BT::NodeStatus::SUCCESS;    // 无障碍物
  }
};

4.3 常见树结构错误与修复

树结构错误，是新手最容易踩的坑。我整理了几种高频问题：

错误类型	现象	原因	修复方法
死循环	机器人反复执行同一个动作	ReactiveFallback或Sequence的子节点没有正确返回SUCCESS或FAILURE	检查子节点返回值逻辑，确保有终止条件
树提前终止	行为执行到一半突然停止	某个Action节点返回了FAILURE，而父节点是Sequence	将父节点改为ReactiveSequence或添加Fallback处理失败情况
黑板变量未定义	节点报错“port not found”	子节点读取了父节点未设置的端口	在XML中显式定义<Blackboard>，或使用setOutput设置默认值
节点超时	Groot2显示节点一直“RUNNING”	异步节点没有正确设置超时时间	在tick()中检查时间戳，超时后返回FAILURE

我曾经遇到一个特别隐蔽的错误：Sequence节点下挂了三个子节点，第二个子节点偶尔返回FAILURE，导致整个序列中断。当时我盯着Groot2看了半小时，才发现是传感器数据抖动引起的。解决方案很简单——在CheckObstacle节点里加了一个去抖逻辑，连续三次检测到障碍物才返回FAILURE。

4.4 行为树调试的核心流程

下面这张图，是我自己总结的调试流程。每次遇到BT问题，我就按这个步骤来：

嗯，这套流程我用了两年，基本能解决90%的BT问题。剩下的10%，往往是硬件或传感器数据本身的问题，那就得结合其他调试手段了。