< >
Home » ROS2与Navigation2入门教程 » ROS2与Navigation2入门教程-编写新的Costmap2D插件

ROS2与Navigation2入门教程-编写新的Costmap2D插件

说明：

  • 介绍如何编写新的Costmap2D插件

概述

  • 本教程将会展示如何创建您自己的Costmap2D简单插件。

  • 开始本教程之前，请观看这个视频，该视频含有Costmap2D图层设计和插件基本操作原理的相关信息。

要求

  • 要求在本地机器上已经安装或构建好了ROS 2、Gazebo和TurtleBot3 软件包。请确保Navigation2项目也是在本地构建的

具体步骤

  • 编写一个新的Costmap2D插件

  • 为了演示目的，本示例会创建一个成本地图插件，该插件会将重复的成本梯度放入到成本地图中。

  • 本教程的注释代码可以在资源库navigation2_tutorials的ROS 2软件包nav2_gradient_costmap_plugin中找到。

  • 在创建自己的Costmap2D图层插件时可以参考该代码

  • 插件类nav2_gradient_costmap_plugin::GradientLayer是继承自基本类nav2_costmap_2d::Layer的：

namespace nav2_gradient_costmap_plugin
{
class GradientLayer : public nav2_costmap_2d::Layer

  • 该基本类提供了一组虚拟方法API，用于在插件中处理成本地图图层。

  • 这些方法在运行时由LayeredCostmap调用。

  • 这些方法的列表、它们的简介以及在插件代码中使用这些方法的必要性如下表所示：

虚拟方法方法简介是否要求覆写
onInitialize()插件初始化结束时调用该方法。通常有ROS参数的声明。任何需要的初始化都应该在该方法进行。否
updateBounds()调用该方法以询问插件需要更新成本地图图层的哪个区域。该方法有表示机器人位置和方向的3个输入参数及表示窗口边界指针的4个输出参数。这些边界用于提高性能—更新窗口内有可用新信息的区域，避免在每次迭代时更新整个成本地图。是
updateCosts()每次需要重新计算成本地图时调用该方法。该方法仅更新边界窗口内的成本图层。该方法有用于计算窗口边界的4个输入参数和引用结果成本地图master_grid的1个输出参数。Layer类为插件提供一个内部成本地图costmap_用于更新。应使用以下更新方法之一用窗口边界内的值更新 master_grid：updateWithAddition(), updateWithOverwrite(), updateWithMax()或updateWithTrueOverwrite()。是
matchSize()每次更改地图尺寸时都会调用该方法。否
onFootprintChange()每次更改足迹时都会调用该方法。否
reset()在成本地图重置期间，可以有任何要执行的代码。是

  • 在本示例中，这些方法具有以下功能：

  • GradientLayer::onInitialize()方法包含具有默认值的ROS参数申明：

declareParameter("enabled", rclcpp::ParameterValue(true));
node_->get_parameter(name_ + "." + "enabled", enabled_);
  • 并设置边界重新计算指标need_recalculation_的值：
need_recalculation_ = false;

  • 若need_recalculation_为true，则GradientLayer::updateBounds()方法会重新计算窗口边界并更新它们，而不管need_recalculation_值。

  • 在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表示每一步成本地图值的递减量，
  • 如下图所示：

请输入图片描述

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

  • GradientLayer::onFootprintChanged()方法只是重置 need_recalculation_值

  • GradientLayer::reset()方法是虚拟的，在本示例插件中没有使用它。该方法保留在这里，是因为需要覆盖父类Layer中的纯虚拟函数reset()

导出并制作GradientLayer插件

  • 与其基本父类相似，编写的插件会在运行时加载，然后由插件处理模块调用（对于costmap2d插件而言是由LayeredCostmap调用的）。插件库在运行时会打开一个给定的插件，并从导出的类中提供可调用的方法。

  • 类导出机制会告知插件库在这些调用期间应该使用哪个基本类。这样允许在不知道应用程序源代码或无需重新编译该应用程序的情况下通过插件扩展应用程序。

  • 在本示例中，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)

  • 这两行代码通常会被放置在所编写插件类的cpp文件（本示例中为 gradient_layer.cpp）的末尾。

  • 将这两行放在文件末尾是一种很好的做法，但从技术上来说也可以放在cpp文件的顶部。

  • 插件信息应该存储到插件描述文件中。这是通过在插件软件包中使用单独的XML文件（在本示例中为gradient_plugins.xml）来完成的。

  • 该XML文件包含以下信息：

    • path：插件所在库的路径和名称。

    • name：在plugin_types参数中引用的插件类型（有关详细信息，请参阅下一节）。可以是你想要的任何类型。

    • type：具有命名空间的插件类，该命名空间取自源代码。

    • basic_class_type：派生插件类的基本父类。

    • description：文本形式的插件描述。

<library path="nav2_gradient_costmap_plugin_core">
  <class name="nav2_gradient_costmap_plugin/GradientLayer" 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-function包含到CMakeLists.txt中来执行的。这个函数会将插件描述文件安装到share目录中，并为插件描述XML设置ament索引以作为所选类型的插件可被发现：
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" />

  • 做完这一切后，将插件软件包放入某个ROS 2工作空间的src目录中，必要时构建该插件软件包（colcon build --packages-select nav2_gradient_costmap_plugin --symlink-install），并对该工作空间install目录下的setup.bash文件进行source。

  • 现在该插件准备就绪可以使用了。

在Costmap2D中启用插件

  • 下一步需要将新插件告知Costmap2D。为此，应将该插件添加到 nav2_params.yaml文件的plugin_names和plugin_types列表中，可以选择放在local_costmap和/或global_costmap中，以便在运行时为控制器/规划器服务器启用该插件。plugin_names列表包含插件对象的名称。这些名称可以是您想要的任何名称。plugin_types包含plugin_names对象中列出的类型。这些类型应该对应于插件描述XML文件中指定的插件类的name字段。

  • 对于Galactic或更高版本，plugin_names和plugin_types已替换为插件名称的单个plugins字符串向量。这些类型现在是在 plugin:字段的plugin_name命名空间中定义的（例如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
-      plugin_names: ["obstacle_layer", "voxel_layer", "inflation_layer"] # For Eloquent and earlier
-      plugin_types: ["nav2_costmap_2d::ObstacleLayer", "nav2_costmap_2d::VoxelLayer",     "nav2_costmap_2d::InflationLayer"] # For Eloquent and earlier
+      plugin_names: ["obstacle_layer", "voxel_layer", "gradient_layer"] # For Eloquent and earlier
+      plugin_types: ["nav2_costmap_2d::ObstacleLayer", "nav2_costmap_2d::VoxelLayer",     "nav2_gradient_costmap_plugin/GradientLayer"] # For Eloquent and earlier
-      plugins: ["obstacle_layer", "voxel_layer", "inflation_layer"] # For Foxy and later
+      plugins: ["obstacle_layer", "voxel_layer", "gradient_layer"] # For Foxy and later
       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
-      plugin_names: ["static_layer", "obstacle_layer", "voxel_layer", "inflation_layer"] # For Eloquent and earlier
-      plugin_types: ["nav2_costmap_2d::StaticLayer", "nav2_costmap_2d::ObstacleLayer",     "nav2_costmap_2d::VoxelLayer", "nav2_costmap_2d::InflationLayer"] # For Eloquent and earlier
+      plugin_names: ["static_layer", "obstacle_layer", "voxel_layer", "gradient_layer"] # For Eloquent and     earlier
+      plugin_types: ["nav2_costmap_2d::StaticLayer", "nav2_costmap_2d::ObstacleLayer",     "nav2_costmap_2d::VoxelLayer", "nav2_gradient_costmap_plugin/GradientLayer"] # For Eloquent and earlier
-      plugins: ["static_layer", "obstacle_layer", "voxel_layer", "inflation_layer"] # For Foxy and later
+      plugins: ["static_layer", "obstacle_layer", "voxel_layer", "gradient_layer"] # For Foxy and later
       robot_radius: 0.22
       resolution: 0.05
       obstacle_layer:

  • YAML文件还可能包含每个插件的参数列表（如果有），由插件对象名称进行标识。

  • 请注意：可能会同时加载多个同一种类型的插件对象。

  • 为此， plugin_names列表应包含不同的插件名称，无论plugin_types是否保持相同的类型。

  • 例如：

plugin_names: ["obstacle_layer", "gradient_layer_1", "gradient_layer_2"] # For Eloquent and earlier
plugin_types: ["nav2_costmap_2d::ObstacleLayer", "nav2_gradient_costmap_plugin/GradientLayer",     "nav2_gradient_costmap_plugin/GradientLayer"] # For Eloquent and earlier
plugins: ["obstacle_layer", "gradient_layer_1", "gradient_layer_2"] # For Foxy and later
  • 在这种情况下，会通过YAML文件中各个插件对象自己的参数树来处理每个插件对象，如下所示：
gradient_layer_1:
  plugin: nav2_gradient_costmap_plugin/GradientLayer # For Foxy and later
  enabled: True
  ...
gradient_layer_2:
  plugin: nav2_gradient_costmap_plugin/GradientLayer # For Foxy and later
  enabled: False
  ...

运行GradientLayer插件

  • 运行带有已启用Nav2的Turtlebot3机器人仿真。

  • 有关如何启用Nav2的详细说明请参阅“开始使用Nav2”教程。

  • 下面是运行该仿真的快捷命令：

$ ros2 launch nav2_bringup tb3_simulation_launch.py

  • 然后进入到RViz中并单击顶部的“2D Pose Estimate”按钮，并按照“开始使用Nav2”教程所介绍的那样在地图上点击机器人的初始位置。机器人将会在地图上定位，且结果应该如下图所示。

  • 这样可以会看到梯度成本地图。

  • 还可以注意到这两个事项：由GradientLayer::updateCosts()方法在其边界内动态更新成本地图，以及由梯度弯曲的全局路径。

请输入图片描述

参考：

  • https://navigation.ros.org/plugin_tutorials/docs/writing_new_costmap2d_plugin.html

纠错,疑问,交流: 请进入讨论区或 点击加入Q群

获取最新文章: 扫一扫右上角的二维码加入“创客智造”公众号


标签: ros2与navigation2入门教程