使用行為樹控制機器人(一) —— 節點
使用行為樹控制機器人(二) —— 黑板
使用行為樹控制機器人(三) —— 通用端口

有了上述前兩節我們已經可以實現節點間的通信，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. 執行結果