使用行为树控制机器人(二) —— 黑板

在 使用行为树控制机器人(一) —— 节点 实现了节点执行，但节点间是封闭的无法进行数据传递的，此文的目的即是解决节点间的数据传输问题 —— 黑板。
学习时参考链接：ROS机器人行为树教程

一、概念

“黑板” 是一个简单的键/值存储，由树的所有节点共享。黑板上的一个 "条目 "是一个键/值对。输入端口可以读取黑板上的一个条目，而输出端口可以写入一个条目。
下面创建一个行为树逻辑：创建一个ThinWhatToSay节点 使用一个输出端口通过键 topic 写入黑板条目，然后创建另一个节点SaySomething 使用一个输入端口通过键 topic 读取黑板条目该键指向的对应值并将其内容进行打印 (下图中键 topic 其值为"The answer is 42")。

其中，Script Input 用于调试，可使用内置的名为"Script"的动作将一个静态值写入条目中。

<Script code=" topic1:='message by script' " />

二、黑板功能实现

1. 功能实现

1.1 头文件定义

#ifndef BEHAVIOR_TREE_NODES_H
#define BEHAVIOR_TREE_NODES_H

#include "behaviortree_cpp/bt_factory.h"
#include <iostream>

namespace BT
{

// 带输入端口的同步动作节点
class SaySomething : public SyncActionNode
{
public:
  SaySomething(const std::string& name, const NodeConfig& config);

  // 必须实现静态端口声明方法
  static PortsList providedPorts();

  // 节点执行函数
  NodeStatus tick() override;
};

// 带输出端口的同步动作节点
class ThinkWhatToSay : public SyncActionNode
{
public:
  ThinkWhatToSay(const std::string& name, const NodeConfig& config);

  // 端口声明
  static PortsList providedPorts();

  // 节点执行函数
  NodeStatus tick() override;
  std::string _info = "The answer is 42";
};

} // namespace BT

#endif // BEHAVIOR_TREE_NODES_H

注意：自定义的TreeNode有输入和/或输出端口时，这些端口必须在静态方法中声明，可以使用模板方法TreeNode::getInput(key)来读取端口消息的输入。

static PortsList providedPorts();

1.2 源文件实现

#include "behavior_tree_nodes.h"

using namespace BT;

// SaySomething 实现
SaySomething::SaySomething(const std::string& name, const NodeConfig& config) : SyncActionNode(name, config)
{}

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

NodeStatus SaySomething::tick()
{
  // 直接使用 BT::Expected 类型
  BT::Expected<std::string> msg = getInput<std::string>("message");
  
  // 检查是否成功获取输入
  if (!msg)
  {
    throw BT::RuntimeError("missing required input [message]: " + msg.error());
  }
  
  // 输出消息
  std::cout << "Robot says: " << msg.value() << std::endl;
  return NodeStatus::SUCCESS;
}

// ThinkWhatToSay 实现
ThinkWhatToSay::ThinkWhatToSay(const std::string& name, const NodeConfig& config) : SyncActionNode(name, config)
{}

PortsList ThinkWhatToSay::providedPorts()
{
  return {
      InputPort<std::string>("info"),  // 添加输入端口
      OutputPort<std::string>("text")
      };
}

NodeStatus ThinkWhatToSay::tick() {

  auto msg = getInput<std::string>("info");
  
  // 检查是否成功获取输入
  if (!msg)
  {
    throw BT::RuntimeError("missing required input [info]: " + msg.error());
  }
  _info = msg.value();

  setOutput("text", _info);
  return NodeStatus::SUCCESS;
}

1.3 main文件实现

#include "behavior_tree_nodes.h"
#include "behaviortree_cpp/bt_factory.h"

int main(int argc, char *argv[])
{
  std::string think_info="The answer is 43";
  BT::BehaviorTreeFactory factory;
  factory.registerNodeType<BT::SaySomething>("SaySomething");
  factory.registerNodeType<BT::ThinkWhatToSay>("ThinkWhatToSay");

  if (argc > 1) {
    think_info = argv[1];
    std::cout << "Using command line argument: " << think_info << std::endl;
  } else {
    std::cout << "Using default value: " << think_info << std::endl;
  }

  try {
    auto tree = factory.createTreeFromFile("../trees/my_tree.xml");
    tree.rootBlackboard()->set("topic1", think_info); // 设置黑板参数
    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;
}

1.4 my_tree.xml 实现

行为树实现逻辑如上，xml文件定义如下，使用相同的键(topic) 将输出端口与输入端口 “连接” 起来:

<root BTCPP_format="4" main_tree_to_execute="MainTree">
  <BehaviorTree ID="MainTree">
    <Sequence name="root_sequence">
      <!-- Script code 注释，可命令动态传参 -->
      <Script code=" topic1:='message by script' " />
      <!-- 直接传递字符串 -->
      <SaySomething name="say_hello" message="hello"/>
      
      <!-- 通过黑板传递 -->
      <ThinkWhatToSay name="think" text="{topic}" info="{topic1}"/>
      <SaySomething name="say_answer" message="{topic}"/>
    </Sequence>
  </BehaviorTree>
</root>

2. 执行结果

上述行为树执行结果如下：

如不想通过 <Script code=" topic1:='message by script' " /> ，而是通过命令动态传参，且只需将其注释，重新运行即可。