ROS2从入门到精通5-1：详解代价地图与costmap插件编写(以距离场ESDF为例)

0 专栏介绍

本专栏旨在通过对ROS2的系统学习，掌握ROS2底层基本分布式原理，并具有机器人建模和应用ROS2进行实际项目的开发和调试的工程能力。

🚀详情：《ROS2从入门到精通》

1 代价地图介绍

1.1 基本概念

机器人导航必须依赖于地图，而SLAM构建的地图为静态地图，在导航中一般不可以直接使用，因为导航过程中障碍信息是可变的，因此地图信息需要时时更新。

在这里插入图片描述

代价地图就是ROS定义的用于动态导航的地图数据结构，其在静态地图基础上添加了一些辅助信息，主要分为以下图层：

静态地图层(Static Map Layer)：通常是由SLAM建立的静态地图
障碍地图层(Obstacle Map Layer)：用于动态记录传感器感知到的障碍信息
膨胀层(Inflation Layer)：在以上两层地图基础上进行障碍膨胀，主要目的是防止机器人靠近障碍物边缘时，因惯性、不规则形体等原因与障碍产生碰撞，因此需要让机器人充分远离障碍物
其他图层(Other Layers)：可以通过插件形式自定义代价地图层

1.2 代价定义

关于代价地图代价的定义，摘录官方说明：如图所示，横轴是距离机器人中心的距离，纵轴是代价地图中栅格的灰度值，灰度越高代价越大

致命障碍：栅格值254，此时障碍物与机器人中心重叠，必然发生碰撞
内切障碍：栅格值253，此时障碍物处于机器人的内切圆内，必然发生碰撞
外切障碍：栅格值[128,252]，此时障碍物处于其机器人的外切圆内，处于碰撞临界，不一定发生碰撞
非自由空间：栅格值(0,127]，此时机器人处于障碍物附近，属于危险警戒区，进入此区域，将来可能会发生碰撞
自由区域：栅格值0，此处机器人可以自由通过
未知区域：栅格值255，未探明是否有障碍物

在这里插入图片描述
看一个实例：其中紫色区域为致命障碍；浅蓝色区域为内切障碍；红色区域为外切障碍与非自由障碍空间，颜色越深代价越高；蓝色为自由空间。

在这里插入图片描述

2 代价地图配置

2.1 通用配置

通用配置如下所示：

ros__parameters:
	update_frequency: 1.0
	publish_frequency: 1.0
	global_frame: map
	robot_base_frame: base_link
	use_sim_time: True
	robot_radius: 0.22
	resolution: 0.05
	track_unknown_space: true
	plugins: ["static_layer", "obstacle_layer", "inflation_layer"]

global_frame：在全局/局部代价地图中的全局坐标系，一般全局设置为地图坐标系map，局部设置为里程计坐标系odom
robot_base_frame:机器人基坐标系通过global_frame和robot_base_frame就可以计算两个坐标系间的变换，得知机器人在代价地图位置坐标
update_frequency：代价地图的更新频率
publish_frequency：代价地图的发布频率
robot_radius：机器人等效半径，单位是米

2.2 障碍层配置

obstacle_layer:
  plugin: "nav2_costmap_2d::ObstacleLayer"
  observation_sources: scan
  scan:
    topic: /scan
    max_obstacle_height: 2.0
    clearing: True
    marking: True
    data_type: "LaserScan"
    raytrace_max_range: 3.0
    raytrace_min_range: 0.0
    obstacle_max_range: 2.5
    obstacle_min_range: 0.0

observation_sources：设置导航中所使用的传感器，例如激光雷达、碰撞传感器、超声波传感器等。每个传感器需要进行配置：
- sensor_frame：传感器坐标系名称
- data_type：传感器数据类型
- topic：传感器发布的话题名
- marking：是否可使用该传感器来标记障碍物
- clearing：是否可使用该传感器来清除障碍物标记
obstacle_range：设置机器人检测障碍物的最大范围，只有进入该范围内才把该障碍物当作影响路径规划和移动的障碍物

2.3 静态层配置

static_layer:
  plugin: "nav2_costmap_2d::StaticLayer"
  map_subscribe_transient_local: True

2.4 膨胀层配置

inflation_layer:
  plugin: "nav2_costmap_2d::InflationLayer"
  cost_scaling_factor: 3.0
  inflation_radius: 0.55

inflation_radius：膨胀半径，膨胀层会把障碍物代价膨胀直到该半径为止，一般将该值设置为机器人底盘的直径大小。如果机器人经常撞到障碍物就需要增大该值，若经常无法通过狭窄地方就减小该值，代价膨胀公式：

exp(-1.0 * cost_scaling_factor * (distance_from_obstacle - inscribed_radius)) * (INSCRIBED_INFLATED_OBSTACLE - 1)

其中，distance_from_obstacle - inscribed_radius是机器人到实际障碍物与内切圆半径之差(且该差值小于膨胀半径)，INSCRIBED_INFLATED_OBSTACLE设定为254
cost_scaling_factor：膨胀过程代价比例系数，增大比例因子会降低代价

3 代价地图插件案例

然而，这些默认的图层可能并不能满足实际需要。举例而言，社交地图层(Social Costmap Layer)用于机器人在导航过程中考虑与人类之间的社交交互，这个层级允许机器人更加智能地导航，并遵循一些社交规则，以更好地与人类共享空间，避免产生不适或危险的行为；禁区地图层(Prohibition Costmap Layer)用于标记和表示机器人不能进入的区域。这个层级的作用是在机器人的导航过程中限制其进入特定的区域(例如高压区、悬崖边缘、深水区等)，从而确保机器人在导航时遵守特定的规则和限制，防止可能的事故和损坏。

3.1 构造地图插件类

所有全局规划插件的基类是nav2_costmap_2d::Layer，该基类提供了7个纯虚方法来实现控制器插件，一个合法的控制插件必须覆盖这7个基本方法：

onInitialize()非必须覆盖：在插件初始化结束时调用。通常会在方法里声明ROS参数。任何需要初始化的操作都应该在该方法里完成。
updateBounds()必须覆盖：更新costmap层边界
updateCosts()必须覆盖：每次需要重新计算costmap时都会调用方法。它仅在其边界窗口内更新costmap层
matchSize()非必须覆盖：在每次更改地图大小时调用更新地图尺寸
onFootprintChanged()非必须覆盖：在每次更新机器人footprint位置时调用
reset()必须覆盖：重置costmap层
isClearable()必须覆盖：是否需要在该costmap层执行清除操作

按照上述标准，本文案例中ESDF地图插件的基本成员函数和变量如下所示

class DistanceLayer : public Layer
{
public:
  DistanceLayer() = default;
  virtual ~DistanceLayer() = default;

  void onInitialize() override;
  void updateBounds(double robot_x, double robot_y, double robot_yaw, double* min_x, double* min_y, double* max_x,
                    double* max_y) override;
  void updateCosts(nav2_costmap_2d::Costmap2D& master_grid, int min_i, int min_j, int max_i, int max_j) override;
  void reset() override;
  bool isClearable() override;
};

3.2 注册并导出插件

在创建了自定义地图插件的前提下，需要导出该控制器插件以便地图服务器可以在运行时正确地加载。在ROS2中，插件的导出和加载由pluginlib处理。

源文件配置导出宏

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

配置插件描述文件xxx_costmap_plugin.xml，例如本案例为distance_layer_costmap_plugin.xml文件。此XML文件包含以下信息：
- library path：插件库名称及其位置；
- class name：地图算法类的名称；
- class type：地图算法类的类型；
- base class：地图层基类的名称，统一为nav2_costmap_2d::Layer
- description：插件的描述。
实例如下
```
<library path="esdf_plugin">
	<class name="nav2_costmap_2d/DistanceLayer" type="nav2_costmap_2d::DistanceLayer" base_class_type="nav2_costmap_2d::Layer">
	  <description>This is a nav2 distance layer plugin.</description>
	</class>
</library>
```
配置CMakeLists.txt文件
使用cmake函数pluginlib_export_plugin_description_file()来导出插件。这个函数会将插件描述文件安装到install/share目录中，并设置ament索引以使其可被发现，实例如下
```
pluginlib_export_plugin_description_file(nav2_costmap_2d distance_layer_costmap_plugin.xml)
```

配置package.xml描述文件，实例如下：

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

3.3 编译与使用插件

编译该插件软件包，接着通过配置文件使用插件。

参数的传递链如下：首先在simulation.launch.py中引用配置文件navigation.yaml

declare_params_file_cmd = DeclareLaunchArgument(
	'params_file',
	default_value=os.path.join(simulation_dir, 'config', 'navigation.yaml'),
	description='Full path to the ROS2 parameters file to use for all launched nodes')

接着在navigation.yaml中修改插件配置，默认如下，是用的是静态层、障碍层和膨胀层插件：

plugins: ["static_layer", "obstacle_layer", "inflation_layer"]
	obstacle_layer:
	  plugin: "nav2_costmap_2d::ObstacleLayer"
	  ...
	static_layer:
	  plugin: "nav2_costmap_2d::StaticLayer"
	  ...
	inflation_layer:
	  plugin: "nav2_costmap_2d::InflationLayer"
	  ...

将上述替换为自己的插件，本案例为：

plugins: ["static_layer", "obstacle_layer", "inflation_layer", "distance_layer"]
	distance_layer:
	  plugin: "nav2_costmap_2d/DistanceLayer"
	  ...
  obstacle_layer:
	  plugin: "nav2_costmap_2d::ObstacleLayer"
	  ...
	static_layer:
	  plugin: "nav2_costmap_2d::StaticLayer"
	  ...
	inflation_layer:
	  plugin: "nav2_costmap_2d::InflationLayer"
	  ...