本文主要是介绍ros导航框架-代价地图，希望对大家解决编程问题提供一定的参考价值，需要的开发者们随着小编来一起学习吧！
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_为之前各层插件地图的叠加。
这篇关于ros导航框架-代价地图的文章就介绍到这儿，希望我们推荐的文章对编程师们有所帮助！
