编写新的 Costmap2D 插件

概述
要求
教程步骤

概述

本教程将介绍如何为 Costmap2D 创建自己的简单插件。

在开始本教程之前，请先查看此 video，其中包含有关 Costmap2D 图层设计和插件基本操作原理的信息。

要求

假设 ROS 2、Gazebo 和 TurtleBot3 软件包已在本地安装或构建。请确保 Nav2 项目也在本地构建，因为它是在构建与安装中创建的。

教程步骤

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，用于在插件中使用 costmap 图层。这些方法在运行时由“LayeredCostmap”调用。下表列出了方法列表、方法说明以及在插件代码中使用这些方法的必要性：

Virtual method	Method description	Requires override?
onInitialize()	Method is called at the end of plugin initialization. There is usually declarations of ROS parameters. This is where any required initialization should occur.	No
updateBounds()	Method is called to ask the plugin: which area of costmap layer it needs to update. The method has 3 input parameters: robot position and orientation, and 4 output parameters: pointers to window bounds. These bounds are used for performance reasons: to update the area inside the window where new info is available, avoiding updates of the whole costmap on every iteration.	Yes
updateCosts()	Method is called each time when costmap re-calculation is required. It updates the costmap layer only within its bounds window. The method has 4 input parameters: calculation window bounds, and 1 output parameter: reference to a resulting costmap `master_grid`. The `Layer` class provides the plugin with an internal costmap, `costmap_`, for updates. The `master_grid` should be updated with values within the window bounds using one of the following update methods: `updateWithAddition()`, `updateWithMax()`, `updateWithOverwrite()` or `updateWithTrueOverwrite()`.	Yes
matchSize()	Method is called each time when map size was changed.	No
onFootprintChanged()	Method is called each time when footprint was changed.	No
reset()	It may have any code to be executed during costmap reset.	Yes

在我们的示例中，这些方法具有以下功能：

GradientLayer::onInitialize() 包含 ROS 参数及其默认值的声明：

declareParameter("enabled", rclcpp::ParameterValue(true));
node_->get_parameter(name_ + "." + "enabled", enabled_);

并设置“need_recalculation_”边界重新计算指标：

need_recalculation_ = false;

GradientLayer::updateBounds() 如果“need_recalculation_”为“true”，则重新计算窗口边界，并且不管“need_recalculation_”值如何都更新它们。
GradientLayer::updateCosts() - 在此方法中，梯度直接写入生成的 costmap“master_grid”，而不与之前的层合并。这相当于使用内部“costmap_”，然后调用“updateWithTrueOverwrite()”方法。以下是 master costmap 的梯度生成算法：

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

其中，“梯度大小”是地图单元中每个梯度周期的大小，“梯度因子”是每一步代价地图值的减少量：

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

GradientLayer::onFootprintChanged() just resets need_recalculation_ value.
GradientLayer::reset() 方法是虚拟的：它未在本示例插件中使用。它保留在那里，因为父类“Layer”中的纯虚拟函数“reset()”需要被覆盖。

2- 导出并制作 GradientLayer 插件

编写的插件将在运行时作为其基本父类加载，然后由插件处理模块调用（对于 costmap2d，由“LayeredCostmap”调用）。Pluginlib 在运行时打开给定的插件，并提供可调用的导出类的方法。类导出机制告诉 pluginlib 在这些调用期间应使用哪个基本类。这允许通过插件扩展应用程序，而无需了解应用程序源代码或重新编译它。

在我们的示例中，“nav2_gradient_costmap_plugin::GradientLayer”插件的类应动态加载为“nav2_costmap_2d::Layer”基本类。为此，插件应按如下方式注册：

插件的类应该用加载类的基本类型注册。为此，应该将一个特殊的宏“PLUGINLIB_EXPORT_CLASS”添加到组成插件库的任何源文件中：

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

This part is usually placed at the end of cpp-file where the plugin class was written (in our example gradient_layer.cpp). It is good practice to place these lines at the end of the file, but technically, you can also place at the top.

插件的信息应存储到插件的描述文件中。这是通过在插件包中使用单独的 XML（在我们的示例中为“gradient_plugins.xml”）来完成的。此文件包含以下信息：

path: Path and name of library where plugin is placed.

name: Plugin type referenced in plugin_types parameter (see next section for more details). It could be whatever you want.

type: Plugin class with namespace taken from the source code.

basic_class_type: Basic parent class from which plugin class was derived.

description: Plugin description in a text form.

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

插件的导出是通过将“pluginlib_export_plugin_description_file()”cmake 函数包含到“CMakeLists.txt”中来执行的。此函数将插件描述文件安装到“share”目录中，并为插件描述 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”）并在必要时获取 setup.bash 文件。

现在该插件已可供使用。

3- 在 Costmap2D 中启用插件

下一步需要告诉 Costmap2D 有关新插件的信息。为此，应该将插件添加到“nav2_params.yaml”中的“plugin_names”和“plugin_types”列表中，或者为“local_costmap”/“global_costmap”添加，以便在运行时为 Controller/Planner Server 启用。“plugin_names”列表包含插件对象的名称。这些名称可以是任何你想要的名字。“plugin_types”包含“plugin_names”对象中列出的类型。这些类型应该对应于插件描述 XML 文件中指定的插件类的“name”字段。

Note

对于 Galactic 或更高版本，“plugin_names”和“plugin_types”已被替换为插件名称的单个“plugins”字符串向量。类型现在在“plugin:”字段中的“plugin_name”命名空间中定义（例如“plugin: MyPlugin::Plugin”）。代码块中的内联注释将帮助您完成此操作。

For example:

--- 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
       use_sim_time: True
-      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 文件还可能包含每个插件的参数列表（如果有），由插件对象名称标识。

NOTE: 同一类型的插件对象可能会同时加载多个。为此，“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
  ...

4- 运行 GradientLayer 插件

运行启用 Nav2 的 Turtlebot3 模拟。详细的操作说明写在入门指南。下面是快捷命令：

$ ros2 launch nav2_bringup tb3_simulation_launch.py

然后转到 RViz 并单击顶部的“2D Pose Estimate”按钮，并按照入门指南中所述指向地图上的位置。机器人将在地图上定位，结果应如下图所示。在那里，可以看到梯度代价地图。还有 2 件值得注意的事情：通过 GradientLayer::updateCosts() 动态更新其范围内的代价地图和通过梯度弯曲的全局路径：