1、Costmap2DROS

Costmap2DROS是代价地图与其他ROS模块的接口类，move_base中使用的代价地图就是Costmap2DROS对象。
Costmap2DROS负责对代价地图进行更新，以及发布代价地图，我们在rviz上看到的代价地图就是在这个类中进行发布的。
Costmap2DROS中最重要的一个成员就是layered_costmap_。

protected:LayeredCostmap* layered_costmap_;

LayeredCostmap类的作用是对各层代价地图进行管理以及数据整合。
LayeredCostmap类有两个重要的成员：

std::vector<boost::shared_ptr<Layer> > plugins_;// 各层地图插件
Costmap2D costmap_;     // master layer   各层插件地图合并后的代价地图

Costmap2D类
重要成员：
unsigned char* costmap_ ： costmap的每一个栅格的代价值。
主要功能：提供对代价地图的操作接口-读取信息，修改数据。

在Costmap2DROS的构造函数中，可以看到，

  publisher_ = new Costmap2DPublisher(&private_nh, layered_costmap_->getCostmap(), global_frame_, "costmap",always_send_full_costmap);

1.1 代价地图的更新

代价地图的更新调用链：
Costmap2DROS::mapUpdateLoop(double frequency)->Costmap2DROS::updateMap()->void LayeredCostmap::updateMap(double robot_x, double robot_y, double robot_yaw)
可见最终进入到LayeredCostmap类中的updateMap函数。
void LayeredCostmap::updateMap(double robot_x, double robot_y, double robot_yaw)
1、根据机器人的位置对costmap_进行移动
2、遍历所有的插件地图 调用 updateBounds()
3、遍历所有的插件地图 调用 updateCosts()
将各层代价地图的代价值整合到costmap_上，costmap_就是master layer.

2、 obstacle_layer

障碍物层对应的类是ObstacleLayer，CostmapLayer的子类，通过读取各个传感器的观测数据进行更新，所使用的传感器可以在yaml参数文件中自行设置。
地图数据保存在哪？ ObstacleLayer继承了CostmapLayer的父类，而CostmapLayer类又继承Layer类和Costmap2D类，因此ObstacleLayer的地图数据保存在继承于Costmap2D类的costmap_。
构造函数：
对所使用传感器进行遍历，并一一构造ObservationBuffer放入observation_buffers_，

std::vector<boost::shared_ptr<costmap_2d::ObservationBuffer> > observation_buffers_;     // 保存所使用的传感器的观测buffer

costmap_2d::ObservationBuffer类中有一个重要成员-observation_list_，它会按照时序保存历史的观测数据，

std::list<Observation> observation_list_;

那么也有个函数void purgeStaleObservations() 负责对旧数据进行清理，成员变量observation_keep_time_用来决定数据保留的时间，早于这个时间的数据被丢弃。observation_keep_time_这个变量可以在我们的yaml参数文件中进行配置,一般设为0。

接着将该传感器的观测buffer,设置到marking_buffers_(用于在障碍物层地图山标记障碍)，以及设置到clearing_buffers_(用于清除障碍物标记)。比如，我们一共使用激光雷达和超声波传感器来更新障碍物层，那么marking_buffers_和clearing_buffers_中就会有激光雷达和超声波这两个传感器的观测buffer。

   // marking_buffers_ 保存了每个传感器的ObservationBufferif (marking)marking_buffers_.push_back(observation_buffers_.back());   // check if we'll also add this buffer to our clearing observation buffers// clearing_buffers_ 保存了每个传感器的ObservationBufferif (clearing)clearing_buffers_.push_back(observation_buffers_.back());

最后就是设置读取相应传感器的回调函数。

2.x updateBounds

1、跟随机器人的位置移动地图。
2、根据观测数据更新自由空间

  // raytrace freespacefor (unsigned int i = 0; i < clearing_observations.size(); ++i){raytraceFreespace(clearing_observations[i], min_x, min_y, max_x, max_y);}

3、根据观测到的障碍信息，对costmap_进行更新。

2.x updateCosts

void ObstacleLayer::updateCosts(costmap_2d::Costmap2D& master_grid, int min_i, int min_j, int max_i, int max_j)

将当前层的代价数据更新到master_grid。从前面可以知道，这里传入到master_grid的是LayeredCostmap类的costmap_，也就是将该障碍物层的代价数据更新到LayeredCostmap类的costmap_上。
更新可采用两种方式-updateWithOverwrite和updateWithMax

void CostmapLayer::updateWithOverwrite(costmap_2d::Costmap2D& master_grid, int min_i, int min_j, int max_i, int max_j)
用当前层的代价数据直接覆盖master_grid的数据。

void CostmapLayer::updateWithAddition(costmap_2d::Costmap2D& master_grid, int min_i, int min_j, int max_i, int max_j)
叠加模式。

3、static_layer

静态层对应的类是StaticLayer, 同样是CostmapLayer的子类，因此，代价地图数据保存在继承于Costmap2D类的costmap_。

3.1 数据订阅

在构造函数中可见，map_sub_ = g_nh.subscribe(map_topic, 1, &StaticLayer::incomingMap, this);
订阅的话题是map_topic，数据类型StaticLayer::incomingMap， 回调函数StaticLayer::incomingMap，在回调函数中，将新接收到的静态栅格地图赋值给costmap_。

3.2 updateBounds

这里就是更新了一下地图的边界信息。

3.3 updateCosts

将当前层的代价地图数据costmap_更新到master layer上。

4、inflation_layer

膨胀层InflationLayer不订阅任何数据，要膨胀的地图通过引用传递给updateCosts的master_grid形参即可，

void InflationLayer::updateCosts(costmap_2d::Costmap2D& master_grid, int min_i, int min_j, int max_i, int max_j)

在LayeredCostmap::updateMap中，会将costmap_传入InflationLayer::updateCosts，而costmap_为之前各层插件地图的叠加。