网站导航
> 技术文档 > 使用行为树控制机器人(一) —— 节点_行为树 demo

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

网友: 技术文档 2025-09-03 21:55:43

文章目录

  • 一、背景需求
  • 二、创建ActionNodes
    • 1. 功能实现
      • 1.1 头文件定义
      • 1.2 源文件实现
      • 1.3 main文件实现
      • 1.4 my_tree.xml 实现
    • 2. 执行结果
  • 三、 执行失败处理
    • 1. 添加尝试次数
      • 1.1 功能实现
      • 1.2 实验结果
    • 2. 完善异常处理
      • 2.1 多节点组合兜底
      • 2.2 实验结果

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

近期在从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 // 同步动作节点 (无端口)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 // 同步动作节点 (无端口)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_sequence battery_check open_camera gripper_control RetryUntilSuccessful open_gripper recovery_sequence close_camera_on_failure ForceFailure recovery_success approach_object close_gripper close_camera----------------------------------------------------[ Battery: OK ]CameraInterface::openGripperInterface::open successfully!ApproachObject: approach_objectGripperInterface::closeCameraInterface::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\">  <RetryUntilSuccessful num_attempts=\"3\"> <OpenGripper name=\"open_gripper\"/> </RetryUntilSuccessful>  <Sequence name=\"recovery_sequence\"> <CloseCamera name=\"close_camera_on_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次 打开夹爪并成功运行整个流程。
在这里插入图片描述

网络标签：

相关问题