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

在nav2中使用行为树新建动作节点的步骤如下：

1. 在`<your_package>/src`目录下新建一个名为`my_action_nodes`的包，用于存放自定义的行为节点。

2. 在`my_action_nodes`包中创建一个名为`my_subscription_action`的C++文件，该文件用于实现订阅节点的功能。

3. 在`my_subscription_action`文件中添加以下代码：

#include "my_action_nodes/my_subscription_action.hpp"
#include "rclcpp/rclcpp.hpp"

namespace my_action_nodes
{
MySubscriptionAction::MySubscriptionAction(const std::string& action_name)
: rclcpp_action::ServerGoalHandle<MyAction>(nullptr), Node("my_subscription_action")
{
  using namespace std::placeholders;
  this->action_server_ = rclcpp_action::create_server<MyAction>(
    this,
    action_name,
    std::bind(&MySubscriptionAction::handle_goal, this, _1, _2),
    std::bind(&MySubscriptionAction::handle_cancel, this, _1),
    std::bind(&MySubscriptionAction::handle_accepted, this, _1));

  this->subscription_ = this->create_subscription<std_msgs::msg::String>(
    "/topic_name",
    10,
    std::bind(&MySubscriptionAction::subscription_callback, this, _1));

  RCLCPP_INFO(this->get_logger(), "Created MySubscriptionAction");
}

void MySubscriptionAction::handle_goal(
  const rclcpp_action::GoalUUID& uuid,
  std::shared_ptr<const MyAction::Goal> goal)
{
  RCLCPP_INFO(this->get_logger(), "Received goal request");
  this->goal_handle_ = rclcpp_action::ServerGoalHandle<MyAction>::get_from_server_goal_handle(this->get_goal_handle());
}

void MySubscriptionAction::handle_cancel(
  const std::shared_ptr<rclcpp_action::ServerGoalHandle<MyAction>> goal_handle)
{
  RCLCPP_INFO(this->get_logger(), "Received request to cancel goal");
  this->action_server_->terminate_all();
}

void MySubscriptionAction::handle_accepted(
  const std::shared_ptr<rclcpp_action::ServerGoalHandle<MyAction>> goal_handle)
{
  RCLCPP_INFO(this->get_logger(), "Accepted new goal request");
  this->goal_handle_ = goal_handle;
}

void MySubscriptionAction::subscription_callback(const std_msgs::msg::String::SharedPtr msg)
{
  RCLCPP_INFO(this->get_logger(), "Received message: %s", msg->data.c_str());
  if (this->goal_handle_)
  {
    auto result = std::make_shared<MyAction::Result>();
    result->result = true;
    this->goal_handle_->succeed(result);
  }
}

}  // namespace my_action_nodes

其中，`MySubscriptionAction`类继承自`rclcpp::Node`类，用于创建行为节点。`handle_goal()`、`handle_cancel()`、`handle_accepted()`函数分别处理行为节点的目标、取消和接受请求。`subscription_callback()`函数用于接收订阅消息。

4. 在`my_action_nodes`包中创建一个名为`my_action.xml`的文件，用于定义自定义行为节点。文件内容如下：

<behavior_tree>
  <Action Name="MySubscriptionAction" Type="my_action_nodes/MySubscriptionAction">
    <OutputPort Name="status" />
  </Action>
</behavior_tree>

其中，`Type`字段指定了行为节点的类型。`OutputPort`字段用于定义输出端口。

5. 在行为树中使用自定义行为节点。在需要订阅节点的行为节点处，添加以下代码：

<Action Name="SubscribeToNode" Type="my_action_nodes/MySubscriptionAction">
  <Param name="action_name" value="my_subscription_action" />
  <OutputPort name="status" />
</Action>

其中，`name`字段指定了行为节点的名称，`value`字段指定了订阅的节点名称。