实用指南：使用行为树控制机器人(三) ——通用端口

文章目录

• 一、通用端口功能实现
• • 1. 功能实现
  • • 1.1 头文件定义
    • 1.2 源文件实现
    • 1.3 main文件实现
    • 1.4 tree.xml 实现
  • 2. 执行结果

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

有了上述前两节我们已经可以实现节点间的通信，BehaviorTree.CPP支持将字符串自动转换为常见类型(如int、long、double、bool、NodeStatus等)，但作为导航，我们希望节点间实现自定义数据的传递，如：导航点的数据传递：

// 自定义二维位姿类型
struct Pose2D
{
double x;
double y;
double yaw;
};

接下来，我们将展示如何将通用的 C++ 类型作为端口。

一、通用端口功能实现

1. 功能实现

1.1 头文件定义

为了让XML加载器能从字符串中实例化Pose2D，需提供convertFromString(BT::StringView str) 的模板类实现。如何将Pose2D序列化为字符串参考如下：

// 在BT命名空间中特化字符串转换
namespace BT
{
template <
>
inline Pose2D convertFromString(BT::StringView str)
{
// 使用字符串流替代 splitString
std::string input(str.data(), str.size());
std::istringstream ss(input);
Pose2D output;
char delimiter;
// 解析格式: "x;y;yaw"
if (!(ss >> output.x >> delimiter >> output.y >> delimiter >> output.yaw) || delimiter != ';')
{
throw BT::RuntimeError("invalid input for Pose2D: " + input);
}
return output;
}
} // end namespace BT

接着按照惯例，就是头文件定义操作了。

#ifndef BEHAVIOR_TREE_NODES_H
#define BEHAVIOR_TREE_NODES_H
#include "behaviortree_cpp/bt_factory.h"
#include <iostream>
  #include <sstream>
    // 添加 sstream 头文件
    // 自定义二维位姿类型
    struct Pose2D
    {
    double x;
    double y;
    double yaw;
    };
    // 在BT命名空间中特化字符串转换
    namespace BT
    {
    template <
    >
    inline Pose2D convertFromString(BT::StringView str)
    {
    // 使用字符串流替代 splitString
    std::string input(str.data(), str.size());
    std::istringstream ss(input);
    Pose2D output;
    char delimiter;
    // 解析格式: "x;y;yaw"
    if (!(ss >> output.x >> delimiter >> output.y >> delimiter >> output.yaw) || delimiter != ';')
    {
    throw BT::RuntimeError("invalid input for Pose2D: " + input);
    }
    return output;
    }
    } // end namespace BT
    namespace BTNodes
    {
    // 写入端口 "goal"
    class CalculateGoal
    : public BT::SyncActionNode
    {
    public:
    CalculateGoal(const std::string& name, const BT::NodeConfig& config);
    static BT::PortsList providedPorts();
    BT::NodeStatus tick() override;
    };
    // 读取端口
    class PrintTarget
    : public BT::SyncActionNode
    {
    public:
    PrintTarget(const std::string& name, const BT::NodeConfig& config);
    static BT::PortsList providedPorts();
    BT::NodeStatus tick() override;
    };
    } // namespace BTNodes
    #endif // BEHAVIOR_TREE_NODES_H

1.2 源文件实现

#include "behavior_tree_nodes.h"
using namespace BTNodes;
// CalculateGoal 实现
CalculateGoal::CalculateGoal(const std::string& name, const BT::NodeConfig& config) : BT::SyncActionNode(name, config)
{
}
BT::PortsList CalculateGoal::providedPorts()
{
return { BT::OutputPort<Pose2D>("goal")
  };
  }
  BT::NodeStatus CalculateGoal::tick()
  {
  // 设置目标位置
  Pose2D mygoal = {
  1.1, 2.3, 0.0
  };
  setOutput<Pose2D>("goal", mygoal);
    return BT::NodeStatus::SUCCESS;
    }
    // PrintTarget 实现
    PrintTarget::PrintTarget(const std::string& name, const BT::NodeConfig& config) : BT::SyncActionNode(name, config)
    {
    }
    BT::PortsList PrintTarget::providedPorts()
    {
    // 可选：添加人类可读的描述
    const char* description = "打印目标位置到控制台";
    return { BT::InputPort<Pose2D>("target", description)
      };
      }
      BT::NodeStatus PrintTarget::tick()
      {
      auto res = getInput<Pose2D>("target");
        if (!res)
        {
        throw BT::RuntimeError("error reading port [target]: " + res.error());
        }
        Pose2D target = res.value();
        printf("目标位置: [%.1f, %.1f, %.1f]\n", target.x, target.y, target.yaw);
        return BT::NodeStatus::SUCCESS;
        }

1.3 main文件实现

#include "behavior_tree_nodes.h"
#include "behaviortree_cpp/bt_factory.h"
#include "behaviortree_cpp/loggers/bt_cout_logger.h"
int main()
{
BT::BehaviorTreeFactory factory;
// 注册自定义节点
factory.registerNodeType<BTNodes::CalculateGoal>("CalculateGoal");
  factory.registerNodeType<BTNodes::PrintTarget>("PrintTarget");
    // 内联 XML 定义
    const std::string xml_text = R"(
    <root BTCPP_format="4" main_tree_to_execute="MainTree">
      <BehaviorTree ID="MainTree">
        <Sequence name="root">
          <CalculateGoal goal="{GoalPosition}" />
          <PrintTarget target="{GoalPosition}" />
          <Script code=" OtherGoal:='-1;3' " />
          <PrintTarget target="{OtherGoal}" />
        </Sequence>
      </BehaviorTree>
    </root>
    )";
    try {
    #if 0
    // 从文本创建行为树
    const std::string tree_xml_path = xml_text;
    auto tree = factory.createTreeFromText(tree_xml_path);
    #else
    // 从XML文件 创建行为树
    const std::string tree_xml_path = "../trees/tree.xml";
    auto tree = factory.createTreeFromFile(tree_xml_path);
    #endif
    // 添加日志记录器（可选）
    BT::StdCoutLogger logger(tree);
    // 打印树结构
    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 tree.xml 实现

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

<root BTCPP_format="4" main_tree_to_execute="MainTree">
    <BehaviorTree ID="MainTree">
      <Sequence name="root">
      <CalculateGoal goal="{GoalPosition}" />
      <PrintTarget target="{GoalPosition}" />
      <Script code=" OtherGoal:='-1;3;3.14' " />
      <PrintTarget target="{OtherGoal}" />
    </Sequence>
  </BehaviorTree>
</root>

2. 执行结果