ROS Navigation Stack之costmap_2d源码分析

最新推荐文章于 2023-12-06 15:42:06 发布

茶色少年

最新推荐文章于 2023-12-06 15:42:06 发布

阅读量2.5k

点赞数 4

分类专栏： ROS开发文章标签： ros 机器人 costmap

本文链接：https://blog.csdn.net/TurboIan/article/details/86611713

版权

ROS开发专栏收录该内容

6 篇文章 1 订阅

订阅专栏

这周开始又要开始阅读navigation stack的代码了，最近一段时间本人尝试将navigation stack中的代价地图和路径规划模块提取出来，丢弃掉ROS的wrapper，以便在不同平台中使用。期间撸了一下costmap的源码，关于代码地图的加载、生成、更新有了新的认识，故整理一下。

Costmap_2D类图

costmap_2d主要类图
在Ubuntu中找了几个从源码生成uml的工具，要么收费（scitool的Understand），要么不好用（不点名），没办法，只能用宇宙IDE，一键生成类图，完美！这里只显示了几个关键的包，其余的组件包没有显示，可以单独阅读。
如果一开始没有这个类图时，容易看得云里雾里，有了这个图，整个包的结构就非常清晰了。用过的都知道，这里的代码地图是分层的，有静态地图层（StaticLayer），也有用于表示传感器信息的（ObstacleLayer）等。但现实世界中，机器人是有尺寸的，所以还需要将静态地图层和障碍物层进行膨胀，从而在规划路径的时候能够考虑机器人的尺寸，而膨胀就由InflationLayer完成。

静态地图层（StaticLayer）

静态地图层就比较简单了，map_server加载地图后，将地图发布出来，costmap_2d_ros接受到地图数据后，便加载各个层，静态地图层只是简单地将灰度图中的像素值换算成ros代价地图中的代价值，然后便进入下一层的更新了。这里只是将转换的代码贴出了说一说：

void StaticLayer::incomingMap(const nav_msgs::OccupancyGridConstPtr& new_map)
{
  // 前面就是一些检查和初始化，忽略之...
  // 这里就是将图片的像素值转换成代价值了，具体转换可以看interpretValue函数
  for (unsigned int i = 0; i < size_y; ++i)
  {
    for (unsigned int j = 0; j < size_x; ++j)
    {
      unsigned char value = new_map->data[index];
      costmap_[index] = interpretValue(value);
      ++index;
    }
  }
}

// 转换函数
unsigned char StaticLayer::interpretValue(unsigned char value)
{
  // 实际上就是高于一个阈值lethal_threshold那么就认为是障碍物
  // 没有信息的就认为是未探测的区域
  // 否则为自由空间
  if (track_unknown_space_ && value == unknown_cost_value_)
    return NO_INFORMATION;
  else if (!track_unknown_space_ && value == unknown_cost_value_)
    return FREE_SPACE;
  else if (value >= lethal_threshold_)
    return LETHAL_OBSTACLE;
  else if (trinary_costmap_)
    return FREE_SPACE;
  
  // 对于其他的数值则进行插值运算
  double scale = (double) value / lethal_threshold_;
  return scale * LETHAL_OBSTACLE;
}

障碍物层（ObstacleLayer）

障碍物层是将传感器检测到的点云投影到地图中，不同的传感器来源会分别存到Observation类中。只要遍历存储Obsrvation的容器，将检测到的点云的每个点分别投影到地图中，将对应的网格的代价值设为LETHAL_OBSTACLE，其实现函数在void ObstacleLayer::updateBounds(...)中：

void ObstacleLayer::updateBounds(double robot_x, double robot_y, double robot_yaw, double* min_x, double* min_y, double* max_x, double* max_y)
{
  ...
  // 先打扫一下旧垃圾，清除一下障碍物
  for (unsigned int i = 0; i < clearing_observations.size(); ++i)
  {
    raytraceFreespace(clearing_observations[i], min_x, min_y, max_x, max_y);
  }

  // 获取每个观察层的点云数据
  for (std::vector<Observation>::const_iterator it = observations.begin(); it != observations.end(); ++it)
  {
    const Observation& obs = *it;
    const pcl::PointCloud<pcl::PointXYZ>& cloud = *(obs.cloud_);
    double sq_obstacle_range = obs.obstacle_range_ * obs.obstacle_range_;

    //将每个点云添加进障碍物层中
    for (unsigned int i = 0; i < cloud.points.size(); ++i)
    {
      double px = cloud.points[i].x, py = cloud.points[i].y, pz = cloud.points[i].z;
      // 障碍物高度约束
      if (pz > max_obstacle_height_)
      {
        ROS_DEBUG("The point is too high");
        continue;
      }

      // 距离约束
      double sq_dist = (px - obs.origin_.x) * (px - obs.origin_.x) + (py - obs.origin_.y) * (py - obs.origin_.y) + (pz - obs.origin_.z) * (pz - obs.origin_.z);

      if (sq_dist >= sq_obstacle_range)
      {
        ROS_DEBUG("The point is too far away");
        continue;
      }

      // 检查一下这个点在不在地图中，不在地图中还放啥放，下一位！
      unsigned int mx, my;
      if (!worldToMap(px, py, mx, my))
      {
        ROS_DEBUG("Computing map coords failed");
        continue;
      }

      unsigned int index = getIndex(mx, my);
      // 查到的点设为障碍物点，投影完毕！
      costmap_[index] = LETHAL_OBSTACLE;
      // 扩张一下领地，让[x,y]囊括进边界内
      touch(px, py, min_x, min_y, max_x, max_y);
    }
  }
  // 将机器人囊括进边界内
  updateFootprint(robot_x, robot_y, robot_yaw, min_x, min_y, max_x, max_y);
}

膨胀层（InflationLayer）

膨胀层的工作原理是根据机器人的大小，生成一个膨胀代价矩阵（我自己这么叫的），然后，去遍历地图中所有障碍物的网格点，根据膨胀代价矩阵，修改障碍物网格点周围的网格点的代价值，最终实现膨胀的效果，请发挥你们的想象力。。。

void InflationLayer::updateCosts(costmap_2d::Costmap2D& master_grid, int min_i, int min_j, int max_i, int max_j)
{ 
  ...
  // 将是障碍物的网格记录到obs_bin中
  std::vector<CellData>& obs_bin = inflation_cells_[0.0];
  for (int j = min_j; j < max_j; j++)
  {
    for (int i = min_i; i < max_i; i++)
    {
      int index = master_grid.getIndex(i, j);
      unsigned char cost = master_array[index];
      if (cost == LETHAL_OBSTACLE)
      {
        obs_bin.push_back(CellData(index, i, j, i, j));
      }
    }
  }

  // Process cells by increasing distance; new cells are appended to the corresponding distance bin, so they
  // can overtake previously inserted but farther away cells
  std::map<double, std::vector<CellData> >::iterator bin;
  for (bin = inflation_cells_.begin(); bin != inflation_cells_.end(); ++bin)
  {
    for (int i = 0; i < bin->second.size(); ++i)
    {
      // process all cells at distance dist_bin.first
      const CellData& cell = bin->second[i];
      unsigned int index = cell.index_;

      // 如果访问过的网格就不在访问了
      if (seen_[index])
      {
        continue;
      }

      seen_[index] = true;

      unsigned int mx = cell.x_;
      unsigned int my = cell.y_;
      unsigned int sx = cell.src_x_;
      unsigned int sy = cell.src_y_;

      // assign the cost associated with the distance from an obstacle to the cell
      
      unsigned char cost = costLookup(mx, my, sx, sy);
      /*
    inline unsigned char costLookup(int mx, int my, int src_x, int src_y)
  	{
    	unsigned int dx = abs(mx - src_x);
    	unsigned int dy = abs(my - src_y);
    	// cached_costs_就是我说的膨胀代价矩阵，根据距离可以得到[mx,my]所在的网格的膨胀代价值
    	return cached_costs_[dx][dy];
  	}
     */
      unsigned char old_cost = master_array[index];
      // 虽然这里写的复杂，但其实就是想表示用高的代价值去替换到旧的低的代价值
      if (old_cost == NO_INFORMATION && (inflate_unknown_ ? (cost > FREE_SPACE) : (cost >= INSCRIBED_INFLATED_OBSTACLE)))
        master_array[index] = cost;
      else
        master_array[index] = std::max(old_cost, cost);

      // 下面这几句就是将[mx,my]四周的网格点也进行膨胀
      if (mx > 0)
        enqueue(index - 1, mx - 1, my, sx, sy);
      if (my > 0)
        enqueue(index - size_x, mx, my - 1, sx, sy);
      if (mx < size_x - 1)
        enqueue(index + 1, mx + 1, my, sx, sy);
      if (my < size_y - 1)
        enqueue(index + size_x, mx, my + 1, sx, sy);
    }
  }
  // 大功告成，收拾下垃圾...
  inflation_cells_.clear();
}

清除

清除代价地图的方式也很好理解，以单线激光雷达的激光线为例子，当激光线发射出去之后，在某处检测到一个障碍物，那说明：从发射的地方至某处之间是free的，那么这之间的旧的障碍物应当被删除，这之间网格的代价值应当被修改为free（在膨胀前）。

void ObstacleLayer::raytraceFreespace(const Observation& clearing_observation, double* min_x, double* min_y, double* max_x, double* max_y)
{
  double ox = clearing_observation.origin_.x;
  double oy = clearing_observation.origin_.y;
  pcl::PointCloud < pcl::PointXYZ > cloud = *(clearing_observation.cloud_);

  // 获取传感器的原点
  unsigned int x0, y0;
  if (!worldToMap(ox, oy, x0, y0))
  {
    ROS_WARN_THROTTLE(
        1.0, "The origin for the sensor at (%.2f, %.2f) is out of map bounds. So, the costmap cannot raytrace for it.",
        ox, oy);
    return;
  }
	...
  // 清除传感器原点到检测到的点之间的障碍物
  for (unsigned int i = 0; i < cloud.points.size(); ++i)
  {
    double wx = cloud.points[i].x;
    double wy = cloud.points[i].y;

    // 但是...要考虑传感器数据如果在边界外，那么要做一些特殊处理
    // 其实就是求出传感器数据点和传感器原点连成的线段和边界的交点
    // 然后清除传感器原点到交点之间的障碍物就可以了
    double a = wx - ox;
    double b = wy - oy;

    // the minimum value to raytrace from is the origin
    if (wx < origin_x)
    {
      double t = (origin_x - ox) / a;
      wx = origin_x;
      wy = oy + b * t;
    }
    if (wy < origin_y)
    {
      double t = (origin_y - oy) / b;
      wx = ox + a * t;
      wy = origin_y;
    }

    // the maximum value to raytrace to is the end of the map
    if (wx > map_end_x)
    {
      double t = (map_end_x - ox) / a;
      wx = map_end_x - .001;
      wy = oy + b * t;
    }
    if (wy > map_end_y)
    {
      double t = (map_end_y - oy) / b;
      wx = ox + a * t;
      wy = map_end_y - .001;
    }

    // now that the vector is scaled correctly... we'll get the map coordinates of its endpoint
    unsigned int x1, y1;

    // check for legality just in case
    if (!worldToMap(wx, wy, x1, y1))
      continue;

    unsigned int cell_raytrace_range = cellDistance(clearing_observation.raytrace_range_);
    MarkCell marker(costmap_, FREE_SPACE);
    // 终于...进行清除
    raytraceLine(marker, x0, y0, x1, y1, cell_raytrace_range);

    updateRaytraceBounds(ox, oy, wx, wy, clearing_observation.raytrace_range_, min_x, min_y, max_x, max_y);
  }
}

  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); // 看x是前进一格还是后退一个格子
      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));
    }

  template<class ActionType>
    inline void bresenham2D(ActionType at, unsigned int abs_da, unsigned int abs_db, int error_b, int offset_a,
                            int offset_b, unsigned int offset, unsigned int max_length)
    {
      unsigned int end = std::min(max_length, abs_da);
      for (unsigned int i = 0; i < end; ++i)
      {
        at(offset);
        offset += offset_a;
        /*
         * 下面这个情况，error_b + abs_db就会超过abs_da，所以每次x+1后，y也会+1
         *         _
         *       _|
         *     _|
         *   _|
         * _|
         *
         * 下面这个情况，error_b + n* abs_db才会超过abs_da,所以每次x+n后，y才会+1
         *             ___|
         *         ___|
         *     ___|
         * ___|
          */
        error_b += abs_db; // 为了控制前进上升的斜率
        if ((unsigned int)error_b >= abs_da)
        {
          offset += offset_b;
          error_b -= abs_da;
        }
      }
      at(offset);
    }