首先介绍下costmap_2d这个包，以下是http://wiki.ros.org/costmap_2d，页面中的官方介绍，对排版进行了整理，并略作删减。
摘要
这个包提供了一个 2D costmap的实现，它接收来自真实世界的传感器数据，构建数据的 2D 或 3D 占用网格（取决于是否使用基于体素的实现），并根据占用网格和用户指定的膨胀半径在 2D costmap中膨胀成本。
该软件包还支持基于 map_server 的costmap初始化、基于滚动窗口的costmap以及基于参数的传感器主题订阅和配置。
一、概述
上图中的各个元素含义：
- 红色单元格代表代价地图中的障碍物
- 蓝色单元格代表被机器人内接半径膨胀的障碍物
- 红色多边形代表机器人的footprint
为了机器人避免碰撞，机器人的footprint不应与红色单元格相交，机器人的中心点不应与蓝色单元格相交。
costmap_2d包提供了一个可配置的结构，该结构以占用网格的形式维护有关机器人应该在哪里导航的信息。costmap 使用静态地图中的传感器数据和信息，通过costmap_2d::Costmap2DROS对象存储和更新世界中的障碍物信息。
costmap_2d ::Costmap2DROS对象为它的用户提供了一个纯二维的界面，这意味着关于障碍物的查询只能在列中进行。例如，XY 平面中相同位置的桌子和鞋子，但 Z 位置不同，将导致costmap_2d::Costmap2DROS中的相应单元格具有相同成本值的对象的costmap。这旨在帮助在平面空间中进行规划。
在 Hydro 版本中，用于将数据写入costmap的底层方法是完全可配置的。每一点功能都存在于一个层中。例如，静态地图是一层，障碍物是另一层。默认情况下，障碍层以三维方式维护信息，维护 3D 障碍物数据可以让图层更智能地处理标记和清除。
costmap_2d::Costmap2DROS类维护了许多与 ROS 相关的功能。
它包含一个costmap_2d::LayeredCostmap，用于跟踪每个层。
costmap_2d ::Costmap2D类实现了用于存储和访问二维costmap的基本数据结构。
每一层都在Costmap2DROS中使用pluginlib进行实例化，并添加到LayeredCostmap中。层本身可以单独编译，允许通过 C++ 接口对costmap进行任意更改。
下面描述了 Costmap 如何更新占用网格的详细信息。
二、标记和清除
costmap 通过 ROS 自动订阅传感器主题并相应地更新costmap的值。每个传感器用于标记（将障碍物信息插入costmap中）、清除（从costmap中移除障碍物信息）或两者兼而有之。
标记操作只是对数组的索引以更改单元格的成本
清除操作包括从传感器的原点向外通过网格的光线追踪，以获取报告的每个观察结果。
如果使用三维结构来存储障碍物信息，则每列的障碍物信息在放入代价图中时会向下投影为二维。
三、已占用、空闲和未知空间
虽然costmap中的每个单元格都可以有 255 个不同的成本值之一（参见膨胀部分），但它使用的基础结构只能表示三个。
这个结构中的每个单元格可以是空闲的、被占用的或未知的。在投影到costmap中时，每个状态都有一个特殊的成本值分配给它。
具有一定数量的占用单元格的列（参见mark_threshold参数）被分配一个costmap_2d::LETHAL_OBSTACLE成本
具有一定数量的未知单元格（参见unknown_threshold参数）的列被分配一个costmap_2d::NO_INFORMATION成本
其他列是分配了costmap_2d::FREE_SPACE成本。
四、地图更新
costmap以update_frequency参数指定的速率执行地图更新周期。
每个周期，传感器数据进入，标记和清除操作在代价图的底层占用结构中执行，并且该结构被投影到代价图中，其中如上所述分配适当的代价值。
在此之后，每个障碍物膨胀都在每个具有costmap_2d::LETHAL_OBSTACLE成本的单元上执行。
这包括将成本值从每个占用的单元向外传播到用户指定的膨胀半径。这个膨胀过程的细节概述如下。
五、tf
为了将来自传感器源的数据插入costmap中，costmap_2d::Costmap2DROS对象广泛使用了tf。
通过指定global_frame参数、robot_base_frame参数和传感器源指定的坐标系之间的所有变换。
transform_tolerance参数设置这些转换之间允许的最大延迟量。如果tf树没有以这个预期的速率更新，Navigation会停止运动。
六、膨胀
膨胀是从随距离减小的占用单元中传播成本值的过程。为此，我们为与机器人相关的costmap值定义了 5 个特定符号。
“Lethal”成本：意味着单元中存在实际（工作空间）障碍。所以如果机器人的中心在那个牢房里，机器人显然会发生碰撞。
“Inscribed”成本：意味着一个单元格距离实际障碍物的距离小于机器人的内接半径。因此，如果机器人中心位于等于或高于内接成本的单元格中，则机器人肯定会与某些障碍物发生碰撞。
“Possibly circumscribed”成本：类似于“内接”，但使用机器人的外接半径作为截止距离。因此，如果机器人中心位于该值或高于该值的单元格中，那么它是否与障碍物碰撞取决于机器人的方向。我们使用“可能”一词是因为它可能不是真正的障碍单元，而是某些用户偏好，将特定的成本值放入地图中。例如，如果用户想要表示机器人应该尝试避开建筑物的特定区域，他们可以将自己的成本插入到该区域的costmap中，而不受任何障碍的影响。请注意，虽然上图中以 128 为例，但真实值受代码中定义的 inscribed_radius 和通货膨胀半径参数的影响。
“Freespace”成本：这意味着没有任何东西可以阻止机器人去那里，成本为零。
“Unknown”成本：意味着没有关于给定单元的信息。
根据它们与“Lethal”单元的距离和用户提供的衰减函数，所有其他成本都被分配一个介于“自由空间”和“可能受限”之间的值。
这些定义背后的基本原理是，我们将其留给规划者实现来关心或不关心确切的footprint，但要给他们足够的信息，以便他们仅在方向真正重要的情况下才会承担追踪footprint的成本。
七、地图类型
初始化costmap_2d::Costmap2DROS对象有两种主要方法。
第一种是使用用户生成的静态地图为其播种（有关构建地图的文档，请参见map_server包）。在这种情况下，代价地图被初始化以匹配静态地图提供的宽度、高度和障碍物信息。此配置通常与诸如amcl之类的定位系统结合使用，该系统允许机器人在地图框架中注册障碍物，并在其行驶通过其环境时根据传感器数据更新其costmap。
第二种是给它一个宽度和高度，并将rolling_window参数设置为 true。rolling_window参数使机器人在世界各地移动时保持在costmap的中心，当机器人远离给定区域时从地图中删除障碍物信息。这种类型的配置最常用于里程坐标系中，其中机器人只关心局部区域内的障碍物。
