Costmap2D类的 setConvexPolygonCost 函数

Costmap2D类的 setConvexPolygonCost 函数

bool Costmap2D::setConvexPolygonCost(const std::vector<geometry_msgs::Point>& polygon, unsigned char cost_value)
{
  // 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;
    // 先将世界系下的多边形顶点转换到地图坐标系，并存放进map_polygon数组
    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);
  }
  std::vector<MapLocation> polygon_cells;

// 通过机器人顶点坐标数组map_polygon得到多边形边缘及内部的全部cell，存放在 polygon_cells
  // get the cells that fill the polygon
  convexFillCells(map_polygon, polygon_cells);

// set the cost of those cells
  // 通过循环对多边形边缘及内部各cell的cost赋值
  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;
  }
  return true;
}

convexFillCells

这个函数用于获取多边形边缘及内部cell

void Costmap2D::convexFillCells(const std::vector<MapLocation>& polygon, std::vector<MapLocation>& polygon_cells)
{
  // we need a minimum polygon of a triangle
  if (polygon.size() < 3)
    return;

// first get the cells that make up the outline of the polygon
   //首先获得轮廓点之间连线的列表，存放在polygon_cells中
  polygonOutlineCells(polygon, polygon_cells);

// quick bubble sort to sort points by x
  MapLocation swap;
  unsigned int i = 0;
  // 对边上的cell点的x做排序，使其按x坐标升序排列
  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;
  }
   /*  遍历所有x，对每个相同的x，检查y，获得y最大和最小的polygon cell，
   将范围内的所有cell填充进polygon_cells，获得多边形边缘及内部的所有cell   */
  i = 0;
  MapLocation min_pt;
  MapLocation max_pt;
  unsigned int min_x = polygon_cells[0].x;
  unsigned int max_x = polygon_cells[polygon_cells.size() - 1].x;

// walk through each column and mark cells inside the polygon
  for (unsigned int x = min_x; x <= max_x; ++x)
  {
    if (i >= polygon_cells.size() - 1)
      break;

if (polygon_cells[i].y < polygon_cells[i + 1].y)
    {
      min_pt = polygon_cells[i];
      max_pt = polygon_cells[i + 1];
    }
    else
    {
      min_pt = polygon_cells[i + 1];
      max_pt = polygon_cells[i];
    }

i += 2;
    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;
    }

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

循环调用raytraceLine函数，不断获取相邻之间的连线，最终组成多边形边上的cell，需要注意的是需要将最后一点和第一点连接起来，形成闭合。

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

raytraceLine

找到两点连线上的cell。对于离散的平面点，指定两个点，这个函数可以找到两个点之间的其他点，使得这些中间组成一个尽可能趋近直线的点集

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 )
{
  int dx = x1 - x0;
  int dy = y1 - y0;

unsigned int abs_dx = abs(dx);
  unsigned int abs_dy = abs(dy);

int offset_dx = sign(dx);
  int offset_dy = sign(dy) * size_x_;

unsigned int offset = y0 * size_x_ + x0;

// we need to chose how much to scale our dominant dimension, based on the maximum length of the line
  double dist = hypot(dx, dy);
  double scale = (dist == 0.0) ? 1.0 : std::min(1.0, max_length / dist);

// if x is dominant
  if (abs_dx >= abs_dy)
  {
    int error_y = abs_dx / 2;
    bresenham2D(at, abs_dx, abs_dy, error_y, offset_dx, offset_dy, offset, (unsigned int)(scale * abs_dx));
    return;
  }
  // otherwise y is dominant
  int error_x = abs_dy / 2;
  bresenham2D(at, abs_dy, abs_dx, error_x, offset_dy, offset_dx, offset, (unsigned int)(scale * abs_dy));
}