laserCallback消息回调函数
void SlamGMapping::laserCallback(const sensor_msgs::LaserScan::ConstPtr& scan)
{
laser_count_++;
if ((laser_count_ % throttle_scans_) != 0)
return;
static ros::Time last_map_update(0,0);
// We can't initialize the mapper until we've got the first scan
// (1) 判断激光是否为第一帧，当为第一帧时初始化地图，并修改标志位；
if(!got_first_scan_)
{
if(!initMapper(*scan)) //initMapper 进行初始化参数
return;
got_first_scan_ = true;
}
GMapping::OrientedPoint odom_pose;//odom_pose事包含x,y,theta信息的结构体
// (2)调用addScan()函数，添加激光数据, 若返回值为真执行(3)；。
if(addScan(*scan, odom_pose)) //核心算法
{
ROS_DEBUG("scan processed");
// (3)获取(2)中处理后，粒子最佳匹配位置；
GMapping::OrientedPoint mpose = gsp_->getParticles()[gsp_->getBestParticleIndex()].pose;//获取粒子滤波的最佳匹配
ROS_DEBUG("new best pose: %.3f %.3f %.3f", mpose.x, mpose.y, mpose.theta);
ROS_DEBUG("odom pose: %.3f %.3f %.3f", odom_pose.x, odom_pose.y, odom_pose.theta);
ROS_DEBUG("correction: %.3f %.3f %.3f", mpose.x - odom_pose.x, mpose.y - odom_pose.y, mpose.theta - odom_pose.theta);
// (4)将(3)中获取的位姿用于激光到地图、里程计到地图、地图到里程计 tf 变换；
tf::Transform laser_to_map = tf::Transform(tf::createQuaternionFromRPY(0, 0, mpose.theta), tf::Vector3(mpose.x, mpose.y, 0.0)).inverse(); // 激光到地图tf变换
tf::Transform odom_to_laser = tf::Transform(tf::createQuaternionFromRPY(0, 0, odom_pose.theta), tf::Vector3(odom_pose.x, odom_pose.y, 0.0)); // 里程计到地图tf比那混
map_to_odom_mutex_.lock(); //线程锁->上锁
map_to_odom_ = (odom_to_laser * laser_to_map).inverse();
map_to_odom_mutex_.unlock(); // 线程锁->解锁
if(!got_map_ || (scan->header.stamp - last_map_update) > map_update_interval_)//地图更新条件
{
updateMap(*scan);//对地图更新
last_map_update = scan->header.stamp;
ROS_DEBUG("Updated the map");
}
} else
ROS_DEBUG("cannot process scan");
}
该函数传入激光雷达消息。
- 首先判断激光是否是第一帧，当为第一帧时初始化地图并修改标志位
- 调用addScan()返回激光数据，若返回值为真执行(3)
- 获取(2)中处理后的粒子最佳匹配位置
- 将(3)中获取的位姿用于激光到地图、里程计到地图、地图到里程计 tf 变换
- 执行地图更新
然后我们对每一步骤进行分析。
- step1: 首先判断激光是否是第一帧，当为第一帧时初始化地图并修改标志位,这里有一个函数
initMapper()，在我们将整个函数看完后再详细解析。 这里初始化了一个变量odom_pose，包含x,y,theta等信息，用来存储里程计信息。 - step2: 调用函数
addScan()返回激光数据信息，若存在激光数据信息则执行后面的步骤。这里的addScan()函数也要放在后面详细的解析。 - step3 定义变量
mpose用来存储粒子滤波的最佳匹配，获取粒子滤波的最佳匹配通过变量GMapping::GridSlamProcessor* gps_来获取，这个类也将在后面详解。 - step4 获取激光到地图
laser_to_map，里程计到激光odom_to_laser，地图到里程计map_to_odom的坐标变换。laser_to_map通过粒子滤波估计的最佳位姿获取;odom_to_laser通过里程计信息获取;map_to_odom则通过前两者的乘机获取。这里对获取map_to_odom加线程锁原因应该是防止其他线程对laser_to_map和odom_to_laser的更新导致map_to_odom数据的出错？是这样吗，还不太确定。 - step5 然后就是对地图进行更新，地图更新的条件就是，获取到了地图，并且更新频率间隔位为5。然后就是更新地图的函数，这个函数也放在后面详细讲解。这里挖的坑有点多，后面就是慢慢填坑的时间。
