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

發表評論
所有評論
還沒有人評論,想成為第一個評論的人麼? 請在上方評論欄輸入並且點擊發布.
相關文章