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

Gene_20222025-08-12 12:02

文章目录

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

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

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

学习时参考链接：ROS机器人行为树教程

一、背景需求

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

二、创建ActionNodes

1. 功能实现

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

1.1 头文件定义

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

  • CheckBattery()
  • GripperInterface::open()
  • GripperInterface::close()
  • CameraInterface::open()
  • CameraInterface::close()
cpp 复制代码 
#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 源文件实现

cpp 复制代码 
#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文件实现

cpp 复制代码 
#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文件定义如下：

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. 执行结果

bash 复制代码 
------ 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::open
GripperInterface::open successfully!
ApproachObject: approach_object
GripperInterface::close
CameraInterface::close

三、 执行失败处理

1. 添加尝试次数

1.1 功能实现

观察整个功能实现逻辑，如出现执行异常(如，夹爪初始化中打开夹爪失败)，无再次尝试的可能。故调整代码，在对打开夹爪进行多次打开尝试，如尝试3次，3次尝试都失败再返回错误。行为树逻辑及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"/>
      <RetryUntilSuccessful num_attempts="3">
            <OpenGripper name="open_gripper"/>
      </RetryUntilSuccessful>
      <ApproachObject name="approach_object"/>
      <CloseGripper name="close_gripper"/>
      <CloseCamera name="close_camera"/>
    </Sequence>
  </BehaviorTree>
</root>

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

cpp 复制代码 
// 夹爪打开
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等操作++，这是不允许的。

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"/>
      
      <!-- 主控制流程 -->
      <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次 打开夹爪并成功运行整个流程。

热门推荐
01全球最强模型Grok4，国内已可免费使用！（附教程）02UV安装并设置国内源03Qwen3-Coder 快速上手教程 | Qwen Code + Claude Code04[已解决]VSCode右键菜单消失恢复052025最新国内服务器可用docker源仓库地址大全（2025年8月更新）06GPT-5 使用限制与国内升级全攻略（免费 / Plus / Pro）【2025 最新】07🚀Cursor CLI+GPT-5保姆级教程+编程能力测评！Cursor CLI零成本免费使用GPT-5！Claude Code的劲敌来了！从安装到实战演示08KGG转MP3工具|非KGM文件|解密音频09Cursor 终端“卡死/无响应”问题的解法10OpenAI重返开源！GPT-OSS本地部署完全指南