costmap_2d详解4：layered_costmap.cpp 分析

关键变量

class Layer;
Costmap2D costmap_; //这里是指master costmap地图
std::string global_frame_; //global_costmap时为map, local_costmap时为odom_combined
bool rolling_window_;  //是否是滚动窗口，global_costmap时为false, local_costmap时为true
bool current_;
double minx_, miny_, maxx_, maxy_;
unsigned int bx0_, bxn_, by0_, byn_;
std::vector<boost::shared_ptr<Layer> > plugins_;
bool initialized_;
bool size_locked_;
//机器人外切和内切半径
double circumscribed_radius_, inscribed_radius_; 
//机器人足迹(模型)参数
std::vector<geometry_msgs::Point> footprint_;

/*构造函数

*参数: global_frame :global_costmap时为map, local_costmap时为odom_combined

*rolling_window: global_costmap时为false, local_costmap时为true

*track_unknown: 都是false，？？obstacle_layer层时为true？？？

*/

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)
{
  /*track_unkown_space参数先给其内部的总地图Costmap2D设置地图
   * 数据缺省值NO_INFORMATION或者FREE_SPACE
   *track_unknown_space is true 
  */
  if (track_unknown)
    costmap_.setDefaultValue(255);
  else
    costmap_.setDefaultValue(0);

}

 

/*作用:更新costmap 地图的边界和cost值

*参数: robot_x,robot_y,robot_yaw :

得到global_frame_ 与robot_base_frame_ tf转换信息中的 translation 坐标信息的x，y和偏转角

如果是global_costmap 应该是地图的像素坐标系原点，即origin 点

如果是local_costmap 应该是机器人当前的坐标原点

*/

void LayeredCostmap::updateMap(double robot_x, double robot_y, double robot_yaw)
{
  //如果我们使用的是滚动缓冲costmap …我们需要使用机器人的位置来更新原点
  //计算出local_costmap 地图左下角坐标,注意该范围边框方向与栅格一样的
  if (rolling_window_)
  {
//机器人当前坐标减去局部costmap长宽的一半
    double new_origin_x = robot_x - costmap_.getSizeInMetersX() / 2;
    double new_origin_y = robot_y - costmap_.getSizeInMetersY() / 2;
//更新Origin 点位置
    costmap_.updateOrigin(new_origin_x, new_origin_y);
  }
  //初始化变量值
  minx_ = miny_ = 1e30;
  maxx_ = maxy_ = -1e30;

  //根据各层的更新情况确定master costmap地图的边界，
  for (vector<boost::shared_ptr<Layer> >::iterator plugin = plugins_.begin(); plugin != plugins_.end();++plugin)
  {
    double prev_minx = minx_; //初始化为1e30
    double prev_miny = miny_;
    double prev_maxx = maxx_; //初始化为-1e30
    double prev_maxy = maxy_;

/*在这里会调用各层的updateBounds 更新各层的costmap地图边界
最小值和最大值，以像素坐标表示，最后会取所有costmap层中(静态层，动态层)最小//和最大的范围，
作为master 综合层
*/
    (*plugin)->updateBounds(robot_x, robot_y, robot_yaw, &minx_, &miny_, &maxx_, &maxy_);

  }

//更新master costmap 综合层的边界
  int x0, xn, y0, yn;
  costmap_.worldToMapEnforceBounds(minx_, miny_, x0, y0);
  costmap_.worldToMapEnforceBounds(maxx_, maxy_, xn, yn);

  x0 = std::max(0, x0);
  xn = std::min(int(costmap_.getSizeInCellsX()), xn + 1);
  y0 = std::max(0, y0);
  yn = std::min(int(costmap_.getSizeInCellsY()), yn + 1);

  //更新master costmap 的范围
  costmap_.resetMap(x0, y0, xn, yn);

  for (vector<boost::shared_ptr<Layer> >::iterator plugin = plugins_.begin(); plugin != plugins_.end();++plugin)
  {
    //调用各层的updateCosts函数用各层的信息去更新更新范围内的地图信息
//x0, y0, xn, yn是该层costmap地图左下角和右上角坐标，以像素表示
//把master costmap 及相应范围传给各层costmap(如static costmap)
    (*plugin)->updateCosts(costmap_, x0, y0, xn, yn);

  }

  bx0_ = x0;
  bxn_ = xn;
  by0_ = y0;
  byn_ = yn;

  //这里才表明master costmap 初始化更新完成
  initialized_ = true;

}