nav2里行为树如何新建动作节点编写create_subscription() 函数来订阅某个节点的主题，并将其与行为节点连接起来

在nav2中新建动作节点时，需要在行为树的XML文件中添加动作节点的定义。动作节点的定义中包含了动作节点的名称、输入参数、输出参数、以及动作执行的具体逻辑。下面是一个示例动作节点的定义：

<BehaviorTree>
  <root main_tree_to_execute="MainTree">
    <BehaviorTree ID="MainTree">
      <Sequence name="root">
        <!-- other nodes -->
        <Action name="subscribe_to_topic" param_topic_name="my_topic" param_message_type="std_msgs/String" output_value="subscription"/>
        <!-- other nodes -->
      </Sequence>
    </BehaviorTree>
  </root>
</BehaviorTree>

在上面的示例中，动作节点的名称为 `subscribe_to_topic`，它有两个输入参数 `param_topic_name` 和 `param_message_type`，以及一个输出参数 `output_value`。动作节点执行的逻辑是使用 `rclcpp::create_subscription()` 函数订阅名为 `my_topic` 的主题，并将订阅对象保存到输出参数 `subscription` 中。

下面是动作节点对应的C++代码实现：

#include "rclcpp/rclcpp.hpp"
#include "behaviortree_cpp_v3/behavior_tree.h"

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

  static BT::PortsList providedPorts()
  {
    return {
      BT::InputPort<std::string>("topic_name", "Name of the topic to subscribe to"),
      BT::InputPort<std::string>("message_type", "Type of the message to subscribe to"),
      BT::OutputPort<rclcpp::SubscriptionBase::SharedPtr>("subscription", "Pointer to the subscription object")
    };
  }

  BT::NodeStatus tick() override
  {
    std::string topic_name;
    if (!getInput<std::string>("topic_name", topic_name)) {
      throw BT::RuntimeError("missing required input [topic_name]");
    }

    std::string message_type;
    if (!getInput<std::string>("message_type", message_type)) {
      throw BT::RuntimeError("missing required input [message_type]");
    }

    auto node = rclcpp::Node::make_shared("subscribe_to_topic_node");
    auto subscription = node->create_subscription<message_type>(
      topic_name,
      10,
      [](const message_type::SharedPtr msg) {
        // Do something with the received message
      });

    setOutput("subscription", subscription);

    return BT::NodeStatus::SUCCESS;
  }
};

在上面的代码中，`SubscribeToTopic` 类继承自 `BT::SyncActionNode`，表示它是一个同步动作节点。在 `providedPorts()` 函数中定义了节点的输入和输出端口，输入端口包括 `topic_name` 和 `message_type`，输出端口为 `subscription`。`tick()` 函数是节点的执行逻辑，其中通过 `getInput()` 函数获取输入参数的值，然后使用 `rclcpp::create_subscription()` 函数订阅主题，并将订阅对象保存到输出参数中。最后返回 `BT::NodeStatus::SUCCESS` 表示节点执行成功。

在行为树的XML文件中，可以使用 `<Action>` 标签来引用上面定义的 `SubscribeToTopic` 类型的动作节点，并通过 `param_topic_name` 和 `param_message_type` 属性来指定输入参数的值。例如：

<Action name="subscribe_to_topic" param_topic_name="my_topic" param_message_type="std_msgs/String" output_value="subscription"/>

这样就完成了动作节点的创建和使用。当行为树执行到这个节点时，它会调用 `SubscribeToTopic::tick()` 函数来执行订阅主题的操作，然后将订阅对象保存到 `output_value` 中。其他节点可以通过输入端口来获取这个订阅对象，然后使用它来访问订阅的主题。