costmap_2d 解析

costmap_2d这个包提供了一种2D代价地图的实现方案，该方案利用输入的传感器数据，构建数据2D或者3D代价地图（取决于是否使用基于voxel的实现），并根据占用网格和用户定义的膨胀半径计算2D代价地图的代价。此外，该包也支持利用map_server初始化代价地图，支持滚动窗口的代价地图，支持参数化订阅和传感器主题的配置。

1、概述

注：红色代表代价地图中的障碍物，蓝色代表的是通过机器人内切圆半径计算的障碍物膨胀，红色的多边形代表机器人外壳。为了使机器人不碰到障碍物，机器人的外壳绝对不允许与红色单元相交，机器人的中心绝对不允许与蓝色单元相交。

costmap_2d包提供了一个可配置的结构，以网格占有的形式保存机器人在该处网格的导航信息。该包使用静态地图的传感器数据与信息，通过 costmap_2d::Costmap2DROS 来储存和升级世界中的障碍物信息。The costmap_2d::Costmap2DROS对象为用户提供了一个纯粹的二维界面，这意味着对障碍物的查询只能在列中进行，例如，在XY平面上位于同一位置的桌子和鞋，虽然在Z方向上有差异但是它们在costmap_2d::Costmap2DROS对象代价地图中对应的cell上拥有相同的代价值。这种设计对平面空间进行路径规划是有帮助的。

从Hydro发布版本开始，用来写数据到代价地图的底层方法已经完全可配置了。每种功能放置一层中。例如，静态地图是一层，障碍物是另一层。缺省情况下，障碍物层维护的是3D信息，3D障碍物数据可以让层更加灵活的标记和清除障碍物。该包提供的ROS化功能接口主要就是costmap_2d::Costmap2DROS，它使用costmap_2d::LayeredCostmap
来跟踪每一层。每一层在Costmap2DROS中以插件方式被实例化，并被添加到LayeredCostmap。
每一层可以独立编译，且可使用C++接口实现对代价地图的随意修改。costmap_2d::Costmap2D
类中实现了用来存储和访问2D代价地图的的基本数据结构。关于代价地图如何更新占用珊格的细节将在下面介绍，同时提供了页面链接供查看某一层具体如何工作。

2、障碍物清楚与占有（clearing and mark）

代价地图自动订阅传感器发布的主题并基于数据进行相应自我更新。
对每个传感器来说，其可以用来执行mark（将障碍物信息插入到代价地图），也可以用来执行clear（从代价地图移除障碍物）或者二者都执行。marking操作就是索引到数组内修改cell的代价。然而对于clearing操作，每次观测报告都需要传感器源向被观测对象发射线。
如果存储的障碍物信息是3D的，需要将每一列的障碍物信息投影成2D后才能放入到代价地图。

3、空间状态（占有、自由、未知）

虽然代价地图中每个cell可用255个不同值中任何一个值(see the inflation section)，可是下层数据结构仅需要3个值。
具体来说在这种下层结构中，每个cell仅需要3个值来表示cell的3种状态：free，occupied，unknown。当投影到代价地图时候，每种状态被赋一个特定的代价值。如果列有一定量的占用(see mark_threshold parameter)
就被赋代价值costmap_2d::LETHAL_OBSTACLE，如果列有一定量的unknown cells (see unknown_threshold parameter)
就被赋代价值costmap_2d::NO_INFORMATION, 剩余其它列赋代价值为costmap_2d::FREE_SPACE。

4、地图更新

代价地图以参数update_frequency 指定的周期进行地图更新。每个周期传感器数据进来后，都要在代价地图底层占用结构上执行标记和清除障碍操作，并且这种结构会被投影到代价地图附上相应代价值。
这完成之后，对代价赋值为costmap_2d::LETHAL_OBSTACLE的每个cell执行障碍物的膨胀操作，即从每个占用cell向外传播代价值，直到用户定义的膨胀半径为止。细节如下6.

5、tf

为了把来自传感器源的数据插入到代价地图，costmap_2d::Costmap2DROS要大量使用tf。所有tf转换由global_frame parameter，the robot_base_frame parameter，以及sensor sources建立联系和更新。The transform_tolerance parameter定义了tf转换所能容忍的的最大延时。如果 tf 树没有以期望速度被更新，那么导航功能包集将会让机器人停止。

6、膨胀（Inflation）

Inflation is the process of propagating cost values out from occupied
cells that decrease with distance. For this purpose, we define 5
specific symbols for costmap values as they relate to a robot.

膨胀是由被占有的栅格（occupied cells ）向外传播其代价值的过程，传播的值随距离增加而递减。为了实现该目的，定义了五个与机器人相关联的特殊符号。

具体状态和值对应有下图：

"Lethal" cost：意思是cell内有障碍物，因此如果机器人中心位于那个cell，很显然机器人会碰到障碍
"Inscribed" cost：意思是cell到障碍物的距离小于机器人内切圆半径。因此如果机器人中心在cell（或者代价>=内切代价），机器人与障碍物肯定有冲突
"Possibly circumscribed"
cost：类似于内切，但是使用机器人外接圆作为截止距离。因此，如果机器人中心位于cell上（或者代价>=外接代价），那么它与障碍物是否冲突依赖于机器人方位。使用术语“可能”意思是这不一定是一个真正障碍cell，但是有些用户喜欢放特定的代价值进去为特定目的。
例如，如果用户要表示机器人应该尝试避开建筑物某特定区域，
它们可以在代价地图上为那个与任何障碍毫无关系的区域（该区域本来就不是障碍区）插入代价值。注意，虽然上图示例中使用了代价值128，但是实际由于代价值受内切圆半径和外接圆半径的影响，真正实际值并不一定是128。

"Freespace" cost：假定为0，意思是没啥东西会阻碍机器人到那里
"Unknown" cost：意思是cell上没有信息

All other costs：被赋值为"Freespace"和"Possibly circumscribed"之间的一个值，取决于到 "Lethal" cell的距离以及用户定义的衰减函数

链接：http://wiki.ros.org/costmap_2d#Inflation