【ROS-Navigation】Costmap2D代价地图源码解读-障碍层ObstacleLayer

记录学习阅读ROS Navigation源码的理解，本文为Costmap2D代价地图源码学习记录，以文字总结、绘制结构图说明、代码注释为主。仍在学习过程中，有错误欢迎指正，共同进步。

障碍层地图通过订阅传感器话题，将传感器输出的障碍物信息存进buffer（剔除过高、过远的点），在本层地图上将观测到的点云标记为障碍物，将传感器到点云点连线上的点标记为FREE_SPACE。最后，在bound范围内，将本层地图合并到主地图上。

【结构示意图】

【相关文件】

• costmap_2d/src/costmap_2d_ros.cpp
• costmap_2d/src/costmap_2d.cpp
• costmap_2d/src/layered_costmap.cpp
• costmap_2d/src/costmap_layer.cpp
• costmap_2d/plugins/static_layer.cpp
• costmap_2d/plugins/obstale_layer.cpp
• costmap_2d/plugins/inflation_layer.cpp

本篇记录对ObstacleLayer类的阅读和理解。

【代码分析】

ObstacleLayer类

–目录–

ObstacleLayer::onInitialize | 初始化

ObstacleLayer::laserScanCallback | 回调函数

ObstacleLayer::updateBounds | 更新障碍地图边界

ObstacleLayer::raytraceFreespace | 清理传感器到障碍物间的cell

ObstacleLayer::updateCosts | 更新障碍地图代价

<1> 初始化 StaticLayer::onInitialize

首先从参数服务器加载参数，确定rolling_window_、track_unknown_space的值，并调用matchSize函数，根据主地图的参数来设置障碍层地图。

void ObstacleLayer::onInitialize()
{
  ros::NodeHandle nh("~/" + name_), g_nh;
  rolling_window_ = layered_costmap_->isRolling();
  bool track_unknown_space;
  nh.param("track_unknown_space", track_unknown_space, layered_costmap_->isTrackingUnknown());
  if (track_unknown_space)
    default_value_ = NO_INFORMATION;
  else
    default_value_ = FREE_SPACE;

  ObstacleLayer::matchSize();
  current_ = true;

  global_frame_ = layered_costmap_->getGlobalFrameID();
  double transform_tolerance;
  nh.param("transform_tolerance", transform_tolerance, 0.2);

从参数服务器加载订阅的话题名称，即障碍物信息的来源。

  std::string topics_string;
  // get the topics that we'll subscribe to from the parameter server
  nh.param("observation_sources", topics_string, std::string(""));
  ROS_INFO("    Subscribed to Topics: %s", topics_string.c_str());

  //tf前缀
  ros::NodeHandle prefix_nh;
  const std::string tf_prefix = tf::getPrefixParam(prefix_nh);

接下来进入循环，将topics_string记录的观测源逐个输出到source字符串，并找到对应每个观测源的参数。

  // now we need to split the topics based on whitespace which we can use a stringstream for
  std::stringstream ss(topics_string);
  std::string source;
  while (ss >> source)
  {
    ros::NodeHandle source_node(nh, source);

    //获取特定话题的参数
    double observation_keep_time, expected_update_rate, min_obstacle_height, max_obstacle_height;
    std::string topic, sensor_frame, data_type;
    bool inf_is_valid, clearing, marking;

    source_node.param("topic", topic, source);
    source_node.param("sensor_frame", sensor_frame, std::string(""));
    source_node.param("observation_persistence", observation_keep_time, 0.0);
    source_node.param("expected_update_rate", expected_update_rate, 0.0);
    source_node.param("data_type", data_type, std::string("PointCloud"));
    source_node.param("min_obstacle_height", min_obstacle_height, 0.0);
    source_node.param("max_obstacle_height", max_obstacle_height, 2.0);
    source_node.param("inf_is_valid", inf_is_valid, false);
    source_node.param("clearing", clearing, false);
    source_node.param("marking", marking, true);

    if (!sensor_frame.empty())
    {
      sensor_frame = tf::resolve(tf_prefix, sensor_frame);
    }

    if (!(data_type == "PointCloud2" || data_type == "PointCloud" || data_type == "LaserScan"))
    {
      ROS_FATAL("Only topics that use point clouds or laser scans are currently supported");
      throw std::runtime_error("Only topics that use point clouds or laser scans are currently supported");
    }

    std::string raytrace_range_param_name, obstacle_range_param_name;

    // get the obstacle range for the sensor
    double obstacle_range = 2.5;
    if (source_node.searchParam("obstacle_range", obstacle_range_param_name))
    {
      source_node.getParam(obstacle_range_param_name, obstacle_range);
    }

    // get the raytrace range for the sensor
    double raytrace_range = 3.0;
    if (source_node.searchParam("raytrace_range", raytrace_range_param_name))
    {
      source_node.getParam(raytrace_range_param_name, raytrace_range);
    }、

    ROS_DEBUG("Creating an observation buffer for source %s, topic %s, frame %s", source.c_str(), topic.c_str(),
              sensor_frame.c_str());

为每个观测源创建一个buffer，并将其指针存放进observation_buffers_中进行管理。并根据标志位确定是否将其添加到marking_buffers与clearing_buffers中。

    //创建一个observation buffer
    observation_buffers_.push_back(
        boost::shared_ptr < ObservationBuffer
            > (new ObservationBuffer(topic, observation_keep_time, expected_update_rate, min_obstacle_height,
                                     max_obstacle_height, obstacle_range, raytrace_range, *tf_, global_frame_,
                                     sensor_frame, transform_tolerance)));

    //检查是否要将这个buffer添加到marking observation buffers
    if (marking)
      marking_buffers_.push_back(observation_buffers_.back());

    //检查是否要将这个buffer添加到cleaning observation buffers
    if (clearing)
      clearing_buffers_.push_back(observation_buffers_.back());

    ROS_DEBUG(
        "Created an observation buffer for source %s, topic %s, global frame: %s, "
        "expected update rate: %.2f, observation persistence: %.2f",
        source.c_str(), topic.c_str(), global_frame_.c_str(), expected_update_rate, observation_keep_time);

然后分别针对不同的sensor类型（LaserScan、PointCloud、PointCloud2）如LaserScan PointCloud等注册不同的回调函数，如针对LaserScan的回调函数为laserScanCallback。

    if (data_type == "LaserScan")
    {
      boost::shared_ptr < message_filters::Subscriber<sensor_msgs::LaserScan>
          > sub(new message_filters::Subscriber<sensor_msgs::LaserScan>(g_nh, topic, 50));

      boost::shared_ptr < tf::MessageFilter<sensor_msgs::LaserScan>
          > filter(new tf::MessageFilter<sensor_msgs::LaserScan>(*sub, *tf_, global_frame_, 50));

      if (inf_is_valid)
      {
        filter->registerCallback(
            boost::bind(&ObstacleLayer::laserScanValidInfCallback, this, _1, observation_buffers_.back()));
      }
      else
      {
        filter->registerCallback(
          //回调函数：ObstacleLayer::laserScanCallback
            boost::bind(&ObstacleLayer::laserScanCallback, this, _1, observation_buffers_.back()));
      }

      observation_subscribers_.push_back(sub);
      observation_notifiers_.push_back(filter);

      observation_notifiers_.back()->setTolerance(ros::Duration(0.05));
    }
    else if (data_type == "PointCloud")
    {
      boost::shared_ptr < message_filters::Subscriber<sensor_msgs::PointCloud>
          > sub(new message_filters::Subscriber<sensor_msgs::PointCloud>(g_nh, topic, 50));

      if (inf_is_valid)
      {
       ROS_WARN("obstacle_layer: inf_is_valid option is not applicable to PointCloud observations.");
      }

      boost::shared_ptr < tf::MessageFilter<sensor_msgs::PointCloud>
          > filter(new tf::MessageFilter<sensor_msgs::PointCloud>(*sub, *tf_, global_frame_, 50));
      filter->registerCallback(
          boost::bind(&ObstacleLayer::pointCloudCallback, this, _1, observation_buffers_.back()));

      observation_subscribers_.push_back(sub);
      observation_notifiers_.push_back(filter);
    }
    else
    {
      boost::shared_ptr < message_filters::Subscriber<sensor_msgs::PointCloud2>
          > sub(new message_filters::Subscriber<sensor_msgs::PointCloud2>(g_nh, topic, 50));

      if (inf_is_valid)
      {
       ROS_WARN("obstacle_layer: inf_is_valid option is not applicable to PointCloud observations.");
      }

      boost::shared_ptr < tf::MessageFilter<sensor_msgs::PointCloud2>
          > filter(new tf::MessageFilter<sensor_msgs::PointCloud2>(*sub, *tf_, global_frame_, 50));
      filter->registerCallback(
          boost::bind(&ObstacleLayer::pointCloud2Callback, this, _1, observation_buffers_.back()));

      observation_subscribers_.push_back(sub);
      observation_notifiers_.push_back(filter);
    }

    if (sensor_frame != "")
    {
      std::vector < std::string > target_frames;
      target_frames.push_back(global_frame_);
      target_frames.push_back(sensor_frame);
      observation_notifiers_.back()->setTargetFrames(target_frames);
    }
  }

  dsrv_ = NULL;
  setupDynamicReconfigure(nh);
}

<2> 回调函数 ObstacleLayer::laserScanCallback

这里以LaserScan数据的回调函数为例，先将收到的message的laser数据转换为sensor_msgs::PointCloud2格式的数据，再调用ObservationBuffer类的bufferCloud函数，将点云数据存到buffer中。

void ObstacleLayer::laserScanCallback(const sensor_msgs::LaserScanConstPtr& message,
                                      const boost::shared_ptr<ObservationBuffer>& buffer)
{
  sensor_msgs::PointCloud2 cloud;
  cloud.header = message->header;

  // project the scan into a point cloud
  try
  {
    projector_.transformLaserScanToPointCloud(message->header.frame_id, *message, cloud, *tf_);
  }
  catch (tf::TransformException &ex)
  {
    ROS_WARN("High fidelity enabled, but TF returned a transform exception to frame %s: %s", global_frame_.c_str(),
             ex.what());
    projector_.projectLaser(*message, cloud);
  }

  // buffer the point cloud
  buffer->lock();
  buffer->bufferCloud(cloud);
  buffer->unlock();  
}

ObservationBuffer类是专门用于存储观测数据的类，它是ObstacleLayer的类成员。这里关注一下它的bufferCloud函数：当接收到sensor_msgs::PointCloud2格式的点云后，它将点云转换为pcl::PointCloud < pcl::PointXYZ >格式后，调用bufferCloud的重载函数。

void ObservationBuffer::bufferCloud(const sensor_msgs::PointCloud2& cloud)
{
 try
 {
   pcl::PCLPointCloud2 pcl_pc2;
   pcl_conversions::toPCL(cloud, pcl_pc2);
   // Actually convert the PointCloud2 message into a type we can reason about
   pcl::PointCloud < pcl::PointXYZ > pcl_cloud;
   pcl::fromPCLPointCloud2(pcl_pc2, pcl_cloud);
   bufferCloud(pcl_cloud);
 }
 catch (pcl::PCLException& ex)
 {
   ROS_ERROR("Failed to convert a message to a pcl type, dropping observation: %s", ex.what());
   return;
 }
}

在它的重载函数bufferCloud中，设置好传感器坐标系origin_frame，并将origin_frame下的传感器原点（0，0，0）转换到global系下，得到传感器的坐标，并放进buffer的observation_list_.front().origin_中。

然后将传入的点云cloud转换到global系下，得到global_frame_cloud，接下来，开始遍历点云中的点，剔除z坐标过小或过大的点，将合乎要求的点放进buffer的observation_list_.front().cloud_。这样，便得到了经过筛选后的点云及传感器原点。

void ObservationBuffer::bufferCloud(const pcl::PointCloud<pcl::PointXYZ>& cloud) {   Stamped < tf::Vector3 >
global_origin;

  // create a new observation on the list to be populated  
observation_list_.push_front(Observation());

  // check whether the origin frame has been set explicitly or whether
  //we should get it from the cloud   
string origin_frame = sensor_frame_== "" ? cloud.header.frame_id : sensor_frame_;

  try   {
    // given these observations come from sensors... we'll need to store the origin pt of the sensor
    Stamped < tf::Vector3 > local_origin(tf::Vector3(0, 0, 0),
                            pcl_conversions::fromPCL(cloud.header).stamp, origin_frame);
    tf_.waitForTransform(global_frame_, local_origin.frame_id_, local_origin.stamp_, ros::Duration(0.5));
    tf_.transformPoint(global_frame_, local_origin, global_origin);
    //将传感器原点从传感器坐标系转换到世界坐标系
    observation_list_.front().origin_.x = global_origin.getX();
    observation_list_.front().origin_.y = global_origin.getY();
    observation_list_.front().origin_.z = global_origin.getZ();

    // make sure to pass on the raytrace/obstacle range of the observation buffer to the observations
    observation_list_.front().raytrace_range_ = raytrace_range_;
    observation_list_.front().obstacle_range_ = obstacle_range_;

    pcl::PointCloud < pcl::PointXYZ > global_frame_cloud;

    // transform the point cloud
    //将点云从传感器坐标系转换到世界坐标系
    pcl_ros::transformPointCloud(global_frame_, cloud, global_frame_cloud, tf_);
    global_frame_cloud.header.stamp = cloud.header.stamp;

    // now we need to remove observations from the cloud that are below or above our height thresholds
    pcl::PointCloud < pcl::PointXYZ > &observation_cloud = *(observation_list_.front().cloud_);
    unsigned int cloud_size = global_frame_cloud.points.size();
    //根据传感器点云点数，重设buffer大小
    observation_cloud.points.resize(cloud_size);
    unsigned int point_count = 0;

    // copy over the points that are within our height bounds
    for (unsigned int i = 0; i < cloud_size; ++i)
    {
      if (global_frame_cloud.points[i].z <= max_obstacle_height_
          && global_frame_cloud.points[i].z >= min_obstacle_height_)
      {
        //复制点云，去除掉z坐标太高的点
        observation_cloud.points[point_count++] = global_frame_cloud.points[i];
      }
    }

    // resize the cloud for the number of legal points
    observation_cloud.points.resize(point_count);
    observation_cloud.header.stamp = cloud.header.stamp;
    observation_cloud.header.frame_id = global_frame_cloud.header.frame_id;   }  
    catch (TransformException&ex)   {
    // if an exception occurs, we need to remove the empty observation from the list
    observation_list_.pop_front();
    ROS_ERROR("TF Exception that should never happen for sensor frame: %s, cloud frame: %s, %s", sensor_frame_.c_str(),
              cloud.header.frame_id.c_str(), ex.what());
    return;   }

  // if the update was successful, we want to update the last updated time   
  last_updated_ = ros::Time::now();

  // we'll also remove any stale observations from the list  
purgeStaleObservations(); }

<3> 更新障碍地图边界 ObstacleLayer::updateBoundsFREE

这个函数主要完成：clearing、marking以及确定bound。

和静态地图类似，同样也是先判断是否是rolling地图，若是则更新地图原点。

void ObstacleLayer::updateBounds(double robot_x, double robot_y, double robot_yaw, double* min_x,
                                          double* min_y, double* max_x, double* max_y)
{
  if (rolling_window_)
    updateOrigin(robot_x - getSizeInMetersX() / 2, robot_y - getSizeInMetersY() / 2);
  if (!enabled_)
    return;

  useExtraBounds(min_x, min_y, max_x, max_y);

在接收到传感器数据后，buffer将被更新，同样，marking_buffers_和clearing_buffers_也更新（即buffer中的内容），将二者分别提取到observations和clearing_observations中存储。

  bool current = true;
  std::vector<Observation> observations, clearing_observations;

  // get the marking observations（observations用于“标记”）
  current = current && getMarkingObservations(observations);

  // get the clearing observations（clearing_observations用于“清理”）
  current = current && getClearingObservations(clearing_observations);

  // update the global current status
  current_ = current;

接下来是第一步，对clearing_observations中的点云点执行clearing操作，即将其与传感器的连线上的点标记为FREE_SPACE。这里调用的raytraceFreespace函数内容后述。

  // raytrace freespace
  for (unsigned int i = 0; i < clearing_observations.size(); ++i)
  {
    //首先清理出传感器与被测物之间的距离，标记为FREE_SPACE
    raytraceFreespace(clearing_observations[i], min_x, min_y, max_x, max_y);
  }

第二步是marking操作，即将点云中的点标记为障碍物。

在标记时通过二重循环，外层迭代各观测轮次，内层迭代一次观测得到的点云点，剔除本身太高（z坐标过大）、与传感器距离太远的点，将符合要求的障碍点坐标从global系转换到map系，并在本层地图上标记致命障碍。

并调用touch函数，确保标记的障碍点包含在 bound内。

  // place the new obstacles into a priority queue... each with a priority of zero to begin with
  //然后开始进入mark操作，对于每个测量到的点，标记为obstacle
  //即迭代点云中的点，本身太高太远的障碍和跟其他障碍点离得太远的点都不考虑
  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)
    {
      //把点云中的点坐标记录为px py pz
      double px = cloud.points[i].x, py = cloud.points[i].y, pz = cloud.points[i].z;

      //如果障碍太高，则不加入mark
      if (pz > max_obstacle_height_)
      {
        ROS_DEBUG("The point is too high");
        continue;
      }

      // compute the squared distance from the hitpoint to the pointcloud's origin
      //计算点与点之间的距离平方
      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;
      touch(px, py, min_x, min_y, max_x, max_y);
    }
  }

在以上两步完成后，调用updateFootprint函数，它的作用是基于机器人当前位置确定该位置下的足迹，并在内部调用touch函数保证足迹包含在bound范围内。

  updateFootprint(robot_x, robot_y, robot_yaw, min_x, min_y, max_x, max_y);
}

<4> 清理传感器到障碍物间的cell ObstacleLayer::raytraceFreespace

先将传感器坐标转换到地图坐标系。

void ObstacleLayer::raytraceFreespace(const Observation& clearing_observation, double* min_x, double* min_y,
                                              double* max_x, double* max_y)
{
  //clearing_observation的origin_是传感器坐标，传入
  double ox = clearing_observation.origin_.x;
  double oy = clearing_observation.origin_.y;
  pcl::PointCloud < pcl::PointXYZ > cloud = *(clearing_observation.cloud_);

  // get the map coordinates of the origin of the sensor
  //得到传感器原点在地图上的坐标
  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;
  }

  // we can pre-compute the enpoints of the map outside of the inner loop... we'll need these later
  double origin_x = origin_x_, origin_y = origin_y_;
  double map_end_x = origin_x + size_x_ * resolution_;
  double map_end_y = origin_y + size_y_ * resolution_;

  // 保证传感器原点在bound范围内
  touch(ox, oy, min_x, min_y, max_x, max_y);

接下来的几个判断结构实际上做的主要工作就是不断迭代传感器原点和点云中的点的连线，并对其调用raytraceLine函数，将连线上的点在本层地图上全部标记为FREE_SPACE。

这里获取连线时，要注意该点是否超出地图范围，如果超出，则通过相似三角形去除连线在地图外的部分。

  //对于点云中的每个点，我们要追踪原点和clear obstacles之间的一条线
  for (unsigned int i = 0; i < cloud.points.size(); ++i)
  {
    //wx wy是当前点云中的点的坐标
    double wx = cloud.points[i].x;
    double wy = cloud.points[i].y;

    //a、b是该点跟传感器原点的距离
    double a = wx - ox;
    double b = wy - oy;

    //如果当前点x方向比地图原点还小
    if (wx < origin_x)
    {
      //t（比例）=（地图原点-传感器原点）/（点云中的该点-传感器原点）
      double t = (origin_x - ox) / a;
      //当前点x = 地图原点x
      wx = origin_x;
      //当前点y = 
      //实际上还是把点云点和传感器连线之间清空，只是通过相似三角形丢弃了超出地图原点范围外的部分，下面三个判断结构同理
      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;
    }

    unsigned int x1, y1;

    //转换回map坐标系
    if (!worldToMap(wx, wy, x1, y1))
      continue;

    unsigned int cell_raytrace_range = cellDistance(clearing_observation.raytrace_range_);
    MarkCell marker(costmap_, FREE_SPACE);
    // and finally... we can execute our trace to clear obstacles along that line
    //调用raytraceLine函数清理传感器原点和障碍物点之间的cell
    raytraceLine(marker, x0, y0, x1, y1, cell_raytrace_range);
    //updateRaytraceBounds函数用来根据测量的距离，更新扩张
    updateRaytraceBounds(ox, oy, wx, wy, clearing_observation.raytrace_range_, min_x, min_y, max_x, max_y);
  }
}

updateRaytraceBounds函数的作用是保证连线上的一点（距离传感器特定范围内）被包含进bound。

<5> 更新障碍地图代价 ObstacleLayer::updateCosts

这个函数就是将机器人足迹范围内设置为FREE_SPACE，并且在bound范围内将本层障碍地图的内容合并到主地图上。

void ObstacleLayer::updateCosts(costmap_2d::Costmap2D& master_grid, int min_i, int min_j, int max_i, int max_j)
{
  if (!enabled_)
    return;

  if (footprint_clearing_enabled_)
  { 
    //设置机器人所在区域为FREE_SPACE
    setConvexPolygonCost(transformed_footprint_, costmap_2d::FREE_SPACE);
  }

  switch (combination_method_)
  {
    case 0:  // Overwrite
      updateWithOverwrite(master_grid, min_i, min_j, max_i, max_j);
      break;
    case 1:  // Maximum
      updateWithMax(master_grid, min_i, min_j, max_i, max_j);
      break;
    default:  // Nothing
      break;
  }
}

Neo 2020.3