一、costmap类型
global_costmap:全局代价地图,用于全局路径规划。
locat_costmap:局部代价地图,用于避障。
二、什么是costmap
简单来讲,costmap就是一张代价地图,地图数据是一个unsigned chat数组,每一个点的值表示该点对应的实际坐标的通行性,具体可参见http://wiki.ros.org/costmap_2d/。 costmap坐标系的坐标是整数,世界坐标系的坐标是双浮点数,它们之间通过分辨率以及costmap左下角坐标进行转换。costmap对应的类为costmap_2d::Costmap2D。
三、什么是costmap layer
ros利用插件机制实现了分层的costmap,每一层保存一张由该层对应的数据源生成的costmap,由各自的数据源实时更新,最后,各层的数据上下叠加在一起,就生成了最终的costmap。每一层的costmap为Costmap2D类型,最后叠加在一起的为LayeredCostmap类型,LayeredCostmap类里包含一个Costmap2D类型的成员变量。
四、costmap相关类图
五、costmap更新流程
1、实例化Costmap2DROS对象,在构造函数里主要做以下三件事:
-
创建LayeredCostmap类型的对象layered_costmap_,并初始化costmap起点坐标和大小
-
初始化costmap各层插件并加入到layered_costmap_里
-
启动更新costmap的线程mapUpdateLoop
2、在Costmap2DROS::updateMap函数里先获取机器人位姿,然后调用layered_costmap_的updateMap函数更新各层的costmap
3、在LayeredCostmap::updateMap里,做以下三件事
- 如果设置了rolling_window_为true,更新costmap的初始点坐标
- 依次调用各层插件的updateBounds函数,该函数会根据数据源数据先清空掉视野内已不存在的障碍物,然后再添加视野内新增的障碍物
- 依次调用各层插件的updateCosts函数,把每一层的costmap叠加成最终的costmap。
4、更新完成后,通过publisher_把costmap发送出去
六、添加新的传感器数据生成costmap
可以仿照其他层重写一个插件,主要修改updateBounds里的代码逻辑,实现障碍物的添加与清除,并在costmap_common.yaml、costmap_local.yaml、costmap_global.yaml配置文件里修改相应的配置,使新加的插件层生效,具体怎么添加,网上有教程,这里不再赘述。