目录
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，局部设置为里程计坐标系odomrobot_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设定为254cost_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::Layerdescription：插件的描述。
实例如下
<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"
...
接着运行即可看到距离层地图被发布
完整代码通过下方博主名片联系获取
🔥 更多精彩专栏：
