【ROS-Navigation】Costmap2D代价地图源码解读-2

记录学习阅读ROS Navigation源码的理解，本文为Costmap2D代价地图源码学习记录，以文字总结、绘制结构图说明、代码注释为主。仍在学习过程中，有错误欢迎指正，共同进步。

LayeredCostmap类的地图更新函数主要分为两步，先更新bound，再更新cost，它调用Layer类方法，它在各层子地图中被重载。CostmapLayer类作为静态层和障碍层的基类，提供了一些对地图层进行操作的函数。

【结构示意图】

【相关文件】

• costmap_2d/src/costmap_2d_ros.cpp
• costmap_2d/src/costmap_2d.cpp
• costmap_2d/src/layered_costmap.cpp
• costmap_2d/src/costmap_layer.cpp
• costmap_2d/plugins/static_layer.cpp
• costmap_2d/plugins/obstale_layer.cpp
• costmap_2d/plugins/inflation_layer.cpp

【代码分析】

1. LayeredCostmap类

LayeredCostmap类是Costmap2DROS的成员，含有主地图，并能通过它操作各层子地图。这里关注一下两个被Costmap2DROS调用的函数。

<1> 地图尺寸设置 LayeredCostmap::resizeMap

这个函数在Costmap2DROS动态配置参数的回调函数ReconfigureCB中被调用，作用是在开启地图更新线程之前，调用Costmap2D的resizeMap函数，用给定参数重新设置主地图的尺寸、原点、分辨率，再通过plugin指针调用各层地图的matchSize，使其以上参数和主地图匹配。

void LayeredCostmap::resizeMap(unsigned int size_x, unsigned int size_y, double resolution, double origin_x,
                               double origin_y, bool size_locked)
{
  boost::unique_lock<Costmap2D::mutex_t> lock(*(costmap_.getMutex()));
  size_locked_ = size_locked;
  //调用costmap_的resizeMap方法
  costmap_.resizeMap(size_x, size_y, resolution, origin_x, origin_y);
  //然后根据plugin对每一层的地图调用其父类Costmap2D成员的initial方法，将plugin所指向的每一层地图的大小都设置为costmap_数据成员一样的空间大小
  for (vector<boost::shared_ptr<Layer> >::iterator plugin = plugins_.begin(); plugin != plugins_.end();
      ++plugin)
  {
    (*plugin)->matchSize();
  }
}

<2> 地图更新 LayeredCostmap::updateMap

这个函数在Costmap2DROS的地图更新线程中被循环调用。它分为两步：第一步：更新bound，即确定地图更新的范围；第二步：更新cost，更新每层地图cell对应的cost值后整合到主地图上。

void LayeredCostmap::updateMap(double robot_x, double robot_y, double robot_yaw)
{
  // Lock for the remainder of this function, some plugins (e.g. VoxelLayer)
  // implement thread unsafe updateBounds() functions.
  boost::unique_lock<Costmap2D::mutex_t> lock(*(costmap_.getMutex()));

rolling_window_默认为false，如果开启的话，地图是时刻跟随机器人中心移动的，这里需要根据机器人当前位置和地图大小计算出地图的新原点，设置给主地图。

  //如果我们使用窗口滚动，我们需要更新使用的机器人位置
  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;

接下来进行地图更新的第一步：更新bound
设置好minx_、miny_、maxx_、maxy_的初始值，然后对每一层的子地图调用其updateBounds函数，传入minx_、miny_、maxx_、maxy_，函数将新的bound填充进去。
updateBounds函数在Layer类中声明，在各层地图中被重载，第二步使用到的updateCosts函数也是如此。这两个函数的具体内容在各层地图部分详述。

  minx_ = miny_ = 1e30;
  maxx_ = maxy_ = -1e30;

  for (vector<boost::shared_ptr<Layer> >::iterator plugin = plugins_.begin(); plugin != plugins_.end();
       ++plugin)
  {
    double prev_minx = minx_;
    double prev_miny = miny_;
    double prev_maxx = maxx_;
    double prev_maxy = maxy_;
    //这个阶段会更新每个Layer的更新区域，这样在每个运行周期内减少了数据拷贝的操作时间。 
    //updateBounds传入的是一个矩形范围
    (*plugin)->updateBounds(robot_x, robot_y, robot_yaw, &minx_, &miny_, &maxx_, &maxy_);
    if (minx_ > prev_minx || miny_ > prev_miny || maxx_ < prev_maxx || maxy_ < prev_maxy)
    {
      ROS_WARN_THROTTLE(1.0, "Illegal bounds change, was [tl: (%f, %f), br: (%f, %f)], but "
                        "is now [tl: (%f, %f), br: (%f, %f)]. The offending layer is %s",
                        prev_minx, prev_miny, prev_maxx , prev_maxy,
                        minx_, miny_, maxx_ , maxy_,
                        (*plugin)->getName().c_str());
    }
  }

接下来调用Costmap2D类的worldToMapEnforceBounds函数，将得到的bound转换到地图坐标系。这个函数可以防止转换后的坐标超出地图范围。

  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);

  //范围更新
  ROS_DEBUG("Updating area x: [%d, %d] y: [%d, %d]", x0, xn, y0, yn);

  if (xn < x0 || yn < y0)
    return;

接下来，调用resetMap，将主地图上bound范围内的cell的cost恢复为默认值（track_unknown：255 / 否则：0），再对每层子地图调用updateCosts函数。

  costmap_.resetMap(x0, y0, xn, yn);
  for (vector<boost::shared_ptr<Layer> >::iterator plugin = plugins_.begin(); plugin != plugins_.end();
       ++plugin)
  {
    //第二步，调用layer类的updatecosts
    (*plugin)->updateCosts(costmap_, x0, y0, xn, yn);
  }

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

  initialized_ = true;
}

2. CostmapLayer类

这个类继承自Layer类和Costmap2D类，它是地图插件（静态层和障碍层）的基类。它的类方法主要用于处理bound和用几种不同的策略合并子地图和主地图。

class CostmapLayer : public Layer, public Costmap2D
{
public:
  CostmapLayer() : has_extra_bounds_(false),
    extra_min_x_(1e6), extra_max_x_(-1e6),
    extra_min_y_(1e6), extra_max_y_(-1e6) {}

  bool isDiscretized()
  {
    return true;
  }

  virtual void matchSize();

  void addExtraBounds(double mx0, double my0, double mx1, double my1);

  void updateWithTrueOverwrite(costmap_2d::Costmap2D& master_grid, int min_i, int min_j, int max_i, int max_j);
  
  void updateWithOverwrite(costmap_2d::Costmap2D& master_grid, int min_i, int min_j, int max_i, int max_j);

  void updateWithMax(costmap_2d::Costmap2D& master_grid, int min_i, int min_j, int max_i, int max_j);

  void updateWithAddition(costmap_2d::Costmap2D& master_grid, int min_i, int min_j, int max_i, int max_j);

  void touch(double x, double y, double* min_x, double* min_y, double* max_x, double* max_y);

  void useExtraBounds(double* min_x, double* min_y, double* max_x, double* max_y);
  bool has_extra_bounds_;

private:
  double extra_min_x_, extra_max_x_, extra_min_y_, extra_max_y_;
};

• 四个数据成员extra_min_x_, extra_max_x_, extra_min_y_, extra_max_y_在构造函数中被初始化为1e6和-1e6；
• matchSize函数用主地图的尺寸来设置该层地图的尺寸；
• addExtraBounds函数将传入的bound与数据成员的值比较，如果传入的bound范围更大，则更新数据成员的值；
• useExtraBounds函数在调用addExtraBounds函数后使用，它将传入的bound与更新后的数据成员比较，将更大的范围通过传入的指针填充，并恢复数据成员初始值，认为将add的bound使用过了；
• touch函数传入一个bound和一个坐标，若坐标不在bound范围内，它扩张bound，使其包含坐标；
• updateWithTrueOverwrite函数用当前子地图数据（包括未知cell）覆盖主地图对应区域；
• updateWithOverwrite函数用当前子地图数据（不包括未知cell）覆盖主地图对应区域；
• updateWithMax函数用当前子地图数据（不包括未知cell）更新主地图对应区域，若对应cell的cost值比主地图大或主地图该cell为未知时，用子地图数据覆盖，否则保留主地图原数据；
• updateWithAddition函数用当前子地图数据（不包括未知cell）更新主地图对应区域，若主地图该cell为未知，用子地图数据覆盖；否则，在主地图原数据基础上+子地图数据（将进行限制避免cost值溢出）