behavior tree

1 概述

安装.

sudo apt update
sudo apt install ros-$ROS_DISTRO-behaviortree-cpp -y

行为树类似于有限状态机, 都是用来控制任务流程的. 不同的是, 行为树(BT)是由一串分层节点组成. 每次执行循环, 会向根节点发一个 tick, tick 会一路传到叶子节点.

任何被 tick 的节点都会运行自己的回调, 并返回三个状态之一, SUCCESS/FAILURE/RUNNING. 叶子节点是真正执行动作或做判断的地方, 最常见的叶子是 Action 节点.

1.1 节点的类型

• ControlNode(控制节点): 有 1..N 个孩子, 按自己的规则把 tick 分发给孩子.

• ecoratorNode(装饰器节点)：只能有 1 个孩子, 可以改变孩子的结果、重复执行孩子、或在条件不满足时阻止孩子被 tick.

• ConditionNode(条件)节点：不能返回 RUNNING, 只做判断，不改变系统状态.

• ActionNode(动作节点)：真正“干活”, 可以是同步(一次 tick 就返回 S/F)或异步(可能返回 RUNNING，需要后续再 tick).

1.2 执行策略

• Sequence: 像逻辑 AND. 从左到右依次执行子节点, 遇到第一个不是 SUCCESS 的结果就立刻停.

• Decorator: 只包一名子节点, 在其外层加一层”策略”, 可改变子节点的执行时机或返回值, 如反转、重试、限制频率、超时等.

• Fallback: 像逻辑 OR. 从左到右尝试策略, 一旦某个子节点不是 FAILURE 就停.

1.3 Blackboard

整棵树/子树共享的键值存储(作用域可分层), 用来在节点之间传数据.

1.4 Ports

每个节点在类型层面声明的输入/输出”接口”(如 InputPort<int>(“speed”)), 运行时通过 XML 里的”花括号”变量把端口和黑板里的键绑定/映射起来.

2 XML 结构

最外层是一个 root 标签, BTCPP_format 声明版本信息.

内层嵌套 BehaviorTree 标签, 必须有 ID 属性(这是”这棵树的名字”).

节点用单个 XML 标签表示, 子节点根据 1.1 进行约束.

下面给出示例, 同一棵行为树的不同写法.

<root BTCPP_format="4">
    <BehaviorTree ID="MainTree">
    <Sequence name="root_sequence">
        <SaySomething   name="action_hello" message="Hello"/>
        <OpenGripper    name="open_gripper"/>
        <ApproachObject name="approach_object"/>
        <CloseGripper   name="close_gripper"/>
    </Sequence>
    </BehaviorTree>
</root>

<root BTCPP_format="4">
  <TreeNodeModel>
    <Action ID="SaySomething">
      <input_port name="message" type="std::string"/>
    </Action>
    <Action ID="OpenGripper"/>
    <Action ID="ApproachObject"/>
    <Action ID="CloseGripper"/>
  </TreeNodeModel>

  <BehaviorTree ID="MainTree">
    <Sequence name="root_sequence">
      <SaySomething message="Hello"/>
      <OpenGripper/>
      <ApproachObject/>
      <CloseGripper/>
    </Sequence>
  </BehaviorTree>
</root>

2.1 子树

我们可以把一段流程单独描述成一棵树(例如 GraspObject).

在主树里用 <SubTree ID=”GraspObject”/> 直接调用.

<root BTCPP_format="4">
  <BehaviorTree ID="MainTree">
    <Sequence>
      <Action ID="SaySomething" message="Hello World"/>
      <SubTree ID="GraspObject"/>
    </Sequence>
  </BehaviorTree>

  <BehaviorTree ID="GraspObject">
    <Sequence>
      <Action ID="OpenGripper"/>
      <Action ID="ApproachObject"/>
      <Action ID="CloseGripper"/>
    </Sequence>
  </BehaviorTree>
</root>

2.2 拆分文件

我们可以把多棵树拆到不同文件，用 <include path=”…”/> 引入.

ROS 环境下也可用 ros_pkg=”…” path=”相对路径” 来从包里找文件.

<!-- maintree.xml -->
<root BTCPP_format="4">
  <include path="grasp.xml"/>
  <!-- ROS 变体: \<include ros_pkg="my_pkg" path="bt/grasp.xml"/> -->
  <BehaviorTree ID="MainTree">
    <Sequence>
      <Action ID="SaySomething" message="Hello World"/>
      <SubTree ID="GraspObject"/>
    </Sequence>
  </BehaviorTree>
</root>

<!-- grasp.xml -->
<root BTCPP_format="4">
  <BehaviorTree ID="GraspObject">
    <Sequence>
      <Action ID="OpenGripper"/>
      <Action ID="ApproachObject"/>
      <Action ID="CloseGripper"/>
    </Sequence>
  </BehaviorTree>
</root>

3 代码示例

3.1 运行最小 DEMO

创建 ros2 功能包.

ros2 pkg create --build-type ament_cmake bt_first_tree_demo

.
└── src
    └── bt_first_tree_demo
        ├── CMakeLists.txt
        ├── include
        │   └── bt_first_tree_demo
        ├── my_tree.xml
        ├── package.xml
        └── src
            └── main.cpp

编写 CMakeLists.txt

find_package(ament_cmake REQUIRED)
find_package(behaviortree_cpp REQUIRED)
find_package(ament_index_cpp REQUIRED)
# ...
add_executable(bt_demo src/main.cpp)
ament_target_dependencies(bt_demo behaviortree_cpp ament_index_cpp)

install(TARGETS bt_demo
  DESTINATION lib/${PROJECT_NAME}
)

install(FILES my_tree.xml
  DESTINATION share/${PROJECT_NAME}
)

编写 my_tree.xml

<root BTCPP_format="4">
  <BehaviorTree ID="MainTree">
    <Sequence name="root_sequence">
      <CheckBattery/>
      <OpenGripper/>
      <ApproachObject/>
      <CloseGripper/>
    </Sequence>
  </BehaviorTree>
</root>

加入依赖.

<depend>behaviortree_cpp</depend>
<depend>ament_index_cpp</depend>

编写 main.cpp

#include <behaviortree_cpp/bt_factory.h>
#include <behaviortree_cpp/action_node.h>
#include <ament_index_cpp/get_package_share_directory.hpp>
#include <filesystem>
#include <iostream>

using namespace BT;

class ApproachObject : public SyncActionNode {
public:
  ApproachObject(const std::string& name, const NodeConfig& config)
  : SyncActionNode(name, config) {}

  static PortsList providedPorts() { return {}; }

  NodeStatus tick() override {
    std::cout << "[ApproachObject] approaching target...\n";
    return NodeStatus::SUCCESS;
  }
};

class GripperInterface {
public:
  NodeStatus open()  { std::cout << "[Gripper] open\n";  return NodeStatus::SUCCESS; }
  NodeStatus close() { std::cout << "[Gripper] close\n"; return NodeStatus::SUCCESS; }
};

NodeStatus CheckBattery(TreeNode&) {
  std::cout << "[Battery] OK\n";
  return NodeStatus::SUCCESS;
}

int main(int, char**) {
  BehaviorTreeFactory factory;

  factory.registerNodeType<ApproachObject>("ApproachObject");

  GripperInterface gripper;
  factory.registerSimpleAction("OpenGripper",
      [&gripper](TreeNode&) { return gripper.open(); });
  factory.registerSimpleAction("CloseGripper",
      [&gripper](TreeNode&) { return gripper.close(); });
  factory.registerSimpleCondition("CheckBattery", CheckBattery);

  const std::string share_dir = ament_index_cpp::get_package_share_directory("bt_first_tree_demo");
  const std::filesystem::path xml_path = std::filesystem::path(share_dir) / "my_tree.xml";
  std::cout << "[BT] loading tree: " << xml_path << std::endl;

  try {
    auto tree = factory.createTreeFromFile(xml_path.string());

    NodeStatus status = NodeStatus::RUNNING;
    while (status == NodeStatus::RUNNING) {
      status = tree.tickOnce();
    }
    std::cout << "[Tree] finished with status: " << BT::toStr(status) << std::endl;
    return status == NodeStatus::SUCCESS ? 0 : 1;
  } catch (const std::exception& e) {
    std::cerr << "[BT] failed: " << e.what() << std::endl;
    return 2;
  }
}

编译运行.

colcon build
source install/setup.bash
ros2 run bt_first_tree_demo bt_demo

4 blackboard 和 ports

4.1 基础概念

总体而言, blackboard 用来存储数据, 而 ports 分为 Inputport 和 Outport 负责数据流的传递.

4.1.1 Inputports

两种写法.

• 字面量: 把常量字符串直接写进端口属性.

• 黑板引用: 用花括号写成 {key}，表示”去黑板里拿名为 key 的值”

<SaySomething name="first"  message="hello world"/>
<SaySomething name="second" message="{greetings}"/>

第一行把字面量 “hello world” 传给端口；第二行让节点到黑板的 greetings 条目里取值.

4.1.2 Outputports

我们可以通过 Script 来将常量写进黑板.

<Script code=" the_answer:='The answer is 42' "/>

当然, 我们也可以声明一个输出端口 text，在 tick() 中用 setOutput(“text”, “The answer is 42”) 写入黑板。这样，别的节点就能通过输入端口读取到这个值.

例如这里, 我们可以在 cpp 代码中调用上面的 setOutput(), 这里 text 是输出端口, 之后我们就给 ans 赋值了.

<Writer  text="{ans}"/>

4.2 demo

按照上面的结构, 修改 xml 和 main.cpp.

<root BTCPP_format="4">
  <BehaviorTree ID="Main">
    <Sequence>
      <!-- Writer 的输出端口 text 绑定到黑板键 {ans} -->
      <Writer  text="{ans}"/>

      <!-- Speaker 的输入端口 message 从黑板键 {ans} 读取 -->
      <Speaker message="{ans}"/>

      <!-- 也可直接传字面量（不走黑板、无花括号） -->
      <Speaker message="I am a constant."/>
    </Sequence>
  </BehaviorTree>
</root>

#include <behaviortree_cpp/bt_factory.h>
#include <behaviortree_cpp/action_node.h>
#include <ament_index_cpp/get_package_share_directory.hpp>
#include <filesystem>
#include <iostream>

int main() {
  BT::BehaviorTreeFactory factory;

  // Writer：只有输出端口 text，把字符串写入黑板
  factory.registerSimpleAction(
    "Writer",
    [](BT::TreeNode& self){
      std::string value = "The answer is 42";
      self.setOutput("text", value); // 写输出端口 -> 绑定的黑板键
      return BT::NodeStatus::SUCCESS;
    },
    BT::PortsList{ BT::OutputPort<std::string>("text") }
  );

  // Speaker：只有输入端口 message，从黑板读取后打印
  factory.registerSimpleAction(
    "Speaker",
    [](BT::TreeNode& self){
      auto msg = self.getInput<std::string>("message"); // 读输入端口 -> 来自黑板键
      if (!msg) throw BT::RuntimeError(msg.error());
      std::cout << "[Speaker] " << msg.value() << "\n";
      return BT::NodeStatus::SUCCESS;
    },
    BT::PortsList{ BT::InputPort<std::string>("message") }
  );

  const std::string share = ament_index_cpp::get_package_share_directory("bt_first_tree_demo");
  const std::filesystem::path xml = std::filesystem::path(share) / "my_tree.xml";
  std::cout << "[BT] loading: " << xml << std::endl;

  auto tree = factory.createTreeFromFile(xml.string());
  auto status = BT::NodeStatus::RUNNING;
  while (status == BT::NodeStatus::RUNNING) {
    status = tree.tickOnce();
  }
  std::cout << "[BT] finished: " << BT::toStr(status) << std::endl;
  return status == BT::NodeStatus::SUCCESS ? 0 : 1;
}

5 泛型端口

端口不仅能用字符串, 还能用任意 C++ 类型(包含你自定义的结构体), 并且能把 XML 里的字符串自动转换成这些类型.

5.1 强类型的写法

namespace BT {
template <> inline MyType1 convertFromString(BT::StringView str) {
    // 解析逻辑
    MyType1 out;
    // ...
    return out;
}

template <> inline MyType2 convertFromString(BT::StringView str) {
    // 解析逻辑
    MyType2 out;
    // ...
    return out;
}
}

5.2 demo

main.cpp

#include <behaviortree_cpp/bt_factory.h>
#include <behaviortree_cpp/action_node.h>
#include <ament_index_cpp/get_package_share_directory.hpp>
#include <iostream>
#include <string>
#include <chrono>

// ---- 强类型：速度指令 ----
struct VelocityCmd {
    double v;
    double w;
};

// 字符串 -> 强类型
namespace BT {
template <> inline VelocityCmd convertFromString(BT::StringView str) {
  auto parts = BT::splitString(str, ';');
  if (parts.size() != 2) throw BT::RuntimeError("Invalid VelocityCmd: ", str);
  VelocityCmd out;
  out.v = BT::convertFromString<double>(parts[0]);
  out.w = BT::convertFromString<double>(parts[1]);
  return out;
}
} // namespace BT

int main() {
  BT::BehaviorTreeFactory factory;

  // Planner：输出强类型端口 cmd
  factory.registerSimpleAction(
    "PlanVelocity",
    [](BT::TreeNode& self){
      VelocityCmd c{0.20, 0.60};
      self.setOutput("cmd", c);
      std::cout << "[PlanVelocity] v=" << c.v << " w=" << c.w << "\n";
      return BT::NodeStatus::SUCCESS;
    },
    BT::PortsList{ BT::OutputPort<VelocityCmd>("cmd") }
  );

  // Printer：输入强类型端口 cmd
  factory.registerSimpleAction(
    "PrintVelocity",
    [](BT::TreeNode& self){
      auto in = self.getInput<VelocityCmd>("cmd");
      if (!in) throw BT::RuntimeError(in.error());
      const auto c = in.value();
      std::cout << "[PrintVelocity] v=" << c.v << " w=" << c.w << "\n";
      return BT::NodeStatus::SUCCESS;
    },
    BT::PortsList{ BT::InputPort<VelocityCmd>("cmd") }
  );

  const std::string share = ament_index_cpp::get_package_share_directory("bt_first_tree_demo");
  const std::string xml = share + "/my_tree.xml";
  std::cout << "[BT] loading: " << xml << std::endl;

  auto tree = factory.createTreeFromFile(xml);

  using namespace std::chrono_literals;

  auto status = BT::NodeStatus::RUNNING;
  while (status == BT::NodeStatus::RUNNING) {
    status = tree.tickOnce();
  }


  std::cout << "[BT] finished: " << BT::toStr(status) << std::endl;
  return status == BT::NodeStatus::SUCCESS ? 0 : 1;
}

xml.

<root BTCPP_format="4">
  <BehaviorTree ID="MainTree">
    <Sequence>
      <PlanVelocity cmd="{PlanCmd}"/>
      <PrintVelocity cmd="{PlanCmd}"/>

      <!-- 再写一个强类型到黑板，字符串会自动解析为 VelocityCmd -->
      <Script code=" OtherCmd:='0.50;0.00' "/>
      <PrintVelocity cmd="{OtherCmd}"/>
    </Sequence>
  </BehaviorTree>
</root>

6 反应式行为

反应式行为指的是在行为树里, 一边执行动作, 一边持续复查前置条件. 一旦前置条件变了, 就立刻打断正在进行的动作并改走别的分支.

<root BTCPP_format="4">
  <BehaviorTree ID="Main">
    <ReactiveSequence>
      <A/>
      <B/>
      <C/>
  </ReactiveSequence>
  </BehaviorTree>
</root>

比如以上, 党执行完 A 并执行到 B 的时候, 会从头开始检查 A 的条件是否改变.

当执行到 C 的时候, 会检查 A, B 的条件是否改变.

6.2 demo

main.cpp

#include <behaviortree_cpp/bt_factory.h>
#include <behaviortree_cpp/action_node.h>
#include <ament_index_cpp/get_package_share_directory.hpp>
#include <filesystem>
#include <iostream>
#include <chrono>
#include <thread>

using namespace std::chrono;

// -------- 异步动作：SlowWork(1.5s 完成，可被 halt 中断) --------
class SlowWork : public BT::StatefulActionNode {
public:
  SlowWork(const std::string& name, const BT::NodeConfig& cfg)
  : BT::StatefulActionNode(name, cfg) {}
  static BT::PortsList providedPorts() { return {}; }

  BT::NodeStatus onStart() override {
    std::cout << "[SlowWork] start\n";
    done_at_ = steady_clock::now() + 1500ms;
    return BT::NodeStatus::RUNNING;
  }
  BT::NodeStatus onRunning() override {
    std::this_thread::sleep_for(100ms);
    if (steady_clock::now() >= done_at_) {
      std::cout << "[SlowWork] finished\n";
      return BT::NodeStatus::SUCCESS;
    }
    return BT::NodeStatus::RUNNING;
  }
  void onHalted() override {
    std::cout << "[SlowWork] ABORTED\n";
  }
private:
    steady_clock::time_point done_at_{};
};

int main() {
  BT::BehaviorTreeFactory factory;

  // -------- 条件：SafetyOK，启动 600ms 内返回 SUCCESS，之后一直 FAIL --------
  factory.registerSimpleCondition("SafetyOK", [](BT::TreeNode&){
    static const auto t0 = std::chrono::steady_clock::now();
    auto ms = duration_cast<milliseconds>(steady_clock::now() - t0).count();
    bool ok = ms < 600; // 0~599ms OK，>=600ms FAIL
    std::cout << "[SafetyOK] " << (ok ? "OK" : "FAIL")
              << " at " << ms << "ms\n";
    return ok ? BT::NodeStatus::SUCCESS : BT::NodeStatus::FAILURE;
  });

  factory.registerNodeType<SlowWork>("SlowWork");

  const std::string share = ament_index_cpp::get_package_share_directory("bt_first_tree_demo");
  const std::filesystem::path xml = std::filesystem::path(share) / "my_tree.xml";
  std::cout << "[BT] loading: " << xml << std::endl;

  auto tree = factory.createTreeFromFile(xml.string());

  auto status = BT::NodeStatus::RUNNING;
  while (status == BT::NodeStatus::RUNNING) {
    status = tree.tickOnce();
    if (status == BT::NodeStatus::RUNNING) {
      tree.sleep(std::chrono::milliseconds(100));
    }
  }
  std::cout << "[BT] finished: " << BT::toStr(status) << std::endl;
  return status == BT::NodeStatus::SUCCESS ? 0 : 1;
}

普通版本.

<root BTCPP_format="4">
  <BehaviorTree ID="Main">
    <Sequence>
      <SafetyOK/>
      <SlowWork/>
    </Sequence>
  </BehaviorTree>
</root>

运行结果为.

...
[SafetyOK] OK at 0ms
[SlowWork] start
[SlowWork] finished
[BT] finished: SUCCESS

反应式版本.

<root BTCPP_format="4">
  <BehaviorTree ID="Main">
    <ReactiveSequence>
      <SafetyOK/>
      <SlowWork/>
    </ReactiveSequence>
  </BehaviorTree>
</root>

运行结果为.

...
[SafetyOK] OK at 0ms
[SlowWork] start
[SafetyOK] OK at 100ms
[SafetyOK] OK at 300ms
[SafetyOK] OK at 501ms
[SafetyOK] FAIL at 702ms
[SlowWork] ABORTED
[BT] finished: FAILURE

7 子树的使用

子树在 C++ 代码中我们不需要做什么修改, xml 文件会直接解析. 直接看例子.

xml.

<root BTCPP_format="4">

    <BehaviorTree ID="MainTree">
        <Sequence>
            <Fallback>
                <Inverter>
                    <IsDoorClosed/>
                </Inverter>
                <SubTree ID="DoorClosed"/>
            </Fallback>
            <PassThroughDoor/>
        </Sequence>
    </BehaviorTree>

    <BehaviorTree ID="DoorClosed">
        <Fallback>
            <OpenDoor/>
            <RetryUntilSuccessful num_attempts="5">
                <PickLock/>
            </RetryUntilSuccessful>
            <SmashDoor/>
        </Fallback>
    </BehaviorTree>
    
</root>

main.cpp

#include <behaviortree_cpp/bt_factory.h>
#include <ament_index_cpp/get_package_share_directory.hpp>
#include <filesystem>
#include <iostream>
#include <string>

class CrossDoor
{
public:
    void registerNodes(BT::BehaviorTreeFactory& factory);

    BT::NodeStatus isDoorClosed();
    BT::NodeStatus passThroughDoor();
    BT::NodeStatus pickLock();
    BT::NodeStatus openDoor();
    BT::NodeStatus smashDoor();

private:
    bool _door_open    = false;
    bool _door_locked  = true;
    int  _pick_attempts = 0;
};

void CrossDoor::registerNodes(BT::BehaviorTreeFactory &factory)
{
    factory.registerSimpleCondition("IsDoorClosed",    std::bind(&CrossDoor::isDoorClosed, this));
    factory.registerSimpleAction   ("PassThroughDoor", std::bind(&CrossDoor::passThroughDoor, this));
    factory.registerSimpleAction   ("OpenDoor",        std::bind(&CrossDoor::openDoor, this));
    factory.registerSimpleAction   ("PickLock",        std::bind(&CrossDoor::pickLock, this));
    factory.registerSimpleCondition("SmashDoor",       std::bind(&CrossDoor::smashDoor, this));
}

BT::NodeStatus CrossDoor::isDoorClosed()
{
    std::cout << "[IsDoorClosed] door_open=" << std::boolalpha << _door_open << "\n";
    return (_door_open == false) ? BT::NodeStatus::SUCCESS : BT::NodeStatus::FAILURE;
}

BT::NodeStatus CrossDoor::passThroughDoor()
{
    std::cout << "[PassThroughDoor] trying to pass... ";
    if (_door_open)
    {
        std::cout << "success.\n";
        return BT::NodeStatus::SUCCESS;
    }
    std::cout << "failed (door closed).\n";
    return BT::NodeStatus::FAILURE;
}

BT::NodeStatus CrossDoor::pickLock()
{
    _pick_attempts++;
    std::cout << "[PickLock] attempt " << _pick_attempts << " (locked=" << std::boolalpha << _door_locked << ")\n";
    if (_door_locked && _pick_attempts < 3)
    {
        return BT::NodeStatus::FAILURE;
    }
    _door_locked = false;
    _door_open   = true;
    std::cout << "[PickLock] lock picked. Door is now OPEN.\n";
    return BT::NodeStatus::SUCCESS;
}

BT::NodeStatus CrossDoor::openDoor()
{
    std::cout << "[OpenDoor] trying to open... (locked=" << std::boolalpha << _door_locked << ")\n";
    if (_door_locked)
    {
        std::cout << "[OpenDoor] failed: door is locked.\n";
        return BT::NodeStatus::FAILURE;
    }
    _door_open = true;
    std::cout << "[OpenDoor] success. Door is OPEN.\n";
    return BT::NodeStatus::SUCCESS;
}

BT::NodeStatus CrossDoor::smashDoor()
{
    std::cout << "[SmashDoor] smashing the door! (this always opens it)\n";
    _door_locked = false;
    _door_open   = true;
    return BT::NodeStatus::SUCCESS;
}

int main(int argc, char** argv)
{
    (void)argc; (void)argv;

    BT::BehaviorTreeFactory factory;
    CrossDoor cross_door;
    cross_door.registerNodes(factory);

    std::string pkg_share_dir = ament_index_cpp::get_package_share_directory("bt_first_tree_demo");
    std::filesystem::path xml_file = std::filesystem::path(pkg_share_dir) / "my_tree.xml";

    factory.registerBehaviorTreeFromFile(xml_file.string());
    auto tree = factory.createTree("MainTree");

    BT::printTreeRecursively(tree.rootNode());

    BT::NodeStatus status = BT::NodeStatus::IDLE;
    do {
        status = tree.tickOnce();
    } while (status == BT::NodeStatus::RUNNING);

    std::cout << "Tree finished with status: "
              << (status == BT::NodeStatus::SUCCESS ? "SUCCESS" : "FAILURE") << "\n";
    return 0;
}

8 端口重映射

在大型树里避免命名冲突: 每个(子)树都会有自己的 Blackboard 实例, 互不相同. 为了让父树里的数据（端口/Blackboard键）传进子树, 或把子树的结果带回父树, 就需要显式做端口重映射. 这件事完全在 XML 里完成.

<root BTCPP_format="4">

  <BehaviorTree ID="MainTree">
    <Sequence>
      <Script code=" move_goal='1;2;3' " />
      <SubTree ID="MoveRobot" target="{move_goal}" result="{move_result}" />
      <SaySomething message="{move_result}"/>
    </Sequence>
  </BehaviorTree>

  <BehaviorTree ID="MoveRobot">
    <Fallback>
      <Sequence>
        <MoveBase goal="{target}"/>
        <Script code=" result:='goal reached' " />
      </Sequence>
      <ForceFailure>
        <Script code=" result:='error' " />
      </ForceFailure>
    </Fallback>
  </BehaviorTree>

</root>

父树 MainTree 里, 先用 Script 把 Blackboard 键 move_goal 设为 ‘1;2;3’.

调用子树 MoveRobot 时, 通过 <SubTree … target=”{move_goal}” result=”{move_result}”/> 做端口重映射:

• 把子树内部使用的 {target} 映射到父树的 {move_goal}. 这样子树里 <MoveBase goal=”{target}”/> 实际就拿到了父树的目标.

• 把子树内部使用的 {result} 映射到父树的 {move_result}. 这样子树里设置 result:=’goal reached’ 或 ‘error’ 后, 父树就能用 <SaySomething message=”{move_result}”/> 说出来.

main.cpp

#include <behaviortree_cpp/bt_factory.h>
#include <behaviortree_cpp/action_node.h>
#include <ament_index_cpp/get_package_share_directory.hpp>
#include <filesystem>
#include <iostream>
#include <string>

struct Pose2D
{
    double x = 0, y = 0, theta = 0;
};

namespace BT
{
template <> inline Pose2D convertFromString(StringView str)
{
    auto parts = splitString(str, ';');
    if (parts.size() != 3)
    {
        throw RuntimeError("invalid Pose2D format. Expected: x;y;theta  Got: ", str);
    }
    Pose2D out;
    out.x     = convertFromString<double>(parts[0]);
    out.y     = convertFromString<double>(parts[1]);
    out.theta = convertFromString<double>(parts[2]);
    return out;
}
} // namespace BT

using namespace BT;

class MoveBase : public SyncActionNode
{
public:
    MoveBase(const std::string& name, const NodeConfig& cfg) : SyncActionNode(name, cfg) {}

    static PortsList providedPorts()
    {
        return { InputPort<Pose2D>("goal") };
    }

    NodeStatus tick() override
    {
        auto goal = getInput<Pose2D>("goal");
        if (!goal)
        {
            throw RuntimeError("MoveBase: missing required input [goal]: ", goal.error());
        }
        std::cout << "[MoveBase] Go to (" << goal->x << ", " << goal->y << ", " << goal->theta << ")\n";
        return NodeStatus::SUCCESS;
    }
};

class SaySomething : public SyncActionNode
{
public:
    SaySomething(const std::string& name, const NodeConfig& cfg) : SyncActionNode(name, cfg) {}

    static PortsList providedPorts()
    {
        return { InputPort<std::string>("message") };
    }

    NodeStatus tick() override
    {
        auto msg = getInput<std::string>("message");
        if (!msg)
        {
            throw RuntimeError("SaySomething: missing required input [message]: ", msg.error());
        }
        std::cout << "[SaySomething] " << msg.value() << "\n";
        return NodeStatus::SUCCESS;
    }
};

int main(int argc, char** argv)
{
    std::string package_name = "bt_first_tree_demo";
    std::string pkg_share_dir = ament_index_cpp::get_package_share_directory(package_name);
    std::filesystem::path xml_path = std::filesystem::path(pkg_share_dir) / "my_tree.xml";

    BehaviorTreeFactory factory;

    factory.registerNodeType<MoveBase>("MoveBase");
    factory.registerNodeType<SaySomething>("SaySomething");

    factory.registerBehaviorTreeFromFile(xml_path.string());
    auto tree = factory.createTree("MainTree");

    BT::printTreeRecursively(tree.rootNode());

    // Tick
    BT::NodeStatus status = BT::NodeStatus::IDLE;
    do {
        status = tree.tickOnce();
    } while (status == BT::NodeStatus::RUNNING);

    try {
        auto res = tree.rootBlackboard()->get<std::string>("move_result");
        std::cout << "[Main] move_result = " << res << "\n";
    } catch (const std::exception& e) {
        std::cout << "[Main] no move_result: " << e.what() << "\n";
    }

    return 0;
}

9 多个 XML 文件的使用

这里有两种方式.

• 在 C++ 里手动把多个 XML 注册进 factory, 然后按 ID 创建想要的树.
  behaviortree.dev

XML.

<root>
  <BehaviorTree ID="MainTree">
    <Sequence>
      <SaySomething message="starting MainTree"/>
      <SubTree ID="SubTreeA"/>
      <SubTree ID="SubTreeB"/>
    </Sequence>
  </BehaviorTree>
</root>

<root>
  <BehaviorTree ID="SubTreeA">
    <SaySomething message="Executing Sub_A"/>
  </BehaviorTree>
</root>

<root>
  <BehaviorTree ID="SubTreeB">
    <SaySomething message="Executing Sub_B"/>
  </BehaviorTree>
</root>

main.cpp.

#include <behaviortree_cpp/bt_factory.h>
#include <behaviortree_cpp/action_node.h>
#include <ament_index_cpp/get_package_share_directory.hpp>
#include <filesystem>
#include <iostream>

using namespace BT;

class SaySomething : public SyncActionNode {
public:
  SaySomething(const std::string& name, const NodeConfig& config)
    : SyncActionNode(name, config) {}

  static PortsList providedPorts() {
    return { InputPort<std::string>("message") };
  }

  NodeStatus tick() override {
    const auto msg = getInput<std::string>("message").value_or("");
    std::cout << "Robot says: " << msg << std::endl;
    return NodeStatus::SUCCESS;
  }
};

int main(int argc, char** argv)
{
  BT::BehaviorTreeFactory factory;
  factory.registerNodeType<SaySomething>("SaySomething");

  const std::string share =
      ament_index_cpp::get_package_share_directory("bt_first_xml_demo");
  const std::string trees_dir = share;

  using std::filesystem::directory_iterator;
  for (const auto& entry : directory_iterator(trees_dir)) {
    if (entry.path().extension() == ".xml") {
      factory.registerBehaviorTreeFromFile(entry.path().string());
    }
  }

  std::cout << "----- MainTree tick ----" << std::endl;
  auto main_tree = factory.createTree("MainTree");
  main_tree.tickWhileRunning();

  std::cout << "----- SubTreeA tick ----" << std::endl;
  auto subA = factory.createTree("SubTreeA");
  subA.tickWhileRunning();

  std::cout << "----- SubTreeB tick ----" << std::endl;
  auto subB = factory.createTree("SubTreeB");
  subB.tickWhileRunning();

  return 0;
}

• 在主 XML 里用 <include path=”…”/> 把其它 XML 包进来, 路径相对主 XML 文件，然后直接从这个主 XML 创建树.

XML.

<root BTCPP_format="4">
  <include path="./subtree_A.xml"/>
  <include path="./subtree_B.xml"/>

  <BehaviorTree ID="MainTree">
    <Sequence>
      <SaySomething message="starting MainTree"/>
      <SubTree ID="SubTreeA"/>
      <SubTree ID="SubTreeB"/>
    </Sequence>
  </BehaviorTree>
</root>

main.cpp.

#include <behaviortree_cpp/bt_factory.h>
#include <behaviortree_cpp/action_node.h>
#include <ament_index_cpp/get_package_share_directory.hpp>
#include <filesystem>
#include <iostream>

using namespace BT;

class SaySomething : public SyncActionNode {
public:
  SaySomething(const std::string& name, const NodeConfig& config)
    : SyncActionNode(name, config) {}

  static PortsList providedPorts() {
    return { InputPort<std::string>("message") };
  }

  NodeStatus tick() override {
    const auto msg = getInput<std::string>("message").value_or("");
    std::cout << "Robot says: " << msg << std::endl;
    return NodeStatus::SUCCESS;
  }
};

int main(int argc, char** argv)
{
  BT::BehaviorTreeFactory factory;
  factory.registerNodeType<SaySomething>("SaySomething");

  const std::string share =
      ament_index_cpp::get_package_share_directory("bt_first_xml_demo");
  const std::string trees_dir = share;

  using std::filesystem::directory_iterator;
  for (const auto& entry : directory_iterator(trees_dir)) {
    if (entry.path().extension() == ".xml") {
      factory.registerBehaviorTreeFromFile(entry.path().string());
    }
  }

  auto tree = factory.createTreeFromFile(trees_dir + "/my_tree.xml");
  tree.tickWhileRunning();

  return 0;
}

9 额外传参

当一些参数在部署期就确定、且运行时不会频繁变(比如句柄、阈值、指针/引用、服务客户端、日志器、设备句柄等), 这时用端口反而累赘.

9.1 构造函数时传参

自定义节点把额外参数写进构造函数签名(在 name, config 之后).

注册时用 factory.registerNodeType<T>(“ID”, extra_args…) 把这些参数传进去。

XML 里照常 <T/>，XML 不负责这些额外参数。

using namespace BT;

class NoCopyObj {
public:
  explicit NoCopyObj(int v): _v(v) {}
  NoCopyObj(const NoCopyObj&) = delete;
  int value() const { return _v; }
private:
  int _v{0};
};

class Action_A : public SyncActionNode {
public:
  // point
  Action_A(const std::string& name, const NodeConfig& cfg,
           int arg_int, std::string arg_str, NoCopyObj& ref_obj)
  : SyncActionNode(name, cfg), a_(arg_int), s_(std::move(arg_str)), ref_(ref_obj) {}

  static PortsList providedPorts() { return {}; }
  NodeStatus tick() override {
    std::cout << name() << ": " << a_ << " / " << s_ << " / " << ref_.value() << "\n";
    return NodeStatus::SUCCESS;
  }
private:
  int a_;
  std::string s_;
  NoCopyObj& ref_;
};

int main() {
  BehaviorTreeFactory factory;
  NoCopyObj nc(88);

  // point
  factory.registerNodeType<Action_A>("Action_A", 42, std::string("hello"), std::ref(nc));

  auto tree = factory.createTreeFromText(R"(
    <root BTCPP_format="4"><BehaviorTree><Action_A/></BehaviorTree></root>
  )");
  tree.tickWhileRunning();
}

9.2 用 initialize 初始化

在节点类里提供 initialize(args…).

树创建后(第一次 tick 前)，对树做一次 applyVisitor，找到该类型节点并调用 initialize(…).

class Action_B : public SyncActionNode {
public:
  Action_B(const std::string& name, const NodeConfig& cfg) : SyncActionNode(name, cfg) {}
  void initialize(int arg_int, std::string arg_str) { a_=arg_int; s_=std::move(arg_str); }
  static PortsList providedPorts(){ return {}; }
  NodeStatus tick() override { std::cout << a_ << " / " << s_ << "\n"; return NodeStatus::SUCCESS; }
private: int a_{}; std::string s_;
};

BehaviorTreeFactory factory;
factory.registerNodeType<Action_B>("Action_B");
auto tree = factory.createTreeFromText(R"(
  <root BTCPP_format="4"><BehaviorTree><Action_B/></BehaviorTree></root>
)");

// init
tree.applyVisitor([](TreeNode* node){
  if(auto b = dynamic_cast<Action_B*>(node)){ b->initialize(69, "init_val"); }
});
tree.tickWhileRunning();

10 连接 groot2

groot2 是一个行为树的可视化观察工具.

第一步创建 xml 文件.

const std::string models_xml = BT::writeTreeNodesModelXML(factory);
const std::string models_dir = share_dir + "/models";
fs::create_directories(models_dir);
const std::string models_file = models_dir + "/models.xml";
std::ofstream(models_file) << models_xml;

第二步, 发布节点.

BT::Groot2Publisher publisher(tree);

之后接入 groot2 就可以运行了.

以下为示例.

XML.

<root BTCPP_format="4">

  <!-- 主树 -->
  <BehaviorTree ID="Main">
    <Sequence>
      <!-- 如果门是关的，就去开门；否则直接通过 -->
      <Fallback>
        <Inverter>
          <IsDoorClosed/>
        </Inverter>
        <SubTree ID="DoorClosed"/>
      </Fallback>

      <PassThroughDoor/>
    </Sequence>
  </BehaviorTree>

  <!-- 子树：处理门关闭的情况 -->
  <BehaviorTree ID="DoorClosed">
    <Fallback>
      <!-- 先尝试直接开门 -->
      <OpenDoor/>
      <!-- 再尝试多次撬锁 -->
      <RetryUntilSuccessful num_attempts="5">
        <PickLock/>
      </RetryUntilSuccessful>
      <!-- 最后砸门 -->
      <SmashDoor/>
    </Fallback>
  </BehaviorTree>

</root>

main.cpp

#include <behaviortree_cpp/bt_factory.h>
#include <behaviortree_cpp/loggers/groot2_publisher.h>
#include <behaviortree_cpp/xml_parsing.h>
#include <ament_index_cpp/get_package_share_directory.hpp>

#include <filesystem>
#include <fstream>
#include <iostream>
#include <thread>
#include <chrono>

namespace fs = std::filesystem;
using namespace std::chrono_literals;

class CrossDoor {
public:
    void registerNodes(BT::BehaviorTreeFactory& factory)
    {
    using BT::NodeStatus;
    factory.registerSimpleCondition("IsDoorClosed",
        [this](BT::TreeNode&) -> NodeStatus {
        std::cout << "[IsDoorClosed] door_open=" << std::boolalpha << door_open_ << std::endl;
        return door_open_ ? NodeStatus::FAILURE : NodeStatus::SUCCESS;
        });

    factory.registerSimpleAction("OpenDoor",
        [this](BT::TreeNode&) -> NodeStatus {
        std::cout << "[OpenDoor] opening door\n";
        door_locked_ = false;
        door_open_   = true;
        return NodeStatus::SUCCESS;
        });

    factory.registerSimpleAction("PickLock",
        [this](BT::TreeNode&) -> NodeStatus {
        std::cout << "[PickLock] attempt " << (pick_attempts_+1) << "\n";
        if (pick_attempts_++ >= 2) { // succeed on 3rd try
            door_locked_ = false;
            door_open_   = true;
            std::cout << "[PickLock] success\n";
            return NodeStatus::SUCCESS;
        }
        std::this_thread::sleep_for(std::chrono::milliseconds(300));
        return NodeStatus::FAILURE;
        });

    factory.registerSimpleAction("SmashDoor",
        [this](BT::TreeNode&) -> NodeStatus {
        std::cout << "[SmashDoor] brute force!\n";
        door_locked_ = false;
        door_open_   = true;
        return NodeStatus::SUCCESS;
        });

    factory.registerSimpleAction("PassThroughDoor",
        [this](BT::TreeNode&) -> NodeStatus {
        std::cout << "[PassThroughDoor] ";
        if (door_open_) { std::cout << "passed.\n"; return NodeStatus::SUCCESS; }
        std::cout << "blocked.\n"; return NodeStatus::FAILURE;
        });
    }


  void reset() {
    door_open_ = false;
    door_locked_ = true;
    pick_attempts_ = 0;
  }

private:
  bool door_open_ = false;
  bool door_locked_ = true;
  int  pick_attempts_ = 0;
};

int main(int argc, char** argv)
{
  const std::string share_dir = ament_index_cpp::get_package_share_directory("bt_first_tree_demo");
  std::filesystem::path xml_path = std::filesystem::path(share_dir) / "my_tree.xml";

  BT::BehaviorTreeFactory factory;

  CrossDoor cross_door;
  cross_door.registerNodes(factory);

  const std::string models_xml = BT::writeTreeNodesModelXML(factory);
  const std::string models_dir = share_dir + "/models";
  fs::create_directories(models_dir);
  const std::string models_file = models_dir + "/models.xml";
  std::ofstream(models_file) << models_xml;
  std::cout << "[Info] Wrote models to: " << models_file << std::endl;

  factory.registerBehaviorTreeFromFile(xml_path);
  auto tree = factory.createTree("Main");

  BT::Groot2Publisher publisher(tree);

  while (true)
  {
    std::cout << "=== New run ===" << std::endl;
    cross_door.reset();
    tree.tickWhileRunning();
    std::this_thread::sleep_for(2s);
  }
  return 0;
}

11 References

• https://www.behaviortree.dev/docs/learn-the-basics/BT_basics