行为树节点的输入输出

在行为树（Behavior Tree）框架中，节点的输入和输出是实现节点间通信的关键机制。节点可以通过黑箱（Blackboard）来获取输入和提供输出。以下是一些基本的输入输出函数和示例。

输入函数

1. getInput:

   • 从黑箱中获取输入数据。
   • 用法示例：

     auto goal = getInput<geometry_msgs::msg::PoseStamped>("goal");
     

2. tryGetInput:

   • 尝试从黑箱中获取输入数据，如果键不存在则返回 false。
   • 用法示例：

     bool has_goal = tryGetInput<geometry_msgs::msg::PoseStamped>("goal", goal);
     

3. getInputstd::string:

   • 获取字符串类型的输入数据。
   • 用法示例：

     std::string name = getInput<std::string>("name");
     

4. getInput:

   • 获取整数类型的输入数据。
   • 用法示例：

     int count = getInput<int>("count");
     

输出函数

1. setOutput:

   • 将输出数据写入黑箱。
   • 用法示例：

     setOutput("result", result);
     

2. setOutputstd::string:

   • 将字符串类型的输出数据写入黑箱。
   • 用法示例：

     setOutput<std::string>("status", "success");
     

3. setOutput:

   • 将整数类型的输出数据写入黑箱。
   • 用法示例：

     setOutput<int>("count", 10);
     

示例节点

假设我们有一个行为树节点 NavigateToPose，它需要从黑箱中获取目标位置，并在成功导航后将结果写入黑箱。

#include "behaviortree_cpp_v3/bt_factory.h"

class NavigateToPose : public BT::ActionNode {
public:
    NavigateToPose(const std::string& name, const BT::NodeConfiguration& config)
        : BT::ActionNode(name, config) {}

    static BT::PortsList providedPorts() {
        return {
            BT::InputPort<geometry_msgs::msg::PoseStamped>("goal"),
            BT::OutputPort<geometry_msgs::msg::PoseStamped>("result")
        };
    }

    BT::NodeStatus tick() override {
        geometry_msgs::msg::PoseStamped goal;
        if (!tryGetInput("goal", goal)) {
            throw BT::RuntimeError("Missing required input [goal]");
        }

        // 模拟导航逻辑
        bool navigation_success = navigateTo(goal);

        if (navigation_success) {
            geometry_msgs::msg::PoseStamped result = getCurrentPose();
            setOutput("result", result);
            return BT::NodeStatus::SUCCESS;
        } else {
            return BT::NodeStatus::FAILURE;
        }
    }

private:
    bool navigateTo(const geometry_msgs::msg::PoseStamped& goal) {
        // 模拟导航逻辑
        return true;
    }

    geometry_msgs::msg::PoseStamped getCurrentPose() {
        // 模拟获取当前位置
        geometry_msgs::msg::PoseStamped current_pose;
        current_pose.pose.position.x = 1.0;
        current_pose.pose.position.y = 2.0;
        return current_pose;
    }
};

BT_REGISTER_NODES(factory) {
    factory.registerNodeType<NavigateToPose>("NavigateToPose");
}

行为树 XML 配置

<root>
    <BehaviorTree>
        <NavigateToPose ID="nav_to_pose" goal="{goal}" />
    </BehaviorTree>
</root>

在这个例子中：

• NavigateToPose 节点从黑箱中获取名为 "goal" 的输入端口。
• 如果导航成功，它将当前位置写入名为 "result" 的输出端口。
• 黑箱中的 "goal" 键需要在行为树执行之前被正确设置。

通过这种方式，节点可以接收输入数据，并在执行完成后提供输出数据，从而实现节点间的通信和协作。