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

文章目录

• 一、通用端口功能实现
• • [1. 功能实现](#1. 功能实现)
  • • [1.1 头文件定义](#1.1 头文件定义)
    • [1.2 源文件实现](#1.2 源文件实现)
    • [1.3 main文件实现](#1.3 main文件实现)
    • [1.4 tree.xml 实现](#1.4 tree.xml 实现)
  • [2. 执行结果](#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. 执行结果