后面的observations是下面这样获得的

std::vector<Observation>  observations;
// get the marking observations
getMarkingObservations(observations);

Observation保存的是 observation in terms of a point cloud and the origin of the source，有4个成员变量：

• geometry_msgs::Point origin_; The origin point of the observation
• pcl::PointCloud<pcl::PointXYZ>* cloud_; point cloud of the observation
• double obstacle_range_; The range out to which an observation should be able to insert obstacles
• double raytrace_range_; The range out to which an observation should be able to clear via raytracing

获得的observations实际上是从bufferCloud里的observation_list_而来。

// place the new obstacles into a priority queue
// each with a priority of zero to begin with
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_);
    // obstacle_range_ 其实就是参数 obstacle_range
    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 the obstacle is too high or too far away from the robot we won't add it
      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 the point is far enough away... we won't consider it
      if (sq_dist >= sq_obstacle_range)
      {
        ROS_DEBUG("The point is too far away");
        continue;
      }
      // now we need to compute the map coordinates for the observation
      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(robot_x, robot_y, robot_yaw, min_x, min_y, max_x, max_y);

对于多线雷达，(pz - obs.origin_.z) * (pz - obs.origin_.z)是0，可以考虑把这段去掉。 另外点云数据量比单线大了很多，在检测近处障碍的时候，计算sq_dist会得到很多点(虽然delta_z不是0)。单线雷达扫描的是一条线，多线其实是很多条线，但是代价地图是2D的，投影会有很多重复的，需要针对xy去重，显著降低了效率，这又是costmap_2d不适合多线雷达的一个例子。 另外这里就是参数obstacle_range出现的地方。