编写新的导航器插件

概述

本教程将介绍如何基于 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/tree/main/nav2_bt_navigator>`_ 中找到，作为``NavigateToPoseNavigator``。此包可视为编写您自己的插件的参考。

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

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

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

Virtual method

Method description

Requires override?

getDefaultBTFilepath()

Method is called on initialization to retrieve the default BT filepath to use for navigation. This may be done via parameters, hardcoded logic, sentinel files, etc.

Yes

configure()

Method is called when BT navigator server enters on_configure state. This method should implement operations which are necessary before navigator goes to an active state, such as getting parameters, setting up the blackboard, etc.

No

activate()

Method is called when BT navigator server enters on_activate state. This method should implement operations which are necessary before navigator goes to an active state, such as create clients and subscriptions.

No

deactivate()

Method is called when BT navigator server enters on_deactivate state. This method should implement operations which are necessary before navigator goes to an inactive state.

No

cleanup()

Method is called when BT navigator server goes to on_cleanup state. This method should clean up resources which are created for the navigator.

No

goalReceived()

Method is called when a new goal is received by the action server to process. It may accept or deny this goal with its return signature. If accepted, it may need to load the appropriate parameters from the request (e.g. which BT to use), add request parameters to the blackboard for your applications use, or reset internal state.

Yes

onLoop()

Method is called periodically while the behavior tree is looping to check statuses or more commonly to publish action feedback statuses to the client.

Yes

onPreempt()

Method is called when a new goal is requesting preemption over the existing goal currently being processed. If the new goal is viable, it should make all appropriate updates to the BT and blackboard such that this new request may immediately start being processed without hard cancellation of the initial task (e.g. preemption).

Yes

goalCompleted()

Method is called when a goal is completed to populate the action result object or do any additional checks required at the end of a task.

Yes

getName()

Method is called to get the name of this navigator type

Yes

在 Navigate to Pose Navigator 中，“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 Navigator 提供的里程计平滑器一起存储，以便稍后填充有意义的反馈。与此相辅相成的是，“清理”方法将重置这些资源。此特定导航器不使用激活和停用方法。

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

在“getDefaultBTFilepath()”中，我们使用参数“default_nav_to_pose_bt_xml”来获取默认行为树 XML 文件（如果导航请求未提供），并使用热加载的行为树初始化 BT Navigator。如果参数文件中未提供，则我们会在“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 方法为您提供 goal 和返回值（如果正在处理）。此信息将发送回操作服务器以通知客户端。在这种情况下，我们要确保目标的行为树有效，否则我们无法继续。如果它有效，那么我们可以将目标姿势初始化到黑板上并重置某些状态，以便干净地处理这个新请求。

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;
}

一旦完成此目标，我们需要填充 Action 的结果（如果需要且有意义）。在此导航器的情况下，当导航请求成功完成时，它不包含任何结果信息，因此此方法为空。对于其他导航器类型，您可以填充提供的“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 Action 节点负责，以规划一条到达目标的新路线（然后通过之前设置的黑板 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: Plugin’s library name and it’s location.

  • class name: Name of the class.

  • class type: Type of class.

  • base class: Name of the base class.

  • description: Description of the plugin.

<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. 下一步是使用 CMake 函数“pluginlib_export_plugin_description_file()”导出“CMakeLists.txt”插件。此函数将插件描述文件安装到“share”目录并设置索引以使其可发现。

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-通过 params 文件传递​​插件名称

要启用该插件，我们需要修改“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 # or $(find-pkg-share my_package)/behavior_tree/my_nav_to_pose_bt.xml
    default_nav_through_poses_bt_xml: replace/with/path/to/bt.xml # or $(find-pkg-share my_package)/behavior_tree/my_nav_through_poses_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" # In Iron and older versions, "/" was used instead of "::"
    navigate_through_poses:
      plugin: "nav2_bt_navigator::NavigateThroughPosesNavigator" # In Iron and older versions, "/" was used instead of "::"

在上面的代码片段中，您可以观察到我们的“nav2_bt_navigator::NavigateToPoseNavigator”插件到其 id“navigate_to_pose”的映射。 为了传递插件特定的参数，我们使用了“<plugin_id>.<plugin_specific_parameter>”。

4-运行插件

运行启用 Nav2 的 Turtlebot3 模拟。有关如何运行的详细说明，请参阅：ref:getting_started。下面是快捷命令：

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

然后转到 RViz，单击顶部的“2D 姿势估计”按钮，并指向地图上的位置，如 入门指南 中所述。 机器人将在地图上定位，然后单击“Nav2 目标”，然后单击您希望机器人导航到的姿势。 之后，导航器将接管提供给它的行为树 XML 文件行为定义。