costmap_2d 代价地图

导航功能包用代价地图来存储障碍物的信息，代价地图自动获取传感器的信息来进行自我更新。传感器被用来在地图中标记障碍物信息/清除障碍物信息。标记障碍物信息的过程其实是更改单元值的过程。

costmap_2d类的costmap2DROS对象提供二维接口，传感器更新数据。

1.概况

costmap_2d package提供以网格占有的方式，保管机器人应该在哪里导航的信息的配置结构。

costmap用从静态地图中获得的激光数据和信息，通过costmap_2d::Costmap2DROS来存储和更新障碍物信息。costmap_2d::Costmap2DROS提供给用户纯粹的二维界面，意味着障碍物的查询只能在列项中生成。例如，在XY平面中的椅子和鞋相同的坐标下，但由于Z方向的位置不同，导致了相应的costmap_2d::Costmap2DROS的costmap有特定的代价值。这有利于空间之中的规划。

Hydro中，将数据写入到costmap的底层的方法是完全可以配置的。相同功能存在于一层。例如，静态地图是一层，障碍物是一层。默认地，障碍物层保存的是三维信息（voxel_grid）.维护三维障碍物的信息数据可以更加智能的处理标记和清理。

主界面是costmap_2d::Costmap2DROS，它保存了ROS大多数的相关的功能。它所包含的costmap_2d::LayeredCostmap，用来跟踪每一个层。每层使用pluginlib来添加到用Costmap2DROS中，并且被添加到LayeredCostmap。各个层可以被独立的编译，允许通过C++来随意的改变costmap.costmap_2d::Costmap2D类实现存储和访问二位costmap的基本数据结构。

关于costmap如何来更新占有网格的细节将在以下进行描述，同时分页描述各个层是如何单独工作的。

2.标记和清除

costmap通过ROS自动订阅传感器的topics并且相应的更新自己。每个传感器或者被标记（将障碍物信息放到costmap中），或者被清除（删除从costmap中的障碍物的信息），或者都有。标记操作只是一个改变单元代价的索引。清理操作包括通过来自于对每个观测报告的传感器的原始网格光线跟踪。
3.占有/空闲/未知空间

尽管costmap的每个单元中可以有255个不同的代价值（参见inflation部分），它采用的底层的结构只能表示3个。具体的，该结构中的每个单元可以是空闲/占有/未知。每个状态都有特殊的代价值被分配到costmap中去。有一定数量的列占有细胞（见mark_threshold参数）被分配到costmap_2d::LETHAL_OBSTACLE成本。有一定数量的列未知细胞（见unknown_threshold参数）被分配到costmap_2d::NO_INFORMATION成本。其他列被分配到costmap_2d::FREE_SPACE 成本。

4.地图更新

的costmap中的成本值。在此之后，各个障碍物的膨胀在每个单元中以costmap_2d::LETHAL_OBSTACLE 表现。这包括来自每个占有单元向外的传播到用户指定的inflation半径的成本值.该inflation进程细节如下：

5.TF

为了使数据从传感器数据源到costmap，costmap_2d::Costmap2DROS广泛使用TF。具体的，假定在坐标系结构间的坐标变换通过global_frame参数/robot_base_frame参数和激光来进行，并被及时更新。 transform_tolerance参数设置这些转换之间所允许的最大数值的等待时间。如果tf树没有以此频率更新，导航stack停止机器人。

6.Inflation

inflation是将成本值随着距离增加而减小，传播出去的过程。为此，我们为costmap值定义5个特定符号，因为他们涉及到机器人。
    Lethal -- 意味着单元中有实际的障碍物（工作区）。所以，如果机器人的中心在该单元的中心，机器人显然被碰撞。
    Inscribed -- 意味着单元小于机器人的内接半径，远离实际的障碍物。所以机器人如果机器人中心在单元中处于或者高于内切值是，肯定碰撞。
    Possibly circumscribed --与inscribed相似，但是使用机器人的外切半径作为截止距离。因此，如果机器人中心唯一单元或者高于此值时，这取决于机器人的方向是否于障碍物碰撞。我们使用可能，是由于它可能表示真正的障碍物单元，但是游戏用户的喜好，将特定值放到地图中。例如，如果一个用户想表达机器人应该尝试避免障碍物的特定区域，他们可能插入他们自己的成本到costmap中，这个区域独立于任何障碍物。
    Freespace -- 成本为0，意味着没有东西，可以使机器人在此通过。
    Unknown -- 意味着给定的单元没有相应的信息。该costmap的用户可以理解为他们所认为合适的。
所有其他代价根据他们之间距离Lethal单元，定义在"Freespace" 和 "Possibly circumscribed“之间，衰变函数由用户来提供。定义背后的理念是我们把它留给规划师来实施特定的足迹，但是给他们足够的信息。

7.Map Types

有两张方法来初始化 costmap_2d::Costmap2DROS 对象。第一种，用用户生成的静态地图（见map_server包建立地图）来处理。在这种情况下，costmap被初始化匹配静态地图提供的宽度/高度/障碍物信息。该结构与定位系统一起正常使用，像AMCL，允许机器人在地图中登记障碍物信息，通过传感器从环境中获得的数据更新costmap。
第二种方法来初始化costmap_2d::Costmap2DROS object 。给定宽度和高度来设置rolling_window参数为true。rolling_window参数保持机器人在costmap
的中心。它通过全局，当机器人离给定的区域太远时，丢弃地图中的障碍物信息。这种类型的配置最常用在里程计坐标系，其中机器人只关心局部障碍物。

8.Component API