编写新的规划器插件

梯度演示动画

概述

本教程演示如何创建自定义规划器插件。

环境要求

ROS 2（二进制安装或源码构建）
Nav2（包括依赖）
Gazebo
TurtleBot3

教程步骤

1- 创建新的规划器插件

我们将创建一个简单的直线规划器。本教程中的带注释代码可以在 navigation_tutorials 仓库中找到，即 nav2_straightline_planner。该包可作为编写规划器插件的参考。

示例插件继承自基类 nav2_core::GlobalPlanner。基类提供了 5 个纯虚方法用于实现规划器插件。该插件将被规划器服务器用来计算路径。下面来了解编写规划器插件所需的方法。

虚方法	方法描述	需要重写？
configure()	当规划器服务器进入 on_configure 状态时调用。通常在此方法中声明 ROS 参数并初始化规划器成员变量。该方法接收 4 个参数：父节点的共享指针、规划器名称、tf 缓冲区指针以及代价地图的共享指针。	是
activate()	当规划器服务器进入 on_activate 状态时调用。通常在此方法中完成规划器进入激活状态前所需的操作。	是
deactivate()	当规划器服务器进入 on_deactivate 状态时调用。通常在此方法中完成规划器进入非激活状态前所需的操作。	是
cleanup()	当规划器服务器进入 on_cleanup 状态时调用。通常在此方法中清理规划器所创建的资源。	是
createPlan()	当规划器服务器需要针对给定的起始位姿、目标位姿和中间航点位姿生成全局规划时调用。该方法返回携带全局规划的 `nav_msgs::msg::Path`。该方法接收 4 个参数：起始位姿、目标位姿、中间航点的向量，以及一个用于检查动作是否已被取消的函数。	是

本教程将使用 StraightLine::configure() 和 StraightLine::createPlan() 方法来创建直线规划器。

在规划器中，configure() 方法需根据 ROS 参数设置成员变量并完成所需的初始化：

  node_ = parent;
  tf_ = tf;
  name_ = name;
  costmap_ = costmap_ros->getCostmap();
  global_frame_ = costmap_ros->getGlobalFrameID();

  // Parameter initialization
  nav2_util::declare_parameter_if_not_declared(node_, name_ + ".interpolation_resolution", rclcpp::ParameterValue(0.1));
  node_->get_parameter(name_ + ".interpolation_resolution", interpolation_resolution_);

这里，name_ + ".interpolation_resolution" 获取的是规划器专属的 ROS 参数 interpolation_resolution。Nav2 支持加载多个插件，为便于管理，每个插件都映射到一个 ID/名称。要获取特定插件的参数，使用 <mapped_name_of_plugin>.<name_of_parameter> 格式，如上面代码所示。例如，示例规划器映射到名称「GridBased」，要获取「GridBased」专属的 interpolation_resolution 参数，使用 Gridbased.interpolation_resolution。也就是说，GridBased 被用作插件专属参数的命名空间。后文介绍参数文件时会有更详细的说明。

createPlan() 方法需要根据给定的起始位姿和目标位姿创建一条路径，如有中间航点则依次经过。StraightLine::createPlan() 使用起始位姿、目标位姿和中间航点向量来求解全局路径规划问题，成功后将路径转换为 nav_msgs::msg::Path 返回给规划器服务器。下面是该方法的实现。

  nav_msgs::msg::Path global_path;

  // copy the viapoints and append the goal since intermediate points would not include the goal
  std::vector<geometry_msgs::msg::PoseStamped> goals = viapoints;
  goals.push_back(goal);

  // Checking if the goal and start state is in the global frame
  if (start.header.frame_id != global_frame_) {
    RCLCPP_ERROR(
      node_->get_logger(), "Planner will only except start position from %s frame",
      global_frame_.c_str());
    return global_path;
  }

  if (goal.header.frame_id != global_frame_) {
    RCLCPP_INFO(
      node_->get_logger(), "Planner will only except goal position from %s frame",
      global_frame_.c_str());
    return global_path;
  }

  global_path.poses.clear();
  global_path.header.stamp = node_->now();
  global_path.header.frame_id = global_frame_;

  geometry_msgs::msg::PoseStamped start_i = start;
  for (auto goal_i : goals)
  {
    // calculating the number of loops for current value of interpolation_resolution_
    int total_number_of_loop = std::hypot(
      goal_i.pose.position.x - start_i.pose.position.x,
      goal_i.pose.position.y - start_i.pose.position.y) /
      interpolation_resolution_;
    double x_increment = (goal_i.pose.position.x - start_i.pose.position.x) / total_number_of_loop;
    double y_increment = (goal_i.pose.position.y - start_i.pose.position.y) / total_number_of_loop;

    for (int i = 0; i < total_number_of_loop; ++i) {
      geometry_msgs::msg::PoseStamped pose;
      pose.pose.position.x = start_i.pose.position.x + x_increment * i;
      pose.pose.position.y = start_i.pose.position.y + y_increment * i;
      pose.pose.position.z = 0.0;
      pose.pose.orientation.x = 0.0;
      pose.pose.orientation.y = 0.0;
      pose.pose.orientation.z = 0.0;
      pose.pose.orientation.w = 1.0;
      pose.header.stamp = node_->now();
      pose.header.frame_id = global_frame_;
      global_path.poses.push_back(pose);
    }
    start_i = goal_i;
  }
  global_path.poses.push_back(goal);

  return global_path;

其余方法虽然未被使用，但必须重写。这里按规则对它们进行了重写，但函数体留空。

2- 导出规划器插件

创建自定义规划器后，需要导出规划器插件，使规划器服务器能够发现它。插件在运行时加载，如果未正确导出，规划器服务器将无法加载。在 ROS 2 中，插件的导出和加载由 pluginlib 处理。

在本教程中，类 nav2_straightline_planner::StraightLine 将作为基类 nav2_core::GlobalPlanner 的子类被动态加载。

导出规划器需要以下两行代码：

  #include "pluginlib/class_list_macros.hpp"
  PLUGINLIB_EXPORT_CLASS(nav2_straightline_planner::StraightLine, nav2_core::GlobalPlanner)

这里需要 pluginlib 来导出插件类。pluginlib 提供了宏 PLUGINLIB_EXPORT_CLASS，由它完成所有导出工作。

建议将这些代码放在文件末尾，但放在文件顶部也可以。

下一步在包的根目录中创建插件描述文件。例如本教程包中的 global_planner_plugin.xml 文件，该文件包含以下信息：

library path：插件库的名称及其位置。
class name：类的名称（可选）。如果未设置，将默认为 class type。
class type：类的类型。
base class：基类的名称。
description：插件的描述。

  <library path="nav2_straightline_planner_plugin">
    <class type="nav2_straightline_planner::StraightLine" base_class_type="nav2_core::GlobalPlanner">
      <description>This is an example plugin which produces straight path.</description>
    </class>
  </library>

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

  pluginlib_export_plugin_description_file(nav2_core global_planner_plugin.xml)

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

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

编译后插件即被注册。可通过以下命令验证是否注册成功：

  $ ros2 plugin list

你应该会看到类似于下面的输出：

  nav2_straightline_planner:
        Plugin(name='nav2_straightline_planner::StraightLine', type='nav2_straightline_planner::StraightLine', base='nav2_core::GlobalPlanner')

接下来使用该插件。

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

要启用插件，需修改 nav2_params.yaml 文件，将原有参数替换为：

注意：对于 Galactic 及更高版本，plugin_names 和 plugin_types 已被一个用于插件名称的 plugins 字符串向量取代。类型现在在 plugin_name 命名空间的 plugin: 字段中定义（例如 plugin: MyPlugin::Plugin）。代码块中的内联注释可帮助理解。

  planner_server:
    ros__parameters:
      plugins: ["GridBased"]
      GridBased:
        plugin: "nav2_navfn_planner::NavfnPlanner" # For Foxy and later. In Iron and older versions, "/" was used instead of "::"
        tolerance: 2.0
        use_astar: false
        allow_unknown: true

替换为

  planner_server:
    ros__parameters:
      plugins: ["GridBased"]
      GridBased:
        plugin: "nav2_straightline_planner::StraightLine"
        interpolation_resolution: 0.1

在上面的代码中，可以看到 nav2_straightline_planner::StraightLine 规划器映射到其 ID GridBased。要传递插件专属参数，使用 <plugin_id>.<plugin_specific_parameter> 格式。

4- 运行 StraightLine 插件

运行启用 Nav2 的 TurtleBot3 仿真。详细步骤请参阅快速入门，快捷命令如下：

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

然后进入 RViz，点击顶部的“2D Pose Estimate”按钮，按照快速入门中的说明在地图上指定位置。机器人完成定位后，点击“Nav2 Goal”，再点击目标位姿。规划器将规划路径，机器人随即开始向目标移动。