ros costmap框架分析

一、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配置文件里修改相应的配置，使新加的插件层生效，具体怎么添加，网上有教程，这里不再赘述。