使用行为树控制机器人(一) —— 节点

使用行为树控制机器人(一) —— 节点
使用行为树控制机器人(二) —— 黑板
使用行为树控制机器人(三) —— 通用端口

近期在从ros1导航跨到ros2导航，ros2导航的任务调度逻辑采用的是行为树，所以在魔改ros2导航前先知己知彼学习一下行为树到底是个啥？本文也是在大佬的基础上加入自己的学习过程记录。(如整理有误，还请指点)
学习时参考链接：ROS机器人行为树教程

一、背景需求

关于行为树各节点的定义及其作用的博客整理，可以先参考链接中的教程，后续看自己是否有时间整理，本文直接根据需求进行代码实操。通过行为树控制机器人的一简单场景需求如下：

二、创建ActionNodes

1. 功能实现

对于每个动作作为简单案例多以打印输出后，直接返回成功，后续根据对行为树的掌握程度结合实际需求进行自定义复杂需求。

1.1 头文件定义

根据场景需求创建如下 SimpleActionNode：

• CheckBattery()
• GripperInterface::open()
• GripperInterface::close()
• CameraInterface::open()
• CameraInterface::close()

#ifndef BEHAVIOR_TREE_NODES_H
#define BEHAVIOR_TREE_NODES_H#include "behaviortree_cpp/bt_factory.h"
#include <iostream>// 同步动作节点 (无端口)
class ApproachObject : public BT::SyncActionNode
{
public:ApproachObject(const std::string& name, const BT::NodeConfig& config) : BT::SyncActionNode(name, config){}// 端口是节点与黑板（Blackboard）之间进行数据交换的接口// 必须实现静态端口声明方法static BT::PortsList providedPorts() {return {}; // 无端口}BT::NodeStatus tick() override;
};// 夹爪控制接口
class GripperInterface
{
public:GripperInterface() : _open(true) {}BT::NodeStatus open();BT::NodeStatus close();private:bool _open; // 共享状态
};// 相机控制接口
class CameraInterface
{
public:CameraInterface() : _open(true) {}BT::NodeStatus open();BT::NodeStatus close();private:bool _open; // 共享状态
};// 电池检查函数声明
BT::NodeStatus CheckBattery();#endif // BEHAVIOR_TREE_NODES_H

1.2 源文件实现

#ifndef BEHAVIOR_TREE_NODES_H
#define BEHAVIOR_TREE_NODES_H#include "behaviortree_cpp/bt_factory.h"
#include <iostream>// 同步动作节点 (无端口)
class ApproachObject : public BT::SyncActionNode
{
public:ApproachObject(const std::string& name, const BT::NodeConfig& config) : BT::SyncActionNode(name, config){}// 端口是节点与黑板（Blackboard）之间进行数据交换的接口// 必须实现静态端口声明方法static BT::PortsList providedPorts() {return {}; // 无端口}BT::NodeStatus tick() override;
};// 夹爪控制接口
class GripperInterface
{
public:GripperInterface() : _open(true) {}BT::NodeStatus open();BT::NodeStatus close();private:bool _open; // 共享状态
};// 相机打开
BT::NodeStatus CameraInterface::open() 
{_open = true;std::cout << "CameraInterface::open" << std::endl;return BT::NodeStatus::SUCCESS;
}// 相机关闭
BT::NodeStatus CameraInterface::close() 
{std::cout << "CameraInterface::close" << std::endl;_open = false;return BT::NodeStatus::SUCCESS;
}// 电池检查函数声明
BT::NodeStatus CheckBattery();#endif // BEHAVIOR_TREE_NODES_H

1.3 main文件实现

#include "behavior_tree_nodes.h"
#include "behaviortree_cpp/bt_factory.h"int main()
{// 创建行为树工厂BT::BehaviorTreeFactory factory;// 注册自定义节点factory.registerNodeType<ApproachObject>("ApproachObject");// 注册简单条件节点 (使用正确的lambda签名)factory.registerSimpleCondition("CheckBattery", [](BT::TreeNode&) { return CheckBattery(); });// 创建相机、夹爪实例并注册动作CameraInterface camera;GripperInterface gripper;factory.registerSimpleAction("OpenCamera", [&camera](BT::TreeNode&) { return camera.open(); });factory.registerSimpleAction("OpenGripper", [&gripper](BT::TreeNode&) { return gripper.open(); });factory.registerSimpleAction("CloseGripper", [&gripper](BT::TreeNode&) { return gripper.close(); });factory.registerSimpleAction("CloseCamera", [&camera](BT::TreeNode&) { return camera.close(); });try {// 从XML文件加载行为树auto tree = factory.createTreeFromFile("../trees/my_tree.xml");// 打印树结构 (调试用)std::cout << "------ Behavior Tree Structure ------" << std::endl;BT::printTreeRecursively(tree.rootNode());std::cout << "------------------------------------" << std::endl;// 执行行为树tree.tickWhileRunning();} catch (const std::exception& e) {std::cerr << "Error: " << e.what() << std::endl;return 1;}return 0;
}

上述采用的是三种注册节点方式：

• registerNodeType
• registerSimpleCondition
• registerSimpleAction
   

特性维度	registerNodeType	registerSimpleAction	registerSimpleCondition
节点基类	需继承 BT::ActionNode 等	自动封装为 SimpleActionNode	自动封装为 SimpleConditionNode
返回值	自行实现 tick() 返回状态 SUCCESS / FAILURE / RUNNING	Lambda 返回 NodeStatus SUCCESS / FAILURE / RUNNING	Lambda 返回 bool SUCCESS / FAILURE
数据交互	通过黑板端口(强类型)	可选端口(需手动检查)	无端口(纯条件检查)
状态维护	支持(成员变量)	不支持(无状态)	不支持
典型用途	复杂动作/控制节点	简单一次性动作	条件判断(瞬时完成)
生命周期控制	可重写 halt() 等方法	无	无
线程安全	更易实现	需外部保证	需外部保证

1.4 my_tree.xml 实现

行为树实现逻辑如上，xml文件定义如下：

<root BTCPP_format="4" main_tree_to_execute="MainTree"><BehaviorTree ID="MainTree"><Sequence name="root_sequence"><CheckBattery name="battery_check"/><OpenCamera name="open_camera"/><OpenGripper name="open_gripper"/><ApproachObject name="approach_object"/><CloseGripper name="close_gripper"/><CloseCamera name="close_camera"/></Sequence></BehaviorTree>
</root>

注意
XML中使用的标识符必须与用于注册TreeNodes的标识符相一致，比如：xml文件定义的CheckBattery 要和 main文件中 factory.registerSimpleCondition("CheckBattery", [](BT::TreeNode&) { return CheckBattery(); });的CheckBattery 作对应。

2. 执行结果

------ Behavior Tree Structure ------
----------------
root_sequencebattery_checkopen_cameragripper_controlRetryUntilSuccessfulopen_gripperrecovery_sequenceclose_camera_on_failureForceFailurerecovery_successapproach_objectclose_gripperclose_camera
----------------
------------------------------------
[ Battery: OK ]
CameraInterface::open
GripperInterface::open successfully!
ApproachObject: approach_object
GripperInterface::close
CameraInterface::close

三、 执行失败处理

1. 添加尝试次数

1.1 功能实现

观察整个功能实现逻辑，如出现执行异常(如，夹爪初始化中打开夹爪失败)，无再次尝试的可能。故调整代码，在对打开夹爪进行多次打开尝试，如尝试3次，3次尝试都失败再返回错误。行为树逻辑及xml文件如下：

<root BTCPP_format="4" main_tree_to_execute="MainTree"><BehaviorTree ID="MainTree"><Sequence name="root_sequence"><CheckBattery name="battery_check"/><OpenCamera name="open_camera"/><RetryUntilSuccessful num_attempts="3"><OpenGripper name="open_gripper"/></RetryUntilSuccessful><ApproachObject name="approach_object"/><CloseGripper name="close_gripper"/><CloseCamera name="close_camera"/></Sequence></BehaviorTree>
</root>

为了实现打开次数的尝试，为打开夹爪添加变量计数：

// 夹爪打开
BT::NodeStatus GripperInterface::open() 
{if(_open_cnt < 2){_open_cnt ++;std::cout << "\033[1;31m"<< "GripperInterface::open failed!"<< "\033[0m" << std::endl;return BT::NodeStatus::FAILURE;}_open = true;_open_cnt = 0;std::cout << "\033[1;32m"<< "GripperInterface::open successfully!"<<"\033[0m" << std::endl;return BT::NodeStatus::SUCCESS;
}

1.2 实验结果

下图第一次测试，修改xml尝试 2次 打开夹爪失败后，直接返回失败；第二次测试，尝试 3次 打开夹爪并成功运行整个流程。

2. 完善异常处理

2.1 多节点组合兜底

观察尝试多次打开夹爪实现，发现若尝试打开夹爪多次依旧打开失败，直接躺平，没有进行关闭相机操作，实际场景这是不允许的！！此时就用到了其他节点的组合(此处，想一想该如何组合节点)：

上述行为树表示：
当3次尝试打开夹爪均返回失败，此时通过 Fallback节点，进行关闭相机操作。>

注意：
ForceFailure装饰器 无论相机是否关闭成功，即：
CloseCamera (SUCCESS) → ForceFailure(AlwaysSuccess) → 最终返回FAILURE CloseCamera (FAILURE) → ForceFailure(AlwaysSuccess) → 最终返回FAILURE
 
为什么要添加 ForceFailure节点，想象一下，若没有该节点，通过Fallback节点关闭相机成功，此时返回成功，会继续执行ApproachObject、CloseGripper等操作，这是不允许的。

<root BTCPP_format="4" main_tree_to_execute="MainTree"><BehaviorTree ID="MainTree"><Sequence name="root_sequence"><CheckBattery name="battery_check"/><OpenCamera name="open_camera"/><!-- 主控制流程 --><Fallback name="gripper_control"><!-- 尝试3次开夹爪 --><RetryUntilSuccessful num_attempts="3"><OpenGripper name="open_gripper"/></RetryUntilSuccessful><!-- 失败恢复流程 --><Sequence name="recovery_sequence"><CloseCamera name="close_camera_on_failure"/><!-- ForceFailure  装饰器, 强制覆盖子节点结果，始终返回FAILURE--><ForceFailure><AlwaysSuccess name="recovery_success"/></ForceFailure></Sequence></Fallback><ApproachObject name="approach_object"/><CloseGripper name="close_gripper"/><CloseCamera name="close_camera"/></Sequence></BehaviorTree>
</root>

2.2 实验结果

通过下图可以看出：尝试 2次 打开夹爪失败后，先关闭相机后 再直接返回失败；第二次测试，尝试 3次 打开夹爪并成功运行整个流程。