编写新的导航器插件

概述

本教程展示了如何基于 nav2_core::BehaviorTreeNavigator 基类创建自己的行为树导航器 插件。

在本教程中，我们将回顾“导航到位姿（Navigate to Pose）”行为树导航器插件，它是Nav2的基础导航器和ROS 1 Navigation的补充行为。这完成了点对点导航。本教程将回顾ROS 2 Iron版本的代码和结构。尽管随着时间的推移可能会进行一些小的变化，但这应该足以让您开始编写自己的导航器，因为我们不希望在此系统上进行重大的API更改。

如果您有自定义的操作消息定义，并且希望与导航一起使用而不是使用提供的“NavigateToPose”或“NavigateThroughPoses”接口（例如完成全面覆盖或包含其他约束信息），则编写自己的导航器可能会有益处。导航器的作用是从请求中提取信息，传递给行为树/黑板，填充反馈和响应，并在相关时维护行为树的状态。行为树的 XML 将定义实际使用的导航逻辑。

要求

  • ROS 2（二进制或源码编译）

  • Nav2（包括依赖项）

  • Gazebo

  • Turtlebot3

教程步骤

1- 创建新的导航器插件

我们将实现纯点对点导航行为。本教程中的代码可以在`Nav2 的 BT Navigator 包 <https://github.com/ros-planning/navigation2/nav2_bt_navigator>`_ 中找到，命名为“NavigateToPoseNavigator”。可以将此包视为编写自己插件的参考。

我们的示例插件类``nav2_bt_navigator::NavigateToPoseNavigator``继承自基类``nav2_core::BehaviorTreeNavigator``。基类提供了一组虚拟方法来实现导航器插件。这些方法在运行时由BT导航服务器调用，或作为对ROS 2动作的响应以处理导航请求。

请注意，这个类本身有一个基类“NavigatorBase”。该类提供了一个非模板化的基类，用于将插件加载到向量中以进行存储，并在生命周期节点中进行基本状态转换的调用。其成员（例如“on_XYZ”）在“BehaviorTreeNavigator”中已经为您实现，并标记为“final”，因此用户无法重写它们。您要实现的导航器的 API 是“BehaviorTreeNavigator”中的虚拟方法，而不是“NavigatorBase”。这些“on_XYZ” API 在“BehaviorTreeNavigator”中的必要函数中实现，以处理有关行为树和动作服务器的样板逻辑，以最小化导航器实现之间的代码重复（例如，“on_configure”将创建动作服务器，注册回调，将黑板填充为一些必要的基本信息，然后调用用户定义的“configure”函数以满足任何额外的用户特定需求）。

下表列出了方法及其描述和必要性：

虚拟方法

方法描述

需要重写？

getDefaultBTFilepath()

在初始化时调用该方法以获取用于导航的默认BT文件路径。这可以通过参数、硬编码逻辑、哨兵文件等方式完成。

是

configure()

当 BT 导航服务器进入“on_configure”状态时调用此方法。该方法应该实现在导航器进入活动状态之前的必要操作，如获取参数、设置黑板等。

No

activate()

当BT导航服务器进入on_activate状态时调用该方法。此方法应实现在导航器进入活动状态之前必需的操作，例如创建客户端和订阅。

No

deactivate()

当 BT 导航服务器进入“on_deactivate”状态时调用此方法。该方法应该实现在导航器进入非活动状态之前的必要操作。

No

cleanup()

当BT导航服务器进入on_cleanup状态时调用该方法。此方法应清理为导航器创建的资源。

No

goalReceived()

当动作服务器接收到新目标以进行处理时调用该方法。它可以通过返回值来接受或拒绝该目标。如果接受，可能需要从请求中加载适当的参数（例如要使用的BT），将请求参数添加到黑板以供应用程序使用，或重置内部状态。

是

onLoop()

在行为树循环过程中，定期调用此方法来检查状态，或更常见地向客户端发布动作反馈状态。

是

onPreempt()

当一个新目标请求对当前正在处理的现有目标进行抢占时，调用该方法。如果新目标可行，应对BT和黑板进行所有适当的更新，以使该新请求可以立即开始处理，而无需硬取消初始任务（例如抢占）。

是

goalCompleted()

当目标完成以填充动作结果对象或在任务结束时执行任何其他必要检查时调用该方法。

是

getName()

调用该方法以获取此导航器类型的名称。

是

在导航到位点导航器中，configure() 方法必须确定存储目标和路径的黑板参数名称，因为这些是在 onLoop 中处理反馈的关键值，并且对于不同的行为树节点之间进行信息交流也很重要。此外，对于这种导航器类型，我们还创建一个自身的客户端和一个订阅 goal_pose 话题的订阅器，以便处理来自 Rviz2 的默认配置（使用 目标位姿 工具）的请求。

bool NavigateToPoseNavigator::configure(
  rclcpp_lifecycle::LifecycleNode::WeakPtr parent_node,
  std::shared_ptr<nav2_util::OdomSmoother> odom_smoother)
{
  start_time_ = rclcpp::Time(0);
  auto node = parent_node.lock();

  if (!node->has_parameter("goal_blackboard_id")) {
    node->declare_parameter("goal_blackboard_id", std::string("goal"));
  }

  goal_blackboard_id_ = node->get_parameter("goal_blackboard_id").as_string();

  if (!node->has_parameter("path_blackboard_id")) {
    node->declare_parameter("path_blackboard_id", std::string("path"));
  }

  path_blackboard_id_ = node->get_parameter("path_blackboard_id").as_string();

  // Odometry smoother object for getting current speed
  odom_smoother_ = odom_smoother;

  self_client_ = rclcpp_action::create_client<ActionT>(node, getName());

  goal_sub_ = node->create_subscription<geometry_msgs::msg::PoseStamped>(
    "goal_pose",
    rclcpp::SystemDefaultsQoS(),
    std::bind(&NavigateToPoseNavigator::onGoalPoseReceived, this, std::placeholders::_1));
  return true;
}

黑板ID的值与里程平滑器存储在一起，BT导航器提供了填充有意义反馈的功能，以便稍后使用。与此相辅相成的是，``cleanup``方法将重置这些资源。在此特定的导航器中，不使用activate和deactivate方法。

bool NavigateToPoseNavigator::cleanup()
{
  goal_sub_.reset();
  self_client_.reset();
  return true;
}

在 getDefaultBTFilepath() 中，我们使用参数 default_nav_to_pose_bt_xml 来获取默认的行为树 XML 文件，以便在导航请求没有提供文件时使用，并用其初始化 BT 导航器。如果参数文件中没有提供文件，那么我们会在 nav2_bt_navigator 包中选择一个已知且合理的默认 XML 文件：

std::string NavigateToPoseNavigator::getDefaultBTFilepath(
  rclcpp_lifecycle::LifecycleNode::WeakPtr parent_node)
{
  std::string default_bt_xml_filename;
  auto node = parent_node.lock();

  if (!node->has_parameter("default_nav_to_pose_bt_xml")) {
    std::string pkg_share_dir =
      ament_index_cpp::get_package_share_directory("nav2_bt_navigator");
    node->declare_parameter<std::string>(
      "default_nav_to_pose_bt_xml",
      pkg_share_dir +
      "/behavior_trees/navigate_to_pose_w_replanning_and_recovery.xml");
  }

  node->get_parameter("default_nav_to_pose_bt_xml", default_bt_xml_filename);

  return default_bt_xml_filename;
}

当接收到一个目标时，我们需要确定该目标是否有效并且应该被处理。``goalReceived``方法提供了目标和一个返回值，用于确定目标是否正在被处理。这些信息被发送回操作服务器以通知客户端。在这种情况下，我们要确保目标的行为树是有效的，否则我们无法继续。如果有效，我们可以将目标姿态初始化到黑板上，并重置一些状态，以便清洁地处理这个新请求。

bool NavigateToPoseNavigator::goalReceived(ActionT::Goal::ConstSharedPtr goal)
{
  auto bt_xml_filename = goal->behavior_tree;

  if (!bt_action_server_->loadBehaviorTree(bt_xml_filename)) {
    RCLCPP_ERROR(
      logger_, "BT file not found: %s. Navigation canceled.",
      bt_xml_filename.c_str());
    return false;
  }

  initializeGoalPose(goal);

  return true;
}

一旦完成了这个目标，如果需要且有意义，我们需要填充动作的结果。在这个导航器的情况下，当导航请求成功完成时，它不包含任何结果信息，因此该方法为空。对于其他导航器类型，您可以填充提供的 result 对象。

void NavigateToPoseNavigator::goalCompleted(
  typename ActionT::Result::SharedPtr /*result*/,
  const nav2_behavior_tree::BtStatus /*final_bt_status*/)
{
}

然而，如果一个目标被抢占（例如，在处理现有请求时收到新的请求），``onPreempt()``方法将被调用，以确定新的请求是否真实且适合抢占当前处理的目标。例如，如果该请求在本质上与现有的行为树任务有根本不同，或者当您现有的任务具有较高的优先级时，接受抢占请求可能是不明智的。

void NavigateToPoseNavigator::onPreempt(ActionT::Goal::ConstSharedPtr goal)
{
  RCLCPP_INFO(logger_, "Received goal preemption request");

  if (goal->behavior_tree == bt_action_server_->getCurrentBTFilename() ||
    (goal->behavior_tree.empty() &&
    bt_action_server_->getCurrentBTFilename() == bt_action_server_->getDefaultBTFilename()))
  {
    // if pending goal requests the same BT as the current goal, accept the pending goal
    // if pending goal has an empty behavior_tree field, it requests the default BT file
    // accept the pending goal if the current goal is running the default BT file
    initializeGoalPose(bt_action_server_->acceptPendingGoal());
  } else {
    RCLCPP_WARN(
      logger_,
      "Preemption request was rejected since the requested BT XML file is not the same "
      "as the one that the current goal is executing. Preemption with a new BT is invalid "
      "since it would require cancellation of the previous goal instead of true preemption."
      "\nCancel the current goal and send a new action request if you want to use a "
      "different BT XML file. For now, continuing to track the last goal until completion.");
    bt_action_server_->terminatePendingGoal();
  }
}

请注意，在这里您还可以看到调用了 initializeGoalPose 方法。该方法将将导航器的目标参数设置到黑板上，并重置重要的状态信息，以便在不带有旧状态信息的情况下清洁地重用行为树，如下所示：

void
NavigateToPoseNavigator::initializeGoalPose(ActionT::Goal::ConstSharedPtr goal)
{
  RCLCPP_INFO(
    logger_, "Begin navigating from current location to (%.2f, %.2f)",
    goal->pose.pose.position.x, goal->pose.pose.position.y);

  // Reset state for new action feedback
  start_time_ = clock_->now();
  auto blackboard = bt_action_server_->getBlackboard();
  blackboard->set<int>("number_recoveries", 0);  // NOLINT

  // Update the goal pose on the blackboard
  blackboard->set<geometry_msgs::msg::PoseStamped>(goal_blackboard_id_, goal->pose);
}

恢复计数器和启动时间都是客户端了解当前任务状态的重要反馈信息（例如，如果它失败、出现问题或者花费异常长的时间）。将目标设置到黑板上是由``ComputePathToPose``BT动作节点接管的，以规划到目标的新路径（然后将其路径通过之前设置的黑板ID传递给``FollowPath``BT节点）。

最后实现的函数是 onLoop，出于教学目的，下面进行了简化。虽然在此方法中可以执行任何操作，该方法会在BT通过树循环时被调用，通常可以利用此机会填充有关导航请求、机器人或客户端可能感兴趣的元数据的任何必要反馈信息。

void NavigateToPoseNavigator::onLoop()
{
  auto feedback_msg = std::make_shared<ActionT::Feedback>();

  geometry_msgs::msg::PoseStamped current_pose = ...;
  auto blackboard = bt_action_server_->getBlackboard();
  nav_msgs::msg::Path current_path;
  blackboard->get<nav_msgs::msg::Path>(path_blackboard_id_, current_path);

  ...

  feedback_msg->distance_remaining = distance_remaining;
  feedback_msg->estimated_time_remaining = estimated_time_remaining;

  int recovery_count = 0;
  blackboard->get<int>("number_recoveries", recovery_count);
  feedback_msg->number_of_recoveries = recovery_count;
  feedback_msg->current_pose = current_pose;
  feedback_msg->navigation_time = clock_->now() - start_time_;

  bt_action_server_->publishFeedback(feedback_msg);
}

2- 导出导航器插件

现在，我们已经创建了自定义的导航器，需要导出我们的插件，以便BT Navigator服务器能够看到它。插件在运行时加载，如果它们不可见，则我们的服务器将无法加载它。在ROS 2中，导出和加载插件由 pluginlib 处理。

接下来我们的教程中，``nav2_bt_navigator::NavigateToPoseNavigator``类以``nav2_core::NavigatorBase``的形式动态加载，这是我们的基类，由于前面描述的细微差别。

  1. 要导出控制器，我们需要提供两行代码。

#include "pluginlib/class_list_macros.hpp"
PLUGINLIB_EXPORT_CLASS(nav2_bt_navigator::NavigateToPoseNavigator, nav2_core::NavigatorBase)

请注意，它需要 pluginlib 来导出插件的类。Pluginlib 将提供宏 PLUGINLIB_EXPORT_CLASS，该宏执行所有导出的工作。

将这些行代码放在文件末尾是一个好的做法，但从技术上讲，你也可以写在文件顶部。

  1. 下一步是在软件包的根目录中创建插件的描述文件。例如，在我们的教程软件包中创建 navigator_plugin.xml 文件。该文件包含以下信息

  • library path：插件的库名称和位置。

  • class name：类的名称。

  • class type：类的类型。

  • base class：基类的名称。

  • description：插件的描述。

<library path="nav2_bt_navigator">
  <class type="nav2_bt_navigator::NavigateToPoseNavigator" base_class_type="nav2_core::NavigatorBase">
    <description>
      This is pure point-to-point navigation
    </description>
  </class>
</library>

  1. 下一步是使用 CMakeLists.txt 导出插件，使用 CMake 函数 pluginlib_export_plugin_description_file()。此函数将插件描述文件安装到 share 目录，并设置 ament 索引以使其可被发现。

pluginlib_export_plugin_description_file(nav2_core navigator_plugin.xml)

  1. 插件描述文件也应添加到``package.xml``中。

<export>
  <build_type>ament_cmake</build_type>
  <nav2_core plugin="${prefix}/navigator_plugin.xml" />
</export>

  1. 编译后，应该进行注册。接下来，我们将使用此插件。

3- 通过参数文件传递插件名称

要启用插件，我们需要修改 nav2_params.yaml 文件如下所示：

bt_navigator:
  ros__parameters:
    use_sim_time: true
    global_frame: map
    robot_base_frame: base_link
    transform_tolerance: 0.1
    default_nav_to_pose_bt_xml: replace/with/path/to/bt.xml
    default_nav_through_poses_bt_xml: replace/with/path/to/bt.xml
    goal_blackboard_id: goal
    goals_blackboard_id: goals
    path_blackboard_id: path
    navigators: ['navigate_to_pose', 'navigate_through_poses']
    navigate_to_pose:
      plugin: "nav2_bt_navigator/NavigateToPoseNavigator"
    navigate_through_poses:
      plugin: "nav2_bt_navigator/NavigateThroughPosesNavigator"

在上面的片段中，您可以观察到我们的``nav2_bt_navigator/NavigateToPoseNavigator``插件与其id navigate_to_pose``之间的映射关系。为了传递插件特定的参数，我们使用了``<plugin_id>.<plugin_specific_parameter>。

4- 运行插件

使用启用的 Nav2 运行 Turtlebot3 模拟。有关如何运行的详细说明，请参阅 开始使用。下面是一个快捷命令：

$ ros2 launch nav2_bringup tb3_simulation_launch.py params_file:=/path/to/your_params_file.yaml

然后打开RViz，在顶部点击“2D Pose Estimate”按钮，并在地图上指定位置，就像在 开始使用 中描述的那样。机器人将在地图上进行定位，然后点击“Nav2 goal”，并点击您希望机器人导航到的姿势。之后导航器将使用提供的行为树XML文件行为定义接管控制。