GMapping的ROS封装——更新
我们已经介绍了GMapping的ROS封装的初始化过程。大体上就是构建建图引擎GridSlamProcessor对象，tf广播器。 从ROS的参数服务器上获取关于建图引擎的各种配置参数。
本文中，我们详细介绍ROS封装中GMapping的更新过程。
1. 开始在线建图
在main函数中，通过SlamGMapping对象gn的成员函数startLiveSlam()开始在线建图。 该函数如下所示，它首先通过私有句柄发布了"~entropy"的主题， 这就是ROS节点slam_gmapping节点信息中的"/slam_gmapping/entropy"主题。 接着向全局命名空间中发布"map"和"map_metadata"主题。 在第5行中，该函数注册了服务"dynamic_map"，以及服务回调函数SlamGMapping::mapCallback。
void SlamGMapping::startLiveSlam() {
entropy_publisher_ = private_nh_.advertise("entropy", 1, true);
sst_ = node_.advertise("map", 1, true);
sstm_ = node_.advertise("map_metadata", 1, true);
ss_ = node_.advertiseService("dynamic_map", &SlamGMapping::mapCallback, this);
接下来，在第6到第8行中构建了tf消息过滤器scan_filter_。首先用对象scan_filter_sub_订阅了主题"scan"，以监听激光传感器的扫描值。然后用该订阅器构建消息过滤器， 让它同时监听激光扫描消息和里程计坐标变换。最后注册回调函数SlamGMapping::laserCallback，每当传感器的数据可以转换到目标坐标系上时，就调用该函数完成地图的更新。
scan_filter_sub_ = new message_filters::Subscriber(node_, "scan", 5);
scan_filter_ = new tf::MessageFilter(*scan_filter_sub_, tf_, odom_frame_, 5);
scan_filter_->registerCallback(boost::bind(&SlamGMapping::laserCallback, this, _1));
在startLiveSlam的最后，创建一个线程，以transform_publish_period_为周期发布坐标变换。
transform_thread_ = new boost::thread(boost::bind(&SlamGMapping::publishLoop, this, transform_publish_period_));
}
2. 建图更新过程
我们已经提到，每当有了合适的传感器数据和里程计变换，就会调用函数laserCallback进行地图更新。在函数的一开始，就对激光传感器的数据进行计数。 同时根据之前配置的参数，每当接收到throttle_scans个扫描数据就进行一次更新操作。
void SlamGMapping::laserCallback(const sensor_msgs::LaserScan::ConstPtr& scan) {
laser_count_++;
if ((laser_count_ % throttle_scans_) != 0)
return;
接着在该函数中构建一个静态的ros::Time对象，用于记录最近一次的更新时间。
static ros::Time last_map_update(0,0);
在第一次获得扫描数据时，完成建图器的初始化操作。它根据成员变量got_first_scan_来判定是否获得了第一个扫描数据，一旦获得了扫描数据，就会进行建图器的初始化并将该变量置为true， 之后将不再调用函数initMapper。
if(!got_first_scan_) {
if(!initMapper(*scan))
return;
got_first_scan_ = true;
}
接着通过函数addScan，把新的测量值和里程计坐标报告给建图引擎gsp_。
GMapping::OrientedPoint odom_pose;
if(addScan(*scan, odom_pose)) {
如果能够成功添加数据，将进一步的计算地图到里程计的坐标变换并更新地图。首先从粒子集合中挑选最优的粒子，以其地图坐标为基准计算从激光雷达到地图之间的坐标变换。 同时计算从里程计到激光雷达的坐标变换。
GMapping::OrientedPoint mpose = gsp_->getParticles()[gsp_->getBestParticleIndex()].pose;
tf::Transform laser_to_map = tf::Transform(tf::createQuaternionFromRPY(0, 0, mpose.theta),
tf::Vector3(mpose.x, mpose.y, 0.0)).inverse();
tf::Transform odom_to_laser = tf::Transform(tf::createQuaternionFromRPY(0, 0, odom_pose.theta),
tf::Vector3(odom_pose.x, odom_pose.y, 0.0));
接着根据刚刚计算的变换导出从地图到里程计的坐标变换。在更新的时候对map_to_odom_加锁了，以保证更新过程是原子的。
map_to_odom_mutex_.lock();
map_to_odom_ = (odom_to_laser * laser_to_map).inverse();
map_to_odom_mutex_.unlock();
最后根据参数map_update_interval_的设定，定时更新地图。
if(!got_map_ || (scan->header.stamp - last_map_update) > map_update_interval_) {
updateMap(*scan);
last_map_update = scan->header.stamp;
}
}
}
整个地图更新过程大致上可以分为两个部分，首先把激光传感器数据和里程计数据提供给建图引擎gsp_，让其更新粒子集，这一过程在addScan中实现。 然后根据最优粒子的地图数据更新最终的占用栅格地图，这一过程由updateMap实现。
3. addScan
addScan的工作就是收集距离扫描数据和里程计数据，供建图引擎更新粒子集合。下面是该函数的实现，它有两个参数，scan是传感器的扫描数据，gmap_pose则是里程计的累积位姿。 在该函数中首先对输入参数进行一些检查，防止程序发生意外的状况。
bool SlamGMapping::addScan(const sensor_msgs::LaserScan& scan, GMapping::OrientedPoint& gmap_pose) {
if(!getOdomPose(gmap_pose, scan.header.stamp))
return false;
if(scan.ranges.size() != gsp_laser_beam_count_)
return false;
因为GMapping需要一个double型的数组来接收扫描数据，所以下面我们为之新建一个数组并把输入参数scan中的数据拷贝进去。在初始化过程中已经根据扫描数据判定扫描方向， 并用成员函数do_reverse_range_标记。如果扫描角度是从大到小进行的，就在这里对其翻转，保证提供给引擎的扫描角度始终是递增的。这里剔除了那些很小的距离值，它们很可能是噪声数据。
double* ranges_double = new double[scan.ranges.size()];
if (do_reverse_range_) {
int num_ranges = scan.ranges.size();
for(int i=0; i < num_ranges; i++) {
if(scan.ranges[num_ranges - i - 1] < scan.range_min)
ranges_double[i] = (double)scan.range_max;
else
ranges_double[i] = (double)scan.ranges[num_ranges - i - 1];
}
} else {
for(unsigned int i=0; i < scan.ranges.size(); i++) {
if(scan.ranges[i] < scan.range_min)
ranges_double[i] = (double)scan.range_max;
else
ranges_double[i] = (double)scan.ranges[i];
}
}
然后构建了GMapping中的标准扫描读数对象reading。 因为构建reading的过程中会对数组ranges_double进行深度拷贝，所以我们就不再需要这个数组了，故在此释放了其内存。
GMapping::RangeReading reading(scan.ranges.size(), ranges_double, gsp_laser_, scan.header.stamp.toSec());
delete[] ranges_double;
最后，添加里程计数据到reading中，并调用建图引擎gsp_的函数processScan更新粒子集合。
reading.setPose(gmap_pose);
return gsp_->processScan(reading);
}
4. updateMap
updateMap函数的主要功能就是根据最优粒子的地图数据更新最终栅格地图中各个单元的占用概率。该函数只有一个参数scan，它是激光的扫描数据。 并且在更新地图的时候，加了一把锁。
void SlamGMapping::updateMap(const sensor_msgs::LaserScan& scan) {
boost::mutex::scoped_lock map_lock (map_mutex_);
接着构建了一个扫描匹配器，并配置参数
GMapping::ScanMatcher matcher;
matcher.setLaserParameters(scan.ranges.size(), &(laser_angles_[0]), gsp_laser_->getPose());
matcher.setlaserMaxRange(maxRange_);
matcher.setusableRange(maxUrange_);
matcher.setgenerateMap(true);
然后从建图引擎中获取最优的粒子。并且通过函数computePoseEntropy遍历粒子集合计算熵，通过发布器entroy_publisher_发布。
GMapping::GridSlamProcessor::Particle best = gsp_->getParticles()[gsp_->getBestParticleIndex()];
std_msgs::Float64 entropy;
entropy.data = computePoseEntropy();
if(entropy.data > 0.0)
entropy_publisher_.publish(entropy);
接着开始构建地图，它通过成员变量got_map_来记录是否构建过地图，如果没有则赋予地图信息初值。
if(!got_map_) {
map_.map.info.resolution = delta_;
map_.map.info.origin.position.x = 0.0;
map_.map.info.origin.position.y = 0.0;
map_.map.info.origin.position.z = 0.0;
map_.map.info.origin.orientation.x = 0.0;
map_.map.info.origin.orientation.y = 0.0;
map_.map.info.origin.orientation.z = 0.0;
map_.map.info.origin.orientation.w = 1.0;
}
然后根据地图中心创造一个扫描匹配地图对象smap，并遍历最优粒子的轨迹树，计算激活区域更新地图。
GMapping::ScanMatcherMap smap(center, xmin_, ymin_, xmax_, ymax_, delta_);
for(GMapping::GridSlamProcessor::TNode* n = best.node; n; n = n->parent) {
if(!n->reading)
continue;
matcher.invalidateActiveArea();
matcher.computeActiveArea(smap, n->pose, &((*n->reading)[0]));
matcher.registerScan(smap, n->pose, &((*n->reading)[0]));
}
地图有可能会随着探索区域的增加而扩张，需要更新地图信息。
if(map_.map.info.width != (unsigned int) smap.getMapSizeX() || map_.map.info.height != (unsigned int) smap.getMapSizeY()) {
GMapping::Point wmin = smap.map2world(GMapping::IntPoint(0, 0));
GMapping::Point wmax = smap.map2world(GMapping::IntPoint(smap.getMapSizeX(), smap.getMapSizeY()));
xmin_ = wmin.x; ymin_ = wmin.y;
xmax_ = wmax.x; ymax_ = wmax.y;
map_.map.info.width = smap.getMapSizeX();
map_.map.info.height = smap.getMapSizeY();
map_.map.info.origin.position.x = xmin_;
map_.map.info.origin.position.y = ymin_;
map_.map.data.resize(map_.map.info.width * map_.map.info.height);
}
接下来根据占用概率阈值occ_thresh_把地图划分为占用区域(对应栅格值为100)，空闲区域(对应栅格值为0)，以及未知区域(对应栅格值为-1)。
for(int x=0; x < smap.getMapSizeX(); x++) {
for(int y=0; y < smap.getMapSizeY(); y++) {
GMapping::IntPoint p(x, y);
double occ=smap.cell(p);
assert(occ <= 1.0);
if(occ < 0)
map_.map.data[MAP_IDX(map_.map.info.width, x, y)] = -1;
else if(occ > occ_thresh_)
map_.map.data[MAP_IDX(map_.map.info.width, x, y)] = 100;
else
map_.map.data[MAP_IDX(map_.map.info.width, x, y)] = 0;
}
}
got_map_ = true;
最终标记地图已经构建，给地图消息打上时间戳，发布出去。
got_map_ = true;
map_.map.header.stamp = ros::Time::now();
map_.map.header.frame_id = tf_.resolve( map_frame_ );
sst_.publish(map_.map);
sstm_.publish(map_.map.info);
}
5. 总结
本文中详细拆解了建图更新过程中用到的三个函数。从这三个函数中可以大体了解GMapping的建图过程。在建图过程中用到了如下的一些GMapping类，我们将在后续的文章中详细介绍。
- GMapping::OrientedPoint
- GMapping::GridSlamProcessor
- GMapping::RangeReading
- GMapping::ScanMatcher
- GMapping::ScanMatcherMap
- GMapping::GridSlamProcessor::Particle