【ROS-Navigation】Costmap2d代价地图源码分析——ObstacleLayer障碍物层

在学习ROS-Navigation源码过程中，记录自己的理解，本文为分层代价地图中障碍物层源码的学习笔记，针对obstacle_layer.h和obstacle_layer.cpp源码文件，分析障碍物层的建图原理和相应代码的注释。笔者仍在学习阶段，欢迎感兴趣的道友一起交流学习，共同成长。

左侧专栏还在更新ROS-Navigation 包其他源码分析，可以关注一波噢？

如果觉得写得还不错，就请收藏一下啦～～～

---

---

前言

障碍物层的原理简述：ROS-Navigation中的代价地图为分层代价地图，包括了静态层、障碍层和膨胀层。机器人在进行障碍物层的处理之前，已完成静态层（static_layer)的地图构建，以及障碍层和膨胀层的相关准备工作，障碍物层以点云或激光扫描的形式包含来自传感器的信息，以两个维度跟踪。

---

提示：以下是本篇文章正文内容

1. 相关源码文件

1.1 主要头文件（costmap_2d/include）

• costmap_2d_publisher.h
• costmap_2d_ros.h
• costmap_2d.h
• costmap_layer.h
• inflation_layer.h
• layer.h
• layered_costmap.h
• obstacle_layer.h
• static_layer.h
• voxel_layer.h

1.2 主要源代码文件（costmap_2d）

• plugins/static_layer.cpp
• plugins/obstacle_layer.cpp
• plugins/inflation_layer.cpp
• plugins/voxel_layer.cpp
• src/costmap_2d_publisher.cpp
• src/costmap_2d_ros.cpp
• src/costmap_2d.cpp
• src/costmap_layer.cpp
• src/layer.cpp
• src/layered_costmap.cpp

2. 代码分析

2.1 obstacle_layer.h

头文件： 函数原型、内联函数

公共成员函数继承自：costmap_2d::CostmapLayer、costmap_2d::Layer和costmap_2d::Costmap2D

  public:
  ObstacleLayer()
  {
    costmap_ = NULL; 		// 将障碍层设置为空值
  }

  virtual ~ObstacleLayer();
  virtual void onInitialize();
  virtual void updateBounds(double robot_x, double robot_y, double robot_yaw, double* min_x, double* min_y,
                            double* max_x, double* max_y);
  virtual void updateCosts(costmap_2d::Costmap2D& master_grid, int min_i, int min_j, int max_i, int max_j);

  virtual void activate();
  virtual void deactivate();
  virtual void reset();

  void laserScanCallback(const sensor_msgs::LaserScanConstPtr& message,
                         const boost::shared_ptr<costmap_2d::ObservationBuffer>& buffer);	// 处理缓冲激光扫描消息的回调
  void laserScanValidInfCallback(const sensor_msgs::LaserScanConstPtr& message,
                                 const boost::shared_ptr<ObservationBuffer>& buffer);	// 用于处理缓冲激光扫描消息的回调，这些消息需要过滤以将Inf值转换为range_max
  void pointCloudCallback(const sensor_msgs::PointCloudConstPtr& message,
                          const boost::shared_ptr<costmap_2d::ObservationBuffer>& buffer);	// 处理缓冲点云消息的回调
  void pointCloud2Callback(const sensor_msgs::PointCloud2ConstPtr& message,
                           const boost::shared_ptr<costmap_2d::ObservationBuffer>& buffer);
                           
  // 用于测试目的
  void addStaticObservation(costmap_2d::Observation& obs, bool marking, bool clearing);
  void clearStaticObservations(bool marking, bool clearing);

保护成员函数及属性继承自：costmap_2d::CostmapLayer和costmap_2d::Costmap2D

protected:
  virtual void setupDynamicReconfigure(ros::NodeHandle& nh);
  // 获取用于标记空间的观察值
  // 参数：marking_observations 对将填充观测值的向量的引用
  // 如果所有观察缓冲区都是当前的，则返回True，否则返回false
  bool getMarkingObservations(std::vector<costmap_2d::Observation>& marking_observations) const;
  // 获取用于清除空间的观测值
  // 参数：clearing_observations 对将填充观测值的向量的引用
  // 如果所有观察缓冲区都是当前的，则返回True，否则返回false
  bool getClearingObservations(std::vector<costmap_2d::Observation>& clearing_observations) const;
  // 基于一次观测的清除自由空间
  virtual void raytraceFreespace(const costmap_2d::Observation& clearing_observation, double* min_x, double* min_y,
                                 double* max_x, double* max_y);

  void updateRaytraceBounds(double ox, double oy, double wx, double wy, double range, double* min_x, double* min_y,
                            double* max_x, double* max_y);

  std::vector<geometry_msgs::Point> transformed_footprint_;
  bool footprint_clearing_enabled_;
  void updateFootprint(double robot_x, double robot_y, double robot_yaw, double* min_x, double* min_y, 
                       double* max_x, double* max_y);

  std::string global_frame_; 
  double max_obstacle_height_; 
  laser_geometry::LaserProjection projector_; 		// 用于将激光扫描到的信息投影到点云集中
  
  std::vector<boost::shared_ptr<message_filters::SubscriberBase> > observation_subscribers_;  // 用于观察消息筛选器
  std::vector<boost::shared_ptr<tf2_ros::MessageFilterBase> > observation_notifiers_; 	// 用于确保每个传感器都可以使用变换
  std::vector<boost::shared_ptr<costmap_2d::ObservationBuffer> > observation_buffers_; 	// 用于存储来自各种传感器的观测值
  std::vector<boost::shared_ptr<costmap_2d::ObservationBuffer> > marking_buffers_; 	// 用于存储用于标记障碍物的观察缓冲器
  std::vector<boost::shared_ptr<costmap_2d::ObservationBuffer> > clearing_buffers_;  // 用于存储用于清除障碍物的观察缓冲器
  std::vector<costmap_2d::Observation> static_clearing_observations_, static_marking_observations_;  // 仅用于测试目的

  bool rolling_window_;
  dynamic_reconfigure::Server<costmap_2d::ObstaclePluginConfig> *dsrv_;

  int combination_method_;

私有成员函数：costmap_2d::CostmapLayer、costmap_2d::Layer和costmap_2d::Costmap2D

void reconfigureCB(costmap_2d::ObstaclePluginConfig &config, uint32_t level);

2.2 obstacle_layer.cpp

障碍层插件和名称空间声明：

PLUGINLIB_EXPORT_CLASS(costmap_2d::ObstacleLayer, costmap_2d::Layer)

using costmap_2d::NO_INFORMATION;
using costmap_2d::LETHAL_OBSTACLE;
using costmap_2d::FREE_SPACE;

using costmap_2d::ObservationBuffer;
using costmap_2d::Observation;

（1）初始化ObstacleLayer::onInitialize()

1. 创建多个节点句柄nh("~/" + name_)、g_nh、prefix_nh和source_node(nh, source)，通过方法param(param_name, param_val, default_val)从参数服务器检索指示的值param_name,若成功，则将结果存储在param_val中，否则将default_val赋值给param_val。
2. 为"PointCloud2"、"PointCloud"和"LaserScan"类型的数据，分调用回调函数处理传感器数据。
3. 将回调函数处理后的数据添加到观察缓冲区

  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(); // 设置障碍物层的代价值、x值、y值、分辨率和原点坐标
  current_ = true;

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

  std::string topics_string;
  // 从参数服务器获取所订阅的话题
  nh.param("observation_sources", topics_string, std::string(""));
  ROS_INFO("    Subscribed to Topics: %s", topics_string.c_str());

  // 使用字符串流
  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");
    }		// 对传感器数据做进一步的判断，若不是"PointCloud2"、"PointCloud"和"LaserScan"，则抛出异常错误信息。

    std::string raytrace_range_param_name, obstacle_range_param_name;

    // 确定传感器所能探测到障碍物的最大范围值
    double obstacle_range = 2.5;
    if (source_node.searchParam("obstacle_range", obstacle_range_param_name))
    {
      source_node.getParam(obstacle_range_param_name, obstacle_range);
    }

    // 确定传感器所能探测到周围环境的最大范围值
    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());

    // 创建观察缓冲区ObservationBuffer(),其中push_back()在 observation_buffers_最后添加一个元素（参数为要插入的值）
    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)));

    // 检查是否将此缓冲区添加到标记观察缓冲区
    // 此时marking_buffers_虽添加了观察区的数据，但还未调用传感器数据回调函数，其值为空值，
    if (marking)
      marking_buffers_.push_back(observation_buffers_.back()); 	

    // 检查是否还要将此缓冲区添加到清除观察缓冲区
    // 与上面一致
    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);

    // 为"PointCloud2"、"PointCloud"和"LaserScan"类型的数据，分调用回调函数，用于传感器数据的处理
    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()));	
          	// 类方法调用，boost::bind(&类名::方法名，类实例指针，参数1，参数2）
          	// 对LaserScan转换后的数据添加到observation_buffers_
      }
      else
      {
        filter->registerCallback(
            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::setupDynamicReconfigure()

void ObstacleLayer::setupDynamicReconfigure(ros::NodeHandle& nh)
{
  dsrv_ = new dynamic_reconfigure::Server<costmap_2d::ObstaclePluginConfig>(nh);
  dynamic_reconfigure::Server<costmap_2d::ObstaclePluginConfig>::CallbackType cb = boost::bind(
      &ObstacleLayer::reconfigureCB, this, _1, _2);
  dsrv_->setCallback(cb);
}

（3）析构ObstacleLayer::~ObstacleLayer()

ObstacleLayer::~ObstacleLayer()
{
    if (dsrv_)
        delete dsrv_;
}

（4）重新配置ObstacleLayer::reconfigureCB()

void ObstacleLayer::reconfigureCB(costmap_2d::ObstaclePluginConfig &config, uint32_t level)
{
  enabled_ = config.enabled;
  footprint_clearing_enabled_ = config.footprint_clearing_enabled;
  max_obstacle_height_ = config.max_obstacle_height;
  combination_method_ = config.combination_method;
}

（5）处理缓冲激光扫描消息的回调ObstacleLayer::laserScanCallback()

void ObstacleLayer::laserScanCallback(const sensor_msgs::LaserScanConstPtr& message,
                                      const boost::shared_ptr<ObservationBuffer>& buffer)
{
  // 将激光转换到点云中
  sensor_msgs::PointCloud2 cloud;
  cloud.header = message->header;

  // 将扫描转换到点云中
  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->lock();	// 锁定
  buffer->bufferCloud(cloud);	// 缓冲区
  buffer->unlock();		// 解除锁定
}

（6）处理需要过滤才能将 Inf 值转换为range_max的激光扫描消息的回调ObstacleLayer::laserScanValidInfCallback()

void ObstacleLayer::laserScanValidInfCallback(const sensor_msgs::LaserScanConstPtr& raw_message,
                                              const boost::shared_ptr<ObservationBuffer>& buffer)
{
  float epsilon = 0.0001;
  sensor_msgs::LaserScan message = *raw_message;
  for (size_t i = 0; i < message.ranges.size(); i++)
  {
    float range = message.ranges[ i ];
    if (!std::isfinite(range) && range > 0)
    {
      message.ranges[ i ] = message.range_max - epsilon;
    }
  }
  sensor_msgs::PointCloud2 cloud;
  cloud.header = message.header;
  
  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->lock();
  buffer->bufferCloud(cloud);
  buffer->unlock();
}

（7）处理缓冲点云消息的回调ObstacleLayer::pointCloudCallback()

void ObstacleLayer::pointCloudCallback(const sensor_msgs::PointCloudConstPtr& message,
                                               const boost::shared_ptr<ObservationBuffer>& buffer)
{
  sensor_msgs::PointCloud2 cloud2;

  if (!sensor_msgs::convertPointCloudToPointCloud2(*message, cloud2))
  {
    ROS_ERROR("Failed to convert a PointCloud to a PointCloud2, dropping message");
    return;
  }

  buffer->lock();
  buffer->bufferCloud(cloud2);
  buffer->unlock();
}

（8）处理缓冲点云2消息的回调ObstacleLayer::pointCloud2Callback()

void ObstacleLayer::pointCloud2Callback(const sensor_msgs::PointCloud2ConstPtr& message,
                                                const boost::shared_ptr<ObservationBuffer>& buffer)
{
  buffer->lock();
  buffer->bufferCloud(*message);
  buffer->unlock();
}

（9）更新边界ObstacleLayer::updateBounds()

// 传入参数
// robot_x、robot_y和robot_yaw 机器人的x，y坐标值和偏航角
// min_y、min_y、max_x和max_y 更新边界轮廓
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() 确定更新边界轮廓的原点坐标 
  // getSizeInMetersX() 返回的是 （size_x_ - 1 + 0.5) * resolution_
  // getSizeInMetersY() 返回的是 （size_y_ - 1 + 0.5) * resolution_
    updateOrigin(robot_x - getSizeInMetersX() / 2, robot_y - getSizeInMetersY() / 2);
  if (!enabled_)	// 判断更新边界轮廓对象是否有效
    return;
    // 确定更新边界轮廓尺寸
  useExtraBounds(min_x, min_y, max_x, max_y);

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

  // 获取标记观察
  current = current && getMarkingObservations(observations);

  // 获取清除观察
  current = current && getClearingObservations(clearing_observations);

  // 更新全局当前状态
  current_ = current;

  // 光线跟踪自由空间
  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;
      touch(px, py, min_x, min_y, max_x, max_y);	// 更新参数中指定的边界框以包括位置（x，y）
    }
  }

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

（10）更新机器人足迹ObstacleLayer::updateFootprint()

void ObstacleLayer::updateFootprint(double robot_x, double robot_y, double robot_yaw, double* min_x, double* min_y,
                                    double* max_x, double* max_y)
{
    if (!footprint_clearing_enabled_) return;
    transformFootprint(robot_x, robot_y, robot_yaw, getFootprint(), transformed_footprint_);

    for (unsigned int i = 0; i < transformed_footprint_.size(); i++)
    {
      touch(transformed_footprint_[i].x, transformed_footprint_[i].y, min_x, min_y, max_x, max_y);
    }
}

（11）更新代价值ObstacleLayer::updateCosts()

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_)
  {
  	// setConvexPolygonCost将给定在全局坐标系的机器人足迹清除轮廓多边形坐标映射到地图坐标系中，根据映射后的坐标索引设置为自由空间代价值
  	// transformed_footprint_----->传递机器人足迹的尺寸，各个顶点的坐标值
  	// costmap_2d::FREE_SPACE----->自由空间
    setConvexPolygonCost(transformed_footprint_, costmap_2d::FREE_SPACE);
  }

  switch (combination_method_)
  {
    case 0: 
    	// 覆盖更新（updateWithOverwrite），将所操作的地图层的每个有效值（不含未知状态（NO_INFORMATION））写进主栅格地图
    	// master_grid----->在所操作的地图层的特定边界盒子（二维环境为矩形）的主栅格对象，
    	// min_i、min_j----->特定边界盒子的左下角顶点的x、y坐标
    	// max_i、max_j----->特定边界盒子的右上角顶点的x、y坐标
      updateWithOverwrite(master_grid, min_i, min_j, max_i, max_j);
      break;
    case 1:  
    	// 最大值更新（updateWithMax），将主栅格的新值更新为所操作的地图层值和主栅格层值中的最大值
    	// 如果主代价值是未知状态（NO_INFORMATION），则被覆盖更新
    	// 如果所操作的地图层的代价值是未知状态（NO_INFORMATION），则主代价值不变
      updateWithMax(master_grid, min_i, min_j, max_i, max_j);
      break;
    default:  // Nothing
      break;
  }
}

（12）添加静态观察ObstacleLayer::addStaticObservation()

void ObstacleLayer::addStaticObservation(costmap_2d::Observation& obs, bool marking, bool clearing)
{
  if (marking)
    static_marking_observations_.push_back(obs);
  if (clearing)
    static_clearing_observations_.push_back(obs);
}

（13）清除静态观察ObstacleLayer::clearStaticObservations()

void ObstacleLayer::clearStaticObservations(bool marking, bool clearing)
{
  if (marking)
    static_marking_observations_.clear();
  if (clearing)
    static_clearing_observations_.clear();
}

（14）获取标记观察ObstacleLayer::getMarkingObservations()

bool ObstacleLayer::getMarkingObservations(std::vector<Observation>& marking_observations) const
{
  bool current = true;
  // 获得标记观察
  for (unsigned int i = 0; i < marking_buffers_.size(); ++i)
  {
    marking_buffers_[i]->lock();	// 锁定观察缓冲区
    marking_buffers_[i]->getObservations(marking_observations); // 将所有当前观察的副本添加到执行的向量末端 
    current = marking_buffers_[i]->isCurrent() && current;  // 检查观察缓冲区是否按预期率更新
    marking_buffers_[i]->unlock();
  }
  marking_observations.insert(marking_observations.end(),
                              static_marking_observations_.begin(), static_marking_observations_.end());
                              // 在指定位置marking_observations.end()前“插入”区间[static_marking_observations_.begin(), static_marking_observations_.end())的所有元素
  return current;
}

（15）获取清除观察ObstacleLayer::getClearingObservations()

// 
bool ObstacleLayer::getClearingObservations(std::vector<Observation>& clearing_observations) const
{
  bool current = true;
  // 获取清除观察
  for (unsigned int i = 0; i < clearing_buffers_.size(); ++i)
  {
    clearing_buffers_[i]->lock();
    clearing_buffers_[i]->getObservations(clearing_observations);
    current = clearing_buffers_[i]->isCurrent() && current;
    clearing_buffers_[i]->unlock();
  }
  clearing_observations.insert(clearing_observations.end(),
                              static_clearing_observations_.begin(), static_clearing_observations_.end());
  return current;
}

（16）射线自由空间（）ObstacleLayer::raytraceFreespace()

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

  // 我们可以在内循环外预先计算地图的点...我们以后需要这些
  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_;
  
  touch(ox, oy, min_x, min_y, max_x, max_y);

  // 对于点云中的每个点，我们希望从原点追踪一条线，并清除沿途的障碍物
  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;

    // 光线跟踪的最小值是原点
    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;
    }

    // 光线跟踪到的最大值是地图的末端
    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;

    // 检查合法性，以防万一
    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);
  }
}

（17）激活ObstacleLayer::activate()

void ObstacleLayer::activate()
{
  // 如果停止，我们需要重新订阅主题
  for (unsigned int i = 0; i < observation_subscribers_.size(); ++i)
  {
    if (observation_subscribers_[i] != NULL)
      observation_subscribers_[i]->subscribe();
  }

  for (unsigned int i = 0; i < observation_buffers_.size(); ++i)
  {
    if (observation_buffers_[i])
      observation_buffers_[i]->resetLastUpdated();
  }
}

（18）停用ObstacleLayer::deactivate()

void ObstacleLayer::deactivate()
{
  for (unsigned int i = 0; i < observation_subscribers_.size(); ++i)
  {
    if (observation_subscribers_[i] != NULL)
      observation_subscribers_[i]->unsubscribe();
  }
}

（19）更新跟踪边界ObstacleLayer::updateRaytraceBounds()

void ObstacleLayer::updateRaytraceBounds(double ox, double oy, double wx, double wy, double range,
                                         double* min_x, double* min_y, double* max_x, double* max_y)
{
  double dx = wx-ox, dy = wy-oy;
  double full_distance = hypot(dx, dy);
  double scale = std::min(1.0, range / full_distance);
  double ex = ox + dx * scale, ey = oy + dy * scale;
  touch(ex, ey, min_x, min_y, max_x, max_y);
}

（20）重置ObstacleLayer::reset()

void ObstacleLayer::reset()
{
    deactivate();
    resetMaps();
    current_ = true;
    activate();
}

---

参考：
https://github.com/ros-planning/navigation/tree/kinetic-devel
http://wiki.ros.org/costmap_2d

obstacle_layer就是以上分析，内容较多，可以收藏一下，方便以后查看噢～～～如果觉得内容不错，别忘了点赞一下啦