ROS基础教程--CostMap_2D包的一些理解

本文是在综合了多篇文章的基础之上进行的综合.

1.基本概念

Voxel：体素，即顾名思义是体积的像素。用来在三维空间中表示一个显示基本点的单位。类似于二维平面下的pixel（像素）。voxel是三维空间中定义一个点的图象信息的单位。在平面中定义一个点要两个坐标X和Y就够了，而在三维世界中还要有一个坐标。光有3维坐标位置还不行，还要有颜色等信息，这就是voxel的含义。

机器人在costmap_2D中的模型：两个同心圆，在图1里可以看到左下角有两个淡蓝色同心圆，一个机器人的轮廓外切圆和一个机器人内切圆，机器人在costmap里就能够简化成为这两个圆。根据机器人中心至边界或者障碍物的距离和两个同心圆半径比较来判断是否碰撞。

footprint：足迹，即机器人的轮廓。在ROS中，它由二维数组表示[x0,y0] ; [x1,y1] ; [x2,y2]……不需要重复第一个坐标。该占位面积将用于计算内切圆和外接圆的半径，用于以适合此机器人的方式对障碍物进行膨胀。为了安全起见，我们通常将足迹稍大于机器人的实际轮廓。要确定机器人的占地面积，最直接的方法是参考机器人的图纸。 此外，您可以手动拍摄其基座顶视图。 然后使用CAD软件（如Solidworks）适当缩放图像，并将鼠标移动到基座轮廓上并读取其坐标。 坐标的起点应该是机器人的中心。 或者，您可以将机器人移动到一张大纸上，然后绘制基座的轮廓。 然后选择一些顶点并使用标尺来确定它们的坐标。

cost：代价或者占用，0-255的取值，表示该机器人位于该网格点(grid cell)的代价，或者机器人的frootprint中心cell走到该网格点的代价（中心到达某个位置的代价与非中心部分到达某个位置付出的代价不同，如撞击造成的损伤程度等）。

cell：单元，网格，栅格

2. costmap的网格代价计算

无论是激光雷达还是如kinect 或xtion pro深度相机作为传感器跑出的2D或3D SLAM地图，都不能直接用于实际的导航，必须将地图转化为costmap(代价地图),ROS的costmap通常采用grid(网格)形式。以前一直没有搞明白每个栅格的概率是如何算出来的，原因是之前一直忽略了内存的存储结构，栅格地图一个栅格占1个字节，也就是八位，可以存0-255中数据，也就是每个cell cost（网格的值）从0~255我们只需要三种情况：Occupied被占用（有障碍）, Free自由区域（无障碍）,  Unknown Space未知区域。。

Costmap是机器人收集传感器信息建立和更新的二维或三维地图，可以从下图简要了解。

图0. 机器人和障碍物以及膨胀区域在2维地图上的footprint和投影表示

注意: 在上图中，红色cell（图中红色蓝色区域都是一系列cell堆叠出来的）代表的是代价地图中的障碍，蓝色cell代表的是通过机器人内切圆半径计算的障碍物膨胀，红色多边形代表的是机器人footprint(机器人轮廓的垂直投影)。 为了使机器人不碰到障碍物，机器人的footprint绝对不允许与红色cell相交，机器人的中心绝对不允许与蓝色cell相交。

扫描二维码关注公众号，回复： 1028450 查看本文章

    空间状态（Occupied, Free, and Unknown）

ROS的代价地图（costmap）采用网格（grid）形式，每个网格的值（cell cost）从0~255。分成三种状态：被占用（有障碍）、自由区域（无障碍）、未知区域；以激光雷达为传感器（或者kinect之类的深度相机的伪激光雷达），根据激光测量的障碍物距离机器人中心的距离，结合机器人的内切和外切半径，搞一个映射，利用bresenham算法（计算方法参考https://www.cnblogs.com/zjiaxing/p/5543386.html）可以填充由激光雷达的位置到障碍物之间的栅格概率了。

虽然代价地图中每个cell可用255个不同值中任何一个值，可是下层数据结构仅需要3个值。 具体来说在这种下层结构中，每个cell仅需要3个值来表示cell的3种状态：free，occupied，unknown。 当投影到代价地图时候，每种状态被赋一个特定的代价值，也就是说每个cell的cost值是由这个cell对应的各层中对应的cell的状态进行加权得到的。 如果列有一定量的占用就被赋代价值costmap_2d::LETHAL_OBSTACLE， 如果列有一定量的unknown cells 就被赋代价值costmap_2d::NO_INFORMATION, 剩余其它列赋代价值为costmap_2d::FREE_SPACE

在某时刻和机器人当前前进方向上的网格点的代价计算示意图如下（如果机器人的前进方向改变，则网格点的代价也会发生变化）：

图1. 机器人模型以及在当前前进方向上各grid的代价分布示意图

上图下面的红色五边形区域为机器人的轮廓。坐标系内的区域可分为五部分，（cost的计算是以cell为单位进行的，而不是以某个障碍物为单位进行的，也就是一次只计算一个cell的cost值）
（1） Lethal（致命的）:机器人的中心（center cell）与该网格的中心重合，此时机器人必然与障碍物冲突。
（2） Inscribed（内切）：网格中心位于机器人的内切轮廓内，此时机器人也必然与障碍物冲突。
（3） Possibly circumscribed（可能受限）：网格中心位于机器人的外切圆与内切圆轮廓之间，此时机器人相当于靠在障碍物附近，所以不一定冲突，取决于机器人的方位或者说姿态。
（4） Freespace（自由空间）：网格中心位于机器人外切圆之外，属于没有障碍物的空间。

（5） Unknown（未知）：未知的空间。

如果按照cell的三种状态划分，我个人认为上述前3种状态，都属于“被占用”状态。

这里介绍一下costmap_2d中计算cost的方法。假设，机器人内切半径为0.5m，外切半径为0.7m，当激光返回障碍距离在机器人中心附近，叫致命障碍，机器人一定能碰到障碍物，比如说0m,直接贴着机器人，或会取小于栅格的边长，比如小于0.1m范围内,则这个栅格值就设为254；当返回来的数值在0.1-0.5m之间，就设253；当在0.5-0.7之间，则可以设128，或者在252-128找个比例值（程序中可以控制），属于受限区域，可能发生碰撞，是否碰撞，取决于机器人的姿态；当0.7-膨胀半径之间，设1-127之间的映射值，不会发生碰撞；当大于膨胀距离，则设为0，称为freespace。Unknown -- 意味着给定的单元没有相应的信息。我们看坐标系中较细的红色光滑曲线就是cost曲线，x是距离机器人footprint的圆心距离，而y是cost值，cost随着x的增大而减小距离，当x>=内切圆半径时开始有值；当x=0时，y=254；当x=resolution/2时，cost=253；（图中右上角的较粗的台阶状红线是单元格的边线，或者认为是障碍物（单元格化后）的边线）

3. Costmap的分层与更新

3.1 Costmap中的地图与分层

Costmap_2D提供了一种2D代价地图的实现方案，该方案利用输入传感器数据，构建数据2D或者3D（依赖于是否使用基于voxel的实现）代价地图。 此外，该包也支持利用map_server初始化代价地图，支持滚动窗口的代价地图，支持参数化订阅和配置传感器主题。

图2. voxel代价地图示意图（将3维障碍物投影到2维平面，则2维空间内不相撞的导航或者轨迹在三维空间内也肯定不相撞）

从Hydro发布版本开始， 用来写数据到代价地图的底层方法已经完全可配置了。 Costmap由多层组成，每种功能放置一层中。 例如图3所示，静态地图是一层，障碍物是另一层。 缺省情况下，障碍物层维护的是3D信息，3D障碍物数据可以让层更加灵活的标记和清除障碍物。例如在costmap_2d包中，StaticLayer（静态地图层）是第一层，ObstacleLayer（障碍物层）是第二层，InflationLayer（膨胀层）是第三层。这三层组合成了master map（最终的costmap），供给路线规划模块使用。

图3. costmap_2D中的4个分层（从Hydro版本之后采用这种分层结构）

我自己定义的障碍物也可以是一层（假如我不想让机器人通过一个freespace就可以自己插入个障碍物，主要的接口是costmap_2d::Costmap2DROS，在每一层中使用pluginlib实例化Costmap2DROS并将每一层添加到LayeredCostmap），各个层可以被独立的编译。如下图所示：

costmap_2d包提供了一种可配置框架来维护机器人在代价地图上应该如何导航的信息。 代价地图使用来自传感器的数据和来自静态地图中的信息，通过costmap_2d::Costmap2DROS来存储和更新现实世界中障碍物信息。costmap_2d::Costmap2DROS给用户提供了纯2D的接口，这意味着查询障碍只能在列上进行。例如，在XY平面上位于同一位置的桌子和鞋，虽然在Z方向上有差异但是它们在costmap_2d::Costmap2DROS对象代价地图中对应的cell上拥有相同的代价值。 这种设计对平面空间进行路径规划是有帮助的。

costmap_2D提供的ROS化功能接口主要就是 costmap_2d::Costmap2DROS，它使用 costmap_2d::LayeredCostmap 来跟踪每一层。 每一层在 Costmap2DROS中以 插件方式被实例化，并被添加到 LayeredCostmap。 每一层可以独立编译，且可使用C++接口实现对代价地图的随意修改，即LayerdCostmap为Costmap2DROS（用户接口）提供了加载地图层的插件机制，每个插件（即地图层）都是Layer类型的。 costmap_2d::Costmap2D 类中实现了用来存储和访问2D代价地图的的基本数据结构。

图4. costmap中的Layer的继承关系

costmap中各Layer之间的继承关系如图4所示，本文后面我们附的layeredcostmap的相关介绍还会用到这个图。

3.2 Costmap初始化流程

在navigation的主节点move_base中(costmap隶属于navigation包，或者说是navigation的一个子模块)，建立了两个costmap。其中planner_costmap_ros_是用于全局导航的地图，controller_costmap_ros_是用于局部导航用的地图。图5为costmap的初始化流程。

图5. costmap的初始化流程

（1）Costmap初始化首先获得全局坐标系和机器人坐标系的转换
（2）加载各个Layer，例如StaticLayer，ObstacleLayer，InflationLayer。
（3）设置机器人的轮廓
（4）实例化了一个Costmap2DPublisher来发布可视化数据。
（5）通过一个movementCB函数不断检测机器人是否在运动
（6）开启动态参数配置服务，服务启动了更新map的线程。

3.3 costmap中各层的更新

简单整理了下costmap初始化过程中的各层加载的调用过程：

在move_base刚启动时就建立了两个costmap，而这两个costmap都加载了三个Layer插件，它们的初始化过程如上图所示。

StaticLayer主要为处理gmapping或者amcl等产生的静态地图。
ObstacLayer主要处理机器人移动过程中产生的障碍物信息。
InflationLayer主要处理机器人导航地图上的障碍物信息膨胀（让地图上的障碍物比实际障碍物的大小更大一些），尽可能使机器人更安全的移动。
costmap在mapUpdateLoop线程中执行更新地图的操作，每个层的工作流程如下：
(1)StaticLayer工作流程

上图是StaticLayer的工作流程，updateBounds阶段将更新的界限设置为整张地图，updateCosts阶段根据rolling参数（是否采用滚动窗口）设置的值，如果是，那静态地图会随着机器人移动而移动，则首先要获取静态地图坐标系到全局坐标系的转换，再更新静态地图层到master map里。
(2)ObstacleLayer工作流程


上图是ObstacleLayer的工作流程，updateBounds阶段将获取传感器传来的障碍物信息经过处理后放入一个观察队列中，updateCosts阶段则将障碍物的信息更新到master map。
(3)inflationLayer工作流程

上图是inflationLayer的工作流程，updateBounds阶段由于本层没有维护的map，所以维持上一层地图调用的Bounds值（处理区域）。updateCosts阶段用了一个CellData结构存储master map中每个grid点的信息，其中包括这个点的二维索引和这个点附近最近的障碍物的二维索引。改变每个障碍物CELL附近前后左右四个CELL的cost值，更新到master map就完成了障碍物的膨胀。

3.4 Costmap更新

前面说过，“Costmap2DROS使用costmap_2d::LayeredCostmap 来跟踪每一层”。这里我们要清楚每一层有什么操作。

    障碍物标记和清除

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

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

Costmap的更新在mapUpdateLoop线程中实现，此线程分为两个阶段：
（阶段一）UpdateBounds：这个阶段会更新每个Layer的更新区域（增量更新？只更新变化的部分而不是全部更新），这样在每个运行周期内减少了数据拷贝的操作时间。StaticLayer的Static map只在第一次做更新，Bounds 范围是整张Map的大小，而且在UpdateBounds过程中没有对Static Map层的数据做过任何的更新（静态地图，一次写入，后面读取，很少变更）。ObstacleLayer在这个阶段主要的操作是更新Obstacles Map层的数据，然后更新Bounds（清空了Master层对应的bounds内的数据）。InflationLayer则保持上一次的Bounds。
（阶段二）UpdateCosts：这个阶段将各层数据逐一拷贝到Master Map，可以通过下图观察Master Map的生成流程。（图来源于David Lu的《Layered Costmaps for Context-Sensitive Navigation》）

在（a）中，初始有三个Layer和Master costmap,Static Layer和Obstacles Layer维护它们自己的地图，而inflation Layer并没有。为了更新costmap,算法首先在各层上调用自己的UpdateBounds方法（b）。为了决定新的bounds,Obstacles Layer利用新的传感器数据更新它的costmap。然后每个层轮流用UpdateCosts方法更新Master costmap的某个区域,从Static Layer开始（c），然后是Obstacles Layer(d)，最后是inflation Layer(e)。

如果上图不容易理解，可以参考下面这个PPT，里面有更新的动态过程：http://download.csdn.net/download/jinking01/10272584

4. LayeredCostmap代码分析

在数据成员中，有两个重要的变量：Costmap2D costmap_; 和 std::vector<boost::shared_ptr<Layer> > plugins_; 。
这个类相对比较简单，首先来看构造函数：

LayeredCostmap::LayeredCostmap(std::string global_frame, bool rolling_window, bool track_unknown) :
    costmap_(), global_frame_(global_frame), rolling_window_(rolling_window), initialized_(false), size_locked_(false)
{
  if (track_unknown)
    costmap_.setDefaultValue(255);
  else
    costmap_.setDefaultValue(0);
}

调用costmap_ 的setDefaultValue 方法，实际上设定了类costmap_2d 的一个成员变量default_value_ 这个值在class costmap_2d 中是这样使用的：memset(costmap_, default_value_, size_x_ * size_y_ * sizeof(unsigned char)); 实际存储地图的变量就是class costmap_2d 的 costmap_ 数据成员。

析构函数中，所有的操作就是弹出plugin： plugins_.pop_back(); 。

函数LayeredCostmap::resizeMap 就是给class costmap_2d 的 costmap_ 成员的大小重新做分配。然后根据plugin对每一层的地图调用其父类Costmap2D成员的initial 方法，实际效果就是将plugin所指向的每一层地图的大小都设置为和LayeredCostmap::costmap_ 数据成员一样的空间大小。

{
  size_locked_ = size_locked;
  costmap_.resizeMap(size_x, size_y, resolution, origin_x, origin_y);
  for (vector<boost::shared_ptr<Layer> >::iterator plugin = plugins_.begin(); plugin != plugins_.end();
      ++plugin)
  {
    (*plugin)->matchSize();
  }
}

函数 LayeredCostmap::updateMap 完成对每一层地图的更新，更新过程分为两步updateBounds和updateCosts：

void LayeredCostmap::updateMap(double robot_x, double robot_y, double robot_yaw)
{
  if (rolling_window_)
  {
    double new_origin_x = robot_x - costmap_.getSizeInMetersX() / 2;
    double new_origin_y = robot_y - costmap_.getSizeInMetersY() / 2;
    costmap_.updateOrigin(new_origin_x, new_origin_y);
  }

  if (plugins_.size() == 0)
    return;

  minx_ = miny_ = 1e30;
  maxx_ = maxy_ = -1e30;

  for (vector<boost::shared_ptr<Layer> >::iterator plugin = plugins_.begin(); plugin != plugins_.end();
      ++plugin)
  {
    (*plugin)->updateBounds(robot_x, robot_y, robot_yaw, &minx_, &miny_, &maxx_, &maxy_);
  }

  int x0, xn, y0, yn;
  costmap_.worldToMapEnforceBounds(minx_, miny_, x0, y0);
  costmap_.worldToMapEnforceBounds(maxx_, maxy_, xn, yn);

  x0 = std::max(0, x0);
  xn = std::min(int(costmap_.getSizeInCellsX()), xn + 1);
  y0 = std::max(0, y0);
  yn = std::min(int(costmap_.getSizeInCellsY()), yn + 1);

  ROS_DEBUG("Updating area x: [%d, %d] y: [%d, %d]", x0, xn, y0, yn);

  if (xn < x0 || yn < y0)
    return;

  {
    // Clear and update costmap under a single lock
    boost::unique_lock<Costmap2D::mutex_t> lock(*(costmap_.getMutex()));
    costmap_.resetMap(x0, y0, xn, yn);
    for (vector<boost::shared_ptr<Layer> >::iterator plugin = plugins_.begin(); plugin != plugins_.end();
        ++plugin)
    {
      (*plugin)->updateCosts(costmap_, x0, y0, xn, yn);
    }
  }
}

这里我们来看这两个更新过程参数：

  (*plugin)->updateBounds(robot_x, robot_y, robot_yaw, &minx_, &miny_, &maxx_, &maxy_);
   (*plugin)->updateCosts(costmap_, x0, y0, xn, yn);

更新Bounds过程由于传入的参数是&minx_, &miny_, &maxx_, &maxy_ 构成了一个矩形范围。由于针对不同的类的实例，调用不同的类的方法。
对于Static Map：

{
  if (!map_received_ || !(has_updated_data_ || has_extra_bounds_))
    return;

  useExtraBounds(min_x, min_y, max_x, max_y);

  double wx, wy;

  mapToWorld(x_, y_, wx, wy);
  *min_x = std::min(wx, *min_x);
  *min_y = std::min(wy, *min_y);

  mapToWorld(x_ + width_, y_ + height_, wx, wy);
  *max_x = std::max(wx, *max_x);
  *max_y = std::max(wy, *max_y);

  has_updated_data_ = false;
}

Static map 只在第一次做更新，Bounds 范围是整张Map的大小，而且在UpdateBounds过程中没有对Static Map层的数据做过任何的更新。

而对于 ObstacleLayer::updateBounds ：主要的操作是更新Obstacles Map层的数据，然后才是更新Bounds

 std::vector<Observation> observations, clearing_observations;

  // get the marking observations
  current = current && getMarkingObservations(observations);
  // get the clearing observations
  current = current && getClearingObservations(clearing_observations);
  // update the global current status
  current_ = current;
  // raytrace freespace
  for (unsigned int i = 0; i < clearing_observations.size(); ++i)
  {
    raytraceFreespace(clearing_observations[i], min_x, min_y, max_x, max_y);
  }
  for (std::vector<Observation>::const_iterator it = observations.begin(); it != observations.end(); ++it)
  {
    const Observation& obs = *it;

    const pcl::PointCloud<pcl::PointXYZ>& cloud = *(obs.cloud_);
    for (unsigned int i = 0; i < cloud.points.size(); ++i)
    {
      double px = cloud.points[i].x, py = cloud.points[i].y, pz = cloud.points[i].z;
      // now we need to compute the map coordinates for the observation
      unsigned int mx, my;
      if (!worldToMap(px, py, mx, my))
      {
        continue;
      }
      unsigned int index = getIndex(mx, my);
      costmap_[index] = LETHAL_OBSTACLE;
      touch(px, py, min_x, min_y, max_x, max_y);
    }
  }
  updateFootprint(robot_x, robot_y, robot_yaw, min_x, min_y, max_x, max_y);

对于InflationLayer::updateBounds 则保持上一次的min_x, min_y, max_x, max_y 。
对于VoxelLayer::updateBounds 更新过程和 ObstacleLayer::updateBounds 基本一致，只是增加了z 作为判断是否将2d地图的点设定为LETHAL_OBSTACLE 。

updateCosts： 完成updateBounds 后，开始调用(*plugin)->updateCosts(costmap_, x0, y0, xn, yn); 。函数的第一个参数是指的master map，后面的bounds是对每个plugin自己维护的map的更新界限做设定。这里需要分析每一个单独的costmap和master map是哪些类在维护：
Master map： 这是由类LayeredCostmap 的 Costmap2D costmap_ 维护。
StaticLayer StaticLayer VoxelLayer： 这些类是继承于Costmap2D ，因此可以直接操作Costmap2D 的数据成员 unsigned char* costmap_;。因此可以看成每一层地图都是类Costmap2D 的一个实例。
InflationLayer 没有继承于Costmap2D 是因为这个类并不需要维护一张自己的地图，它仅仅是需要直接操作master map的数据就可以了。
每个plugin调用自己代表的层的updateCosts方法：
StaticLayer 和ObstacleLayer 基本上都是调用了CostmapLayer::updateWithOverwrite，CostmapLayer::updateWithTrueOverwrite， CostmapLayer::updateWithMax 等方法。因为CostmapLayer 是这两个的父类。
但是InflationLayer::updateCosts 则不同，因为它既没有自己层的map实例，也不是从CostmapLayer 继承而来。它的updateCosts 是这个类的核心操作。关于他的updateCosts 操作，将在InflationLayer 篇具体分析这个算法的实现过程，这个算法实现了对障碍物膨胀操作。

函数bool LayeredCostmap::isCurrent() 主要的操作是对操作的实时性提供保证，提供是否发生超时的信息。

 bool current = (ros::Time::now() - last_updated_).toSec() <= expected_update_rate_.toSec();

函数void LayeredCostmap::setFootprint(conststd::vector<geometry_msgs::Point>& footprint_spec) ：

{
  footprint_ = footprint_spec;
  costmap_2d::calculateMinAndMaxDistances(footprint_spec, inscribed_radius_, circumscribed_radius_);

  for (vector<boost::shared_ptr<Layer> >::iterator plugin = plugins_.begin(); plugin != plugins_.end();
      ++plugin)
  {
    (*plugin)->onFootprintChanged();
  }
}

inscribed_radius_, circumscribed_radius_ 是计算得到的机器人尺寸的内切圆和外切圆半径。
这里重点关注InflationLayer 类是如何调用onFootprintChanged() 的。对于其他类型的plugin实例来说，其本身并没有重载这个函数，所以都是调用的Layer类的空函数virtual void onFootprintChanged() {}：

  cell_inflation_radius_ = cellDistance(inflation_radius_);
  computeCaches();

函数computeCaches()：

void InflationLayer::computeCaches()
{
  if (cell_inflation_radius_ == 0)
    return;

  // based on the inflation radius... compute distance and cost caches
  if (cell_inflation_radius_ != cached_cell_inflation_radius_)
  {
    deleteKernels();

    cached_costs_ = new unsigned char*[cell_inflation_radius_ + 2];
    cached_distances_ = new double*[cell_inflation_radius_ + 2];

    for (unsigned int i = 0; i <= cell_inflation_radius_ + 1; ++i)
    {
      cached_costs_[i] = new unsigned char[cell_inflation_radius_ + 2];
      cached_distances_[i] = new double[cell_inflation_radius_ + 2];
      for (unsigned int j = 0; j <= cell_inflation_radius_ + 1; ++j)
      {
        cached_distances_[i][j] = hypot(i, j);
      }
    }

    cached_cell_inflation_radius_ = cell_inflation_radius_;
  }

  for (unsigned int i = 0; i <= cell_inflation_radius_ + 1; ++i)
  {
    for (unsigned int j = 0; j <= cell_inflation_radius_ + 1; ++j)
    {
      cached_costs_[i][j] = computeCost(cached_distances_[i][j]);
    }
  }
}

在函数定义中，维护两个指针：

 cached_costs_ = new unsigned char*[cell_inflation_radius_ + 2];
 cached_distances_ = new double*[cell_inflation_radius_ + 2];

第一阶段，计算出cached_distances_: cached_distances_[i][j] = hypot(i, j); ,其中i j 的范围都是0：cell_inflation_radius_ + 1。
第二阶段，通过计算得到的cached_distances_ 计算cached_costs_：cached_costs_[i][j] = computeCost(cached_distances_[i][j]);。通过这个操作，现在可以任意给出在0-cell_inflation_radius_ cell范围的两个cells的costs，以后对地图做膨胀时，只需要查看某个cell（i1,j1）和obstacle cell(i,j)的下标就可以通过查表知道这个cell的代价是多少。这个表的大小仅仅和机器人的几何尺寸相关，一旦机器人尺寸发生改变，这个函数就需要再次被调用。
OK， LayeredCostmap的分析就这么多了～相信这样来来回回反复的分析这些类之间的调用关系，对于理解costmap_2d这个小怪兽是必要的。

原文参考：

http://download.csdn.net/download/jinking01/10272584

http://blog.csdn.net/u013158492/article/details/50490490

http://blog.csdn.net/x_r_su/article/details/53408528

http://blog.csdn.net/lqygame/article/details/71270858

http://blog.csdn.net/lqygame/article/details/71174342?utm_source=itdadao&utm_medium=referral

http://blog.csdn.net/xmy306538517/article/details/72899667

http://docs.ros.org/indigo/api/costmap_2d/html/classcostmap__2d_1_1Layer.html