ros导航框架-代价地图

1、Costmap2DROS

在MoveBase中，代价地图是和对应的规划算法进行绑定的，在MoveBase的构造函数中可以看到：

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

//initialize the global planner
try {
  planner_ = bgp_loader_.createInstance(global_planner);
  planner_->initialize(bgp_loader_.getName(global_planner), planner_costmap_ros_);
} catch (const pluginlib::PluginlibException& ex) {
  ROS_FATAL("Failed to create the %s planner, are you sure it is properly registered and that the containing library is built? Exception: %s", global_planner.c_str(), ex.what());
  exit(1);
}

// 这个就是局部costmap
//create the ros wrapper for the controller's costmap... and initializer a pointer we'll use with the underlying map
controller_costmap_ros_ = new costmap_2d::Costmap2DROS("local_costmap", tf_);
controller_costmap_ros_->pause();

//create a local planner
try {
  tc_ = blp_loader_.createInstance(local_planner);
  ROS_INFO("Created local_planner %s", local_planner.c_str());
  tc_->initialize(blp_loader_.getName(local_planner), &tf_, controller_costmap_ros_);
} catch (const pluginlib::PluginlibException& ex) {
  ROS_FATAL("Failed to create the %s planner, are you sure it is properly registered and that the containing library is built? Exception: %s", local_planner.c_str(), ex.what());
  exit(1);
}

构造了planner_costmap_ros_和controller_costmap_ros_ 分别用于全局路径规划和局部路径规划。

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

protected:
  LayeredCostmap* layered_costmap_;

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

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

plugins_即插件地图，如static_layer，obstacle_layer，inflation_layer。
在Costmap2DROS类的构造函数中，对插件地图进行构造，首先读取相关参数，然后根据参数去构造插件地图。

if (private_nh.hasParam("plugins"))
  {
    XmlRpc::XmlRpcValue my_list;
    private_nh.getParam("plugins", my_list);
    for (int32_t i = 0; i < my_list.size(); ++i)
    {
      std::string pname = static_cast<std::string>(my_list[i]["name"]);
      std::string type = static_cast<std::string>(my_list[i]["type"]);
      ROS_INFO("%s: Using plugin \"%s\"", name_.c_str(), pname.c_str());

      copyParentParameters(pname, type, private_nh);

      boost::shared_ptr<Layer> plugin = plugin_loader_.createInstance(type);
      layered_costmap_->addPlugin(plugin);
      // 调用基类的Layer::initialize
      plugin->initialize(layered_costmap_, name + "/" + pname, &tf_);
    }
  }

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_ 保存了每个传感器的ObservationBuffer
    if (marking)
      marking_buffers_.push_back(observation_buffers_.back());   

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

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

2.x updateBounds

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

  // raytrace freespace
  for (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_为之前各层插件地图的叠加。