编写新的 Costmap2D 插件

概述（Overview）

本教程演示如何为 Costmap2D 创建一个简单的自定义 插件（plugin）。

在开始之前，建议先观看这个 视频，其中介绍了 Costmap2D 图层设计及插件的基本工作原理。

环境要求（Requirements）

假设 ROS 2、Gazebo 和 TurtleBot3 软件包已安装或已在本地构建，同时确保 Nav2 项目已按构建说明在本地编译完成。

教程步骤（Tutorial Steps）

1- 编写一个新的 Costmap2D 插件

作为演示，本例将创建一个在代价地图中生成重复代价梯度的代价地图插件。 本教程的带注释源码位于 navigation2_tutorials 仓库的 nav2_gradient_costmap_plugin ROS 2 软件包中，开发自己的 Costmap2D 图层插件时可作参考。

插件类 nav2_gradient_costmap_plugin::GradientLayer 继承自基类 nav2_costmap_2d::Layer：

  namespace nav2_gradient_costmap_plugin
  {

  class GradientLayer : public nav2_costmap_2d::Layer

基类提供了一套虚方法 API，用于处理代价地图图层。这些方法在运行时由 LayeredCostmap 调用。下表列出了各方法的功能描述，以及是否需要在插件代码中实现：

虚方法（Virtual method）	方法描述	需要重写？
onInitialize()	在插件初始化结束时调用。通常在此声明 ROS 参数，执行所需的初始化操作。	否
updateBounds()	询问代价地图层需要更新的区域范围。该方法有 3 个输入参数（机器人位置和朝向）和 4 个输出参数（指向窗口边界的指针）。设置边界是为了提高性能——只更新窗口内有新信息的区域，避免每次迭代都更新整个代价地图。	是
updateCosts()	在需要重新计算代价地图时调用，仅在其边界窗口内更新代价地图层。该方法有 4 个输入参数（计算窗口边界）和 1 个输出参数（结果代价地图 master_grid 的引用）。Layer 类为插件提供了一个内部代价地图 costmap_ 供更新使用。master_grid 应使用以下方法之一在窗口边界内更新数值：updateWithAddition()、updateWithMax()、updateWithOverwrite() 或 updateWithTrueOverwrite()。	是
matchSize()	在地图大小发生改变时调用。	否
onFootprintChanged()	在 footprint（足迹）发生改变时调用。	否
reset()	可包含代价地图重置时需要执行的代码。	是
isClearable()	指示该插件在清除操作期间是否需要处理。	是

在本示例中，各方法的功能如下：

1. GradientLayer::onInitialize() 包含一个带默认值的 ROS 参数声明：

  node->declare_or_get_parameter(name_ + "." + "enabled", true);

并设置边界重算标志 need_recalculation_：

   need_recalculation_ = false;

1. 当 need_recalculation_ 为 true 时，GradientLayer::updateBounds() 会重新计算窗口边界；无论 need_recalculation_ 取值如何，边界都会被更新。

2. GradientLayer::updateCosts()：将梯度直接写入结果代价地图 master_grid，不与之前的图层合并。这等效于先处理内部 costmap_，再调用 updateWithTrueOverwrite()。主代价地图的梯度生成算法如下：

  int gradient_index;
  for (int j = min_j; j < max_j; j++) {
    // Reset gradient_index each time when reaching the end of re-calculated window
    // by OY axis.
    gradient_index = 0;
    for (int i = min_i; i < max_i; i++) {
      int index = master_grid.getIndex(i, j);
      // setting the gradient cost
      unsigned char cost = (LETHAL_OBSTACLE - gradient_index*GRADIENT_FACTOR)%255;
      if (gradient_index <= GRADIENT_SIZE) {
        gradient_index++;
      } else {
        gradient_index = 0;
      }
      master_array[index] = cost;
    }
  }

其中 GRADIENT_SIZE 是每个梯度周期对应的地图单元格数，GRADIENT_FACTOR 是每一步代价值的递减量：

这些参数定义在插件的头文件中。

1. GradientLayer::onFootprintChanged() 仅重置 need_recalculation_。

2. GradientLayer::reset() 方法体为空——本示例插件未使用该方法，但由于父类 Layer 中 reset() 是纯虚函数，必须予以重写，因此保留空实现。

3. GradientLayer::isClearable() 返回 false，表示该插件不可清除。

2- 导出并制作 GradientLayer 插件

插件在运行时以基类的身份被加载，然后由插件处理模块（Costmap2D 对应的是 LayeredCostmap）调用。Pluginlib 负责在运行时打开给定插件，并调用导出类中的方法。类导出机制告知 pluginlib 在调用期间应使用哪个基类。这样就能在不了解应用程序源代码、无需重新编译的情况下，通过插件扩展应用程序功能。

本示例中，nav2_gradient_costmap_plugin::GradientLayer 应作为 nav2_costmap_2d::Layer 基类被动态加载。为此，需要按以下步骤注册插件：

1. 插件类必须以被加载时的基类类型进行注册。具体做法是在插件库的任一源文件中添加专用宏 PLUGINLIB_EXPORT_CLASS：

  #include "pluginlib/class_list_macros.hpp"
  PLUGINLIB_EXPORT_CLASS(nav2_gradient_costmap_plugin::GradientLayer, nav2_costmap_2d::Layer)

这段代码通常放在插件类 cpp 文件（本示例为 gradient_layer.cpp）的末尾。放在文件末尾是推荐做法，但从技术上讲，放在文件顶部也可以。

1. 插件信息需存储在插件描述文件中，即在插件软件包中创建一个独立的 XML 文件（本示例为 gradient_plugins.xml）。该文件包含以下信息：

• path：插件库的路径和名称。
• name：在 plugin_types 参数中引用的插件类型（详见下一节），可自行命名。
• type：源代码中带命名空间的插件类名。
• basic_class_type：插件类所继承的基类。
• description：插件的文本描述。

  <library path="nav2_gradient_costmap_plugin_core">
    <class type="nav2_gradient_costmap_plugin::GradientLayer" base_class_type="nav2_costmap_2d::Layer">
      <description>This is an example plugin which puts repeating costs gradients to costmap</description>
    </class>
  </library>

接下来在 CMakeLists.txt 中调用 pluginlib_export_plugin_description_file() 函数导出插件。该函数会将插件描述文件安装到 share 目录，并设置 ament 索引，使插件描述 XML 可被对应类型发现：

  pluginlib_export_plugin_description_file(nav2_costmap_2d gradient_layer.xml)

插件描述文件还需添加到 package.xml 中。costmap_2d 是接口定义所在的软件包（本例中对应 Layer），需要在其中指定 XML 文件的路径：

  <export>
    <costmap_2d plugin="${prefix}/gradient_layer.xml" />
    ...
  </export>

完成上述步骤后，将插件软件包放入 ROS 2 工作空间的 src 目录，执行构建（colcon build --packages-select nav2_gradient_costmap_plugin --symlink-install），并在需要时 source setup.bash。

插件现在即可使用。运行以下命令验证是否注册成功：

  $ ros2 plugin list

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

  nav2_gradient_costmap_plugin:
        Plugin(name='nav2_gradient_costmap_plugin::GradientLayer', type='nav2_gradient_costmap_plugin::GradientLayer', base='nav2_costmap_2d::Layer')

3- 在 Costmap2D 中启用插件

接下来需要让 Costmap2D 感知到新插件。将插件添加到 nav2_params.yaml 中的 plugin_names 和 plugin_types 列表中（可分别针对 local_costmap/global_costmap 配置），以便在运行时为控制器/规划器服务器启用。plugin_names 列表包含插件对象的名称，可自行命名；plugin_types 包含各对象对应的类型，需与插件描述 XML 文件中插件类的 name 字段一致。

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

例如：

--- a/nav2_bringup/bringup/params/nav2_params.yaml
+++ b/nav2_bringup/bringup/params/nav2_params.yaml
@@ -124,8 +124,8 @@ local_costmap:
       width: 3
       height: 3
       resolution: 0.05
-      plugins: ["obstacle_layer", "voxel_layer", "inflation_layer"]
+      plugins: ["obstacle_layer", "voxel_layer", "gradient_layer"]
       robot_radius: 0.22
       inflation_layer:
         cost_scaling_factor: 3.0
@@ -171,8 +171,8 @@ global_costmap:
       robot_base_frame: base_link
       global_frame: map
-      plugins: ["static_layer", "obstacle_layer", "voxel_layer", "inflation_layer"]
+      plugins: ["static_layer", "obstacle_layer", "voxel_layer", "gradient_layer"]
       robot_radius: 0.22
       resolution: 0.05
       obstacle_layer:

YAML 文件还可以包含每个插件的参数列表（如果有的话），以插件对象名称作为标识。

同一类型可同时加载多个插件对象。此时 plugin_names 列表中需使用不同的名称，而 plugin_types 保持相同类型。例如：

  plugins: ["obstacle_layer", "gradient_layer_1", "gradient_layer_2"]

此时，每个插件对象在 YAML 文件中都有各自的参数树，例如：

  gradient_layer_1:
    plugin: nav2_gradient_costmap_plugin::GradientLayer # In Iron and older versions, "/" was used instead of "::"
    enabled: True
    ...
  gradient_layer_2:
    plugin: nav2_gradient_costmap_plugin::GradientLayer # In Iron and older versions, "/" was used instead of "::"
    enabled: False
    ...

注意：插件在配置中列出的顺序很重要，它决定了插件应用到代价地图上的先后顺序。例如，如果膨胀层（inflation layer）排在测距层（range layer）之前，那么测距层添加到代价地图中的障碍物将不会被膨胀。

4- 运行 GradientLayer 插件

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

  $ ros2 launch nav2_bringup tb3_simulation_launch.py

然后进入 RViz，点击顶部的“2D Pose Estimate”按钮，按 快速入门 中描述的方式在地图上指定位置。机器人完成定位后，结果应如下图所示，图中可以看到梯度代价地图。还有两个值得注意的现象：GradientLayer::updateCosts() 动态更新其边界内的代价地图，以及全局路径被梯度弯曲：