ROS naviagtion analysis: costmap_2d--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_ = -