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

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

有了上述前两节我们已经可以实现节点间的通信，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){// 使用字符串流替代 splitStringstd::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){// 使用字符串流替代 splitStringstd::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 BTnamespace 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. 执行结果