0 专栏介绍

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

1 为什么需要代价地图插件？

在ROS从入门到精通6-1：图文详解代价地图costmap原理与配置中，我们指出代价地图就是ROS定义的用于动态导航的地图数据结构，其在静态地图基础上添加了一些辅助信息，机器人导航必须依赖于地图，而静态地图在导航中一般不可以直接使用，因为导航过程中障碍信息是可变的，地图信息需要实时更新。

ROS从入门到精通6-8：costmap代价地图插件编写案例(prohibition_layer)-LMLPHP

在costmap_2d中，默认实现了代价地图的以下图层：

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

ROS从入门到精通6-8：costmap代价地图插件编写案例(prohibition_layer)-LMLPHP

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

本文就以实际案例介绍如何编写一个代价地图插件，关于插件开发请参考ROS从入门到精通5-3：ROS插件库与开发

2 自定义代价地图插件

首先创建功能包prohibition_layer用于生成自定义代价地图插件，接着按以下步骤执行

构造基类：由于代价地图插件继承于costmap_2d功能包的BaseGlobalPlanner类，因此无需构造

构造插件类：在prohibition_layer/include中新建prohibition_layer.h，继承自基类costmap_2d::Layer，重点实现其中的onInitialize、updateBounds与updateCosts接口

class ProhibitionLayer : public costmap_2d::Layer
{
public:

ProhibitionLayer();
virtual ~ProhibitionLayer();

virtual void onInitialize();

virtual void updateBounds(double robot_x, double robot_y, double robot_yaw, 
 double *min_x, double *min_y, double *max_x, double *max_y);

virtual void updateCosts(costmap_2d::Costmap2D& master_grid, int min_i, int min_j,
                       int max_i, int max_j);

其中onInitialize是代价地图为用户提供的初始化接口；updateBounds计算插件图层要更新到主图层区域的大小；updateCosts将每个图层的代价值更新到主图层。

注册插件：在prohibition_layer/src中新建prohibition_layer.cpp使用PLUGINLIB_EXPORT_CLASS宏注册插件并实现上述接口，限于篇幅不列出完整代码

#include <prohibition_layer.h>
#include <pluginlib/class_list_macros.h>

PLUGINLIB_EXPORT_CLASS(prohibition_layer::ProhibitionLayer, costmap_2d::Layer)

using costmap_2d::LETHAL_OBSTACLE;

namespace prohibition_layer
{

void ProhibitionLayer::onInitialize()
{
  ros::NodeHandle nh("~/" + name_);
  current_ = true;

 ...
 
  std::string params = "prohibition_areas";
  if (!parseProhibitionListFromYaml(&nh, params))
    ROS_ERROR_STREAM("Reading prohibition areas from '" << nh.getNamespace() << "/" << params << "' failed!");
  
  _fill_polygons = true;
  nh.param("fill_polygons", _fill_polygons, _fill_polygons);
  
  // compute map bounds for the current set of prohibition areas.
  computeMapBounds();
  
  ROS_INFO("ProhibitionLayer initialized.");
}
...

构建插件库.so：编译此功能包prohibition_layer将会在根目录devel/lib中生成插件libprohibition_layer.so

集成插件库到ROS：在功能包prohibition_layer下创建prohibition_layer_plugin.xml描述插件信息和库路径

<library path="lib/libprohibition_layer">
  <class type="prohibition_layer::ProhibitionLayer" base_class_type="costmap_2d::Layer">
    <description>ROS-Package that implements a costmap layer to add prohibited areas to the costmap-2D by a user configuration.</description>
  </class>
</library>

使用插件：在参数配置文件夹中找到global_costmap_params.yaml和local_costmap_params.yaml，在末尾添加或修改

plugins:
  - {name: static_map,        type: "costmap_2d::StaticLayer"}
  - {name: obstacles,         type: "costmap_2d::VoxelLayer"}
  - {name: prohibition_layer, type: "prohibition_layer::ProhibitionLayer"}
  - {name: inflation_layer,   type: "costmap_2d::InflationLayer"}

3 仿真测试

在config目录新建prohibition_areas.yaml

prohibition_areas:
  #定义一个禁止点
  - [6.09, -1.388]
  # 定义一个禁止通行的线
  - [[7.33, 2.11],
    [7.26, 0.11]]
  # 定义一个禁止通行的区域
  - [[5.33, 1.11],
    [5.26, -1.11],
    [3, 1]]

这里我们设置了一个点、一条线和一个三角形

接着在move_base.launch文件加载参数

<node pkg="move_base" type="move_base" respawn="false" name="move_base" output="screen">
	...
    <!-- 加载导航功能参数 -->
    <rosparam file="$(find gazebo_scene)/config/costmap_common_params.yaml" command="load" ns="global_costmap" />
    <rosparam file="$(find gazebo_scene)/config/costmap_common_params.yaml" command="load" ns="local_costmap" />
    <rosparam file="$(find gazebo_scene)/config/prohibition_areas.yaml" command="load" ns="global_costmap/prohibition_layer" />
	...
</node>

假设我们的实际场景如下

ROS从入门到精通6-8：costmap代价地图插件编写案例(prohibition_layer)-LMLPHP

🔥 更多精彩专栏：

《ROS从入门到精通》
《Pytorch深度学习实战》
《机器学习强基计划》
《运动规划实战精讲》
…

Mr.Winter`

ROS从入门到精通6-8：costmap代价地图插件编写案例(prohibition_layer)

目录

0 专栏介绍

1 为什么需要代价地图插件？

2 自定义代价地图插件

3 仿真测试