文章目录
- 一、通用端口功能实现
-
- [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等)，但作为导航，我们希望节点间实现自定义数据的传递，如：导航点的数据传递：
cpp
// 自定义二维位姿类型
struct Pose2D
{
double x;
double y;
double yaw;
};接下来，我们将展示如何将通用的 C++ 类型作为端口。
一、通用端口功能实现
1. 功能实现
1.1 头文件定义
为了让XML加载器能从字符串中实例化Pose2D，需提供convertFromString(BT::StringView str) 的模板类实现。如何将Pose2D序列化为字符串参考如下：
cpp
// 在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接着按照惯例，就是头文件定义操作了。
cpp
#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_H1.2 源文件实现
cpp
#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文件实现
cpp
#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文件定义如下:
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. 执行结果
