ROS naviagtion analysis: costmap_2d--Costmap2D

Costmap2D是存储地图数据的父类。真正的地图数据就存储在数据成员unsigned char *costmap_ 。
首先，分析类的构造函数:
默认构造函数：Costmap2D::Costmap2D() :

// just initialize everything to NULL by defaultCostmap2D::Costmap2D() :    size_x_(0), size_y_(0), resolution_(0.0), origin_x_(0.0), origin_y_(0.0), costmap_(NULL){  access_ = new mutex_t();}

带参数的构造函数：Costmap2D::Costmap2D(unsigned int cells_size_x, unsigned int cells_size_y, double resolution, double origin_x, double origin_y, unsigned char default_value)

Costmap2D::Costmap2D(unsigned int cells_size_x, unsigned int cells_size_y, double resolution,                     double origin_x, double origin_y, unsigned char default_value) :    size_x_(cells_size_x), size_y_(cells_size_y), resolution_(resolution), origin_x_(origin_x),    origin_y_(origin_y), costmap_(NULL), default_value_(default_value){  access_ = new mutex_t();  // create the costmap  initMaps(size_x_, size_y_);  resetMaps();}

Copy 构造函数：Costmap2D::Costmap2D(const Costmap2D& map)

Costmap2D::Costmap2D(const Costmap2D& map) :    costmap_(NULL){  access_ = new mutex_t();  *this = map;}

Assignment 构造函数：Costmap2D& Costmap2D::operator=(const Costmap2D& map)

Costmap2D& Costmap2D::operator=(const Costmap2D& map){  // check for self assignement  if (this == &map)    return *this;  // clean up old data  deleteMaps();  size_x_ = map.size_x_;  size_y_ = map.size_y_;  resolution_ = map.resolution_;  origin_x_ = map.origin_x_;  origin_y_ = map.origin_y_;  // initialize our various maps  initMaps(size_x_, size_y_);  // copy the cost map  memcpy(costmap_, map.costmap_, size_x_ * size_y_ * sizeof(unsigned char));  return *this;}

每次对costmap_ 操作都需要上锁access_=new mutex_t(), ‘mutex_t’ 实际定义是typedef boost::recursive_mutex mutex_t; 递归锁。

函数Costmap2D::setConvexPolygonCost：
首先将机器人坐标系下的机器人轮廓点，全部转到地图坐标系下

 // we assume the polygon is given in the global_frame... we need to transform it to map coordinates  std::vector<MapLocation> map_polygon;  for (unsigned int i = 0; i < polygon.size(); ++i)  {    MapLocation loc;    if (!worldToMap(polygon[i].x, polygon[i].y, loc.x, loc.y))    {      // ("Polygon lies outside map bounds, so we can't fill it");      return false;    }    map_polygon.push_back(loc);  }

然后通过下面的调用，得到在polygon内部的全部cell，存储在polygon_cells：

  std::vector<MapLocation> polygon_cells;  // get the cells that fill the polygon  // this function is to get all the cells inside the polygon  convexFillCells(map_polygon, polygon_cells);

然后获取这些内部cell的index，再对地图costmap_ 遍历进行赋值操作：

  // set the cost of those cells  for (unsigned int i = 0; i < polygon_cells.size(); ++i)  {    unsigned int index = getIndex(polygon_cells[i].x, polygon_cells[i].y);    costmap_[index] = cost_value;  }

那么问题 来了，convexFillCells(map_polygon, polygon_cells); 是怎么获取到的全部的内部点的呢？

  // first get the cells that make up the outline of the polygon  // this function will get the edges along the polygon  polygonOutlineCells(polygon, polygon_cells);

首先获得轮廓点之间连线的cell的列表。然后对这些边缘点做一次排序：

  MapLocation swap;  unsigned int i = 0;  while (i < polygon_cells.size() - 1)  {    if (polygon_cells[i].x > polygon_cells[i + 1].x)    {      swap = polygon_cells[i];      polygon_cells[i] = polygon_cells[i + 1];      polygon_cells[i + 1] = swap;      if (i > 0)        --i;    }    else      ++i;  }

操作完成后得到的polygon_cells 的cell都按照x坐标从小到大排序好了。然后开始沿着x轴，对每个相同的x，检查y值，获取y值最大的和y值最小的polygoncell：

   while (i < polygon_cells.size() && polygon_cells[i].x == x)    {      if (polygon_cells[i].y < min_pt.y)        min_pt = polygon_cells[i];      else if (polygon_cells[i].y > max_pt.y)        max_pt = polygon_cells[i];      ++i;    }

最后将y最大的和y最小的整个列的所有cell全部都塞进polygon_cells去：

    MapLocation pt;    // loop though cells in the column    for (unsigned int y = min_pt.y; y < max_pt.y; ++y)    {      pt.x = x;      pt.y = y;      polygon_cells.push_back(pt);    }

回到刚才，根据轮廓点，就能获得轮廓点连线的全部的边缘点函数polygonOutlineCells：

void Costmap2D::polygonOutlineCells(const std::vector<MapLocation>& polygon, std::vector<MapLocation>& polygon_cells){  PolygonOutlineCells cell_gatherer(*this, costmap_, polygon_cells);  for (unsigned int i = 0; i < polygon.size() - 1; ++i)  {    raytraceLine(cell_gatherer, polygon[i].x, polygon[i].y, polygon[i + 1].x, polygon[i + 1].y);  }  if (!polygon.empty())  {    unsigned int last_index = polygon.size() - 1;    // we also need to close the polygon by going from the last point to the first    raytraceLine(cell_gatherer, polygon[last_index].x, polygon[last_index].y, polygon[0].x, polygon[0].y);  }}

主要的被调用的函数如下，它调用了bresenham2D 函数，这个算法实现了 对于离散的平面点，指定两个点，找到两点之间的其他点，使得这些中间组成一个尽可能趋近直线的点集。

template<class ActionType>inline void raytraceLine(ActionType at, unsigned int x0, unsigned int y0, unsigned int x1, unsigned int y1,unsigned int max_length = UINT_MAX)

函数bool Costmap2D::saveMap(std::string file_name) 执行将costmap2D类中的costmap_这个指针指向的数据全部存储成文件。由于数据本身是一维的，所以需要在文件开头写入x，y的各自size值，另外加上一个分隔符0xff与地图数据分开。
Costmap2D 类分析就是这么多，相比之前的简单得多，毕竟主要是作为父类，供obstacle ，inflation，static， voxel继承用的。