ROS Navigation Stack之costmap_2d源码分析

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

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