nav2里行为树行为节点怎么去连接某个package的某个node

在nav2中，行为树行为节点可以通过在配置文件中指定节点名称或通过程序中的 API 调用来连接到某个package的某个节点。具体步骤如下：

1. 在行为树的配置文件中，使用节点名称来指定要连接的节点。例如，使用以下方式指定一个名为"my_node"的行为节点：

- MyBehavior:
    type: MyBehavior
    my_node: /my_package/my_node

2. 在程序中，使用 `create_subscription()` 函数来订阅某个节点的主题，并将其与行为节点连接起来。例如，在 C++ 中，可以使用以下代码将一个名为"my_topic"的主题与行为节点连接起来：

auto my_subscriber = node->create_subscription<MyMsg>("my_topic", 10);
auto my_behavior = BT::NodeBuilder().createFromType<MyBehavior>("my_behavior", {BT::InputPort<MyMsg>("input_port")});
my_behavior->setInput<std::shared_ptr<MyMsg>>("input_port", my_subscriber);

这样，当 "my_topic" 主题发布新消息时，它将被传递给 "my_behavior" 行为节点的 "input_port" 输入端口中。

相关问题

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`字段指定了要执行的节点的名称。

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`字段指定了订阅的节点名称。