ROS naviagtion analysis: costmap_2d--Costmap2D

博客转载自：https://blog.csdn.net/u013158492/article/details/50492506

Costmap2D是存储地图数据的父类。真正的地图数据就存储在数据成员unsigned char *costmap_ 。

首先，分析类的构造函数:，默认构造函数：Costmap2D::Costmap2D() 

// just initialize everything to NULL by default
Costmap2D::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继承用的。