costmap_2d详解2：Costmap 代码整体分析

代码文件结构：

costmap_2d
├── plugins
│   ├── beifen-virtualWall_layer.cpp
│   ├── inflation_layer.cpp  //膨胀层
│   ├── obstacle_layer.cpp  //动态障碍物层
│   ├── simple_layer.cpp
│   ├── static_layer.cpp   //静态障碍物层
│   ├── virtualWall_layer.cpp
│   └── voxel_layer.cpp //将动态障碍物层分成三维障碍物层
├── src
│   ├── array_parser.cpp
│   ├── costmap_2d_cloud.cpp
│   ├── costmap_2d.cpp
│   ├── costmap_2d_markers.cpp
│   ├── costmap_2d_node.cpp
│   ├── costmap_2d_publisher.cpp  //发布master costmap
│   ├── costmap_2d_ros.cpp  //与ros 对接的入口
│   ├── costmap_layer.cpp
│   ├── costmap_math.cpp
│   ├── footprint.cpp   //足迹
│   ├── layer.cpp  //各种costmap层的基类
│   ├── layered_costmap.cpp  //管理master costmap
│   └── observation_buffer.cpp  //动态障碍物数据信息保存数组

代码类继承关系及整体逻辑流程

mobe_base 中调用costmap_2d_ros 接口

  planner_costmap_ros_ = new costmap_2d::Costmap2DROS("global_costmap", tf_);   

  controller_costmap_ros_ = new costmap_2d::Costmap2DROS("local_costmap", tf_);

costmap_2d_ros.cpp 中调用LayeredCostmap

//实例化LayeredCostmap的指针,track_unknown_space is true
  layered_costmap_ = new LayeredCostmap(global_frame_, rolling_window, track_unknown_space);

//创建一个以 type为类类型的实例变量，然后让plugin这个指针指向这个实例
      boost::shared_ptr plugin = plugin_loader_.createInstance(type);     
      layered_costmap_->addPlugin(plugin);      

      //这里就会调用到插件的initialize 函数
      plugin->initialize(layered_costmap_, name + "/" + pname, &tf_);
      (initialize 在父类layer.cpp 中，它会调用虚函数onInitialize，最后各个plugin 层会实现该方法)

另外一个线程mapUpdateLoop，去更新地图
 

//更新地图
    updateMap(); => layered_costmap_->updateMap(x, y, yaw);

然后更新各个插件层地图范围，和cost 值
    (*plugin)->updateBounds(robot_x, robot_y, robot_yaw, &minx_, &miny_, &maxx_, &maxy_);
    (*plugin)->updateCosts(costmap_, x0, y0, xn, yn);

LayeredCostmap 中更新每一costmap 范围和cell栅格cost 值
//根据各层的更新情况确定地图更新范围的边界
(*plugin)->updateBounds(robot_x, robot_y, robot_yaw, &minx_, &miny_, &maxx_, &maxy_);
//更新该层costmap 大小
costmap_.resetMap(x0, y0, xn, yn);

//调用各层的updateCosts函数用各层的信息去更新更新范围内的地图信息
//x0, y0, xn, yn是该层costmap地图左下角和右上角坐标，以像素表示
(*plugin)->updateCosts(costmap_, x0, y0, xn, yn);