0 专栏介绍

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

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

---

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

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

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

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

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

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

2 自定义代价地图插件

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

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

2. 构造插件类：在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将每个图层的代价值更新到主图层。

3. 注册插件：在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.");
   }
   ...
   

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

5. 集成插件库到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>
   

6. 使用插件：在参数配置文件夹中找到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从入门到精通》
• 《Pytorch深度学习实战》
• 《机器学习强基计划》
• 《运动规划实战精讲》
• …

👇源码获取 · 技术交流 · 抱团学习 · 咨询分享 请联系👇