nav2里行为树如何新建动作节点来执行某个package的某个node，并将其与行为节点连接起来

在nav2中使用行为树新建动作节点来执行某个package的某个node的步骤如下：

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

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

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

#include "my_action_nodes/my_execution_action.hpp"
#include "rclcpp/rclcpp.hpp"
#include <cstdlib>

namespace my_action_nodes
{

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

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

void MyExecutionAction::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());

  std::string node_name;
  if (goal->arguments.size() == 1 && goal->arguments[0].type == MyAction::Goal::Argument::STRING)
  {
    node_name = goal->arguments[0].string_value;
  }
  else
  {
    auto result = std::make_shared<MyAction::Result>();
    result->result = false;
    this->goal_handle_->terminate(result);
    return;
  }

  std::string command = "ros2 run " + node_name;

  int ret = std::system(command.c_str());

  if (ret == 0)
  {
    auto result = std::make_shared<MyAction::Result>();
    result->result = true;
    this->goal_handle_->succeed(result);
  }
  else
  {
    auto result = std::make_shared<MyAction::Result>();
    result->result = false;
    this->goal_handle_->terminate(result);
  }
}

void MyExecutionAction::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 MyExecutionAction::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;
}

}  // namespace my_action_nodes

其中，`MyExecutionAction`类继承自`rclcpp::Node`类，用于创建行为节点。`handle_goal()`、`handle_cancel()`、`handle_accepted()`函数分别处理行为节点的目标、取消和接受请求。`handle_goal()`函数中使用`std::system()`函数执行指定的节点。

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

<behavior_tree>
  <Action Name="MyExecutionAction" Type="my_action_nodes/MyExecutionAction">
    <OutputPort Name="status" />
    <Param Name="action_name" Type="string" />
  </Action>
</behavior_tree>

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

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

<Action Name="ExecuteNode" Type="my_action_nodes/MyExecutionAction">
  <Param name="action_name" value="my_package my_node" />
  <OutputPort name="status" />
</Action>

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