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