costmap_2d包源码学习

0 概述

该软件包订阅现实世界的传感器数据，实现2d栅格costmap。该软件包还支持基于map_server的costmap初始化，基于滚动窗口的costmaps，以及基于参数的订阅和传感器主题的配置。

0.1 层次

分为三层：staticlayer，obstaclelayer，inflationlayer。

0.2 基础cost值

每个单元分为三个状态。
Occupied（costmap_2d::LETHAL_OBSTACLE=254）
Free （ costmap_2d::FREE_SPACE=0）
Unknown Space（costmap_2d::NO_INFORMATION=255）

0.3 inflation

从障碍向外膨胀时，cost值随距离增加而降低。

为此，我们为costmap值定义5个特定符号，因为它们与机器人相关。（可以利用机器人中心所在的单元格值，判断处于什么碰撞状态）
"Lethal"（254）
机器人中心大于等于此值，机器人肯定撞
"Inscribed"（253）
机器人中心大于等于此值，机器人肯定撞
"Possibly circumscribed"（128）
如果机器人中心位于该值或高于该值的单元中，则它取决于机器人的方向是否与障碍物碰撞。
"Freespace"（0）
0-128时，都不撞
"Unknown" 代表该栅格没有信息。

0.4 地图类型

全局-mapserver提供
局部-跟着机器人几何中心走
update_frequency 地图更新周期。每个周期接受sensor数据，按照0.2中的基础cost值更新障碍信息cost，并按照0.3中的数值inflation。
publish_frequency 发布地图频率

0.5 主题

发布
~< name >/footprint (geometry_msgs/Polygon)
订阅
〜< name > / costmap（nav_msgs / OccupancyGrid）
costmap中的值
〜< name> / costmap_updates（map_msgs / OccupancyGridUpdate）
costmap的更新区域的值
〜< name> / voxel_grid（costmap_2d / VoxelGrid）
当基础占用网格使用体素，并且用户请求发布体素网格时，可选择发布。

===========================================================================================================================

1 源码阅读

layer基类是一个虚基类，定义了两个重要接口：
updateBounds、updateCosts。顾名思义，一个更新范围，一个更新cost。
costmap2D基类主要换算坐标等等，负责打杂。里面定义了costmap的存储：unsigned char* costmap_。是一个unsigned char数组。
costmapLayer类定义了几种更新地图的方法。
costmap2dROS 封装了整个功能，对外提供了简洁的接口。
可以采用topdown方式读代码，先读最外层。然后读核心功能。

=======================================

<