栅格地图、障碍物地图与膨胀地图（障碍物地图）

前面我们看完了栅格地图，知道了地图的基本数据结构，今天进一步的看一下障碍物地图。障碍物地图的存在更多是用于局部路径规划中所使用，因为大部分时候全局地图都是比较大的，那么很难保证其始终是一成不变的，所以如果我们只是按照全局地图进行路径规划，很可能会出现原有的地图中没有障碍物时，突然出现了一个障碍物，那么此时仅使用静态地图就容易出错，而同时考虑障碍物地图的话可以将这些新增的障碍物考虑进去，那么在做路径规划时就可以更好的避免一些原本不存在的障碍物阻挡设备运行的情况。

move_base中对于障碍物地图的一些处理

1 初始化Buffer

在move_base中，障碍物地图本身是单独维护的。在ObstacleLayer的onInitialize函数中，我们可以看到其创建了一个指向ObservationBuffer的智能指针

 // create an 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)));

这个类的作用是：从传感器中获取点云，将其转换到所需的坐标系下并进行存储。这个类的初始化需要传递挺多参数的。首先是一个topic，应该就是观测数据来自的数据源了。然后是一个时间参数observation_keep_time，这个参数字面意思很好理解，观测数据保留的时间。这个参数是什么意思呢，它代表的是这帧观测数据能够在ObservationBuffer中所保留的时间，因为ObservationBuffer中其实保留的可以不止一帧雷达数据，这些数据按照时间先后顺序进行维护，例如设置该参数为10s时，则ObservationBuffer每次更新时会查询一下自己的数据，将超过这个时间的数据抛弃掉。有时候我们不希望设备对动态的障碍物表现的非常敏感的话，是不是可以将这个值设置个3-5s这样子？expected_update_rate代表的是Buffer的更新频率，这个值默认为0，代表每一帧激光数据都会进入Buffer，如果改大的话应该会过滤掉一部分数据。min_obstacle_height与max_obstacle_height是障碍物的高度过滤参数，对于2D来说意义不大。obstacle_range与raytrace_range则是距离滤波参数，代表多远的障碍物点云需要被过滤掉。在障碍物地图中本身不会维护一帧激光数据中所有的点，只会维护机器人周围一定范围内的点云，因为距离很远的点一般对于路径规划意义不会太大，2-3米远基本就足够了。剩下的四个参数是tf相关的参数，用于将点云从传感器坐标系转换到地图坐标系。

除了上述的初始化外，ObservationBuffer中一个比较重要的东西在于它使用了Observation类。在它的私有变量中，存在这么一个参数：

std::list<Observation> observation_list_;

这行代码可以说是ObservationBuffer中非常重要的内容。它其实相当于维护了一个Observation类列表。而对于每一个Observation类，它又维护了一个：

sensor_msgs::PointCloud2* cloud_;

也就是一组点云。因此，ObservationBuffer就是通过这种方式维护了一整个点云列表。然后我们在上面看到了一个参数observation_keep_time，这个参数在ObservationBuffer中的purgeStaleObservations函数中使用到了，在这个函数中是这么写的：

 // otherwise... we'll have to loop through the observations to see which ones are stale
    for (obs_it = observation_list_.begin(); obs_it != observation_list_.end(); ++obs_it)
    {
      Observation& obs = *obs_it;
      // check if the observation is out of date... and if it is, remove it and those that follow from the list
      if ((last_updated_ - obs.cloud_->header.stamp) > observation_keep_time_)
      {
        observation_list_.erase(obs_it, observation_list_.end());
        return;
      }
    }

这个函数在每次更新Buffer后调用一次。在调用时检查每一个cloud的时间戳是否超过observation_keep_time，如果超过的话就将这些点云去除。所以其也是通过这种方式维护了一组点云。

2 订阅传感器数据

在完成Buffer的初始化后，自然要根据需求获取激光数据。这里考虑了几种不同情况下对于数据的处理：激光数据、点云数据以及其他。而对于激光数据，又根据inf值进行了再次区分：

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<tf2_ros::MessageFilter<sensor_msgs::LaserScan> > filter(
        new tf2_ros::MessageFilter<sensor_msgs::LaserScan>(*sub, *tf_, global_frame_, 50, g_nh));

      if (inf_is_valid)
      {
        filter->registerCallback(boost::bind(&ObstacleLayer::laserScanValidInfCallback, this, _1,
                                            observation_buffers_.back()));
      }
      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));
    }

可见这里作者其实考虑还是非常周道的，将各种情况都考虑进去了。

3 回调函数的处理

看完了初始化函数，接下来看一下回调函数这块。上面根据不同情况使用了几个不同的回调函数，这里我们简单看一下激光的回调函数即可。

这个函数在ObstacleLayer类里面，它本身其实只执行了两个步骤：将点云原始数据转换程pointcloud形式：

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

以及将转换后的点云交给ObservationBuffer处理：

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

真是熟悉的配方，这个函数就直接调转到了ObservationBuffer类去了。其实ObservationBuffer里面一共就3个函数，前面我们已经讲过一个了，就是根据时间戳删除数据的那个，第二个是返回当前所有点云数据的函数，而这个插入点云的函数就是第三个了。我们详细看一下这个函数：它首先是init了一个新的Observation类，用于存放一组新的数据：

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

有了类，下一步自然是要操作这个类，往里面塞东西。首先塞进去的是一组坐标：

// given these observations come from sensors... we'll need to store the origin pt of the sensor
    geometry_msgs::PointStamped local_origin;
    local_origin.header.stamp = cloud.header.stamp;
    local_origin.header.frame_id = origin_frame;
    local_origin.point.x = 0;
    local_origin.point.y = 0;
    local_origin.point.z = 0;
    tf2_buffer_.transform(local_origin, global_origin, global_frame_);
    tf2::convert(global_origin.point, observation_list_.front().origin_);

这个地方存储的是一组坐标，在Observation中作为一个私有变量存在：

 geometry_msgs::Point origin_;

它的含义其实应该在代表了这个点云所在的这个坐标系在当前时间下在世界坐标系下的位姿。但是我有点疑惑的是为什么使用的是Point而不是Pose表示？
然后是存储了前面我们提到的初始化时给入的两个似乎用于距离过滤的参数：

 // 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_;

暂时还不知道这两个参数在哪里被使用到了，先往下看。

下一步是对点云部分的处理，看完这部分让我对上上一部分的存在更加迷惑了，因为这一部分是将点云转换到了世界坐标系下：

sensor_msgs::PointCloud2 global_frame_cloud;

// transform the point cloud
tf2_buffer_.transform(cloud, global_frame_cloud, global_frame_);
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
sensor_msgs::PointCloud2& observation_cloud = *(observation_list_.front().cloud_);
observation_cloud.height = global_frame_cloud.height;
observation_cloud.width = global_frame_cloud.width;
observation_cloud.fields = global_frame_cloud.fields;
observation_cloud.is_bigendian = global_frame_cloud.is_bigendian;
observation_cloud.point_step = global_frame_cloud.point_step;
observation_cloud.row_step = global_frame_cloud.row_step;
observation_cloud.is_dense = global_frame_cloud.is_dense;

如果得到了现在的世界坐标系下的点云，那么前面存储这个origin_意义似乎不大？没看懂没看懂，先看这部分吧，这里就是设置了一些参数，都是PointCloud2的内置参数，简单知道一下就好。

转换完成后，就是筛选了。虽然前面的raytrace_range_与obstacle_range_没有拿出来使用，但是初始化时还有一组参数max_obstacle_height_与min_obstacle_height_则是在这里被使用了。这里是对转换完的点云进行了一次高度上的滤波，剔除掉了一部分不满足的点云，然后将满足条件的点云存储到前面新建的Observation类的cloud_中：

std::vector<unsigned char>::const_iterator iter_global = global_frame_cloud.data.begin(), iter_global_end = global_frame_cloud.data.end();
std::vector<unsigned char>::iterator iter_obs = observation_cloud.data.begin();
for (; iter_global != iter_global_end; ++iter_z, iter_global += global_frame_cloud.point_step)
{
  if ((*iter_z) <= max_obstacle_height_
      && (*iter_z) >= min_obstacle_height_)
  {
    //把一个序列（sequence）拷贝到一个容器（container）中去，通常用std::copy算法
    //std::copy(start, end, std::back_inserter(container));  
    std::copy(iter_global, iter_global + global_frame_cloud.point_step, iter_obs);
    iter_obs += global_frame_cloud.point_step;
    ++point_count;
  }
}

注意到这里的存储方式不是以一个点的形式存储的，因为PointCloud2中对于点云的具体坐标进行了序列化，类似于变成这样：

header: 
  seq: 2116
  stamp: 
    secs: 1586919439
    nsecs: 448866652
  frame_id: "laser"
height: 1
width: 3
fields: 
  - 
    name: "x"
    offset: 0
    datatype: 7
    count: 1
  - 
    name: "y"
    offset: 4
    datatype: 7
    count: 1
  - 
    name: "z"
    offset: 8
    datatype: 7
    count: 1
is_bigendian: False
point_step: 12
row_step: 36
data: [143, 194, 117, 53, 10, 215, 163, 53, 222, 238, 165, 64, 143, 194, 117, 53, 10, 215, 163, 53, 222, 238, 165, 64, 143, 194, 117, 53, 10, 215, 163, 53, 222, 238, 165, 64]
is_dense: True

我们不太能够直接从data中看出这个点的坐标，同时也需要借助std::copy函数对其进行快速操作。

到这里，一帧障碍物点云的处理基本就完成了。可以看到，它其实过程还是很简单的，就是维护了一个Observation类列表，然后每一个Observation类中包含了一帧点云。对于新的点云，首先将其转换到世界坐标系下，然后剔除一些高度不符合的点，将剩下的点存储到Observation类下的cloud_私有变量中。

虽然到这里我们搞清楚了点云的处理过程，但是从点云到地图的过程又是如何的呢？继续看下一部分。