laserCallback 第2部分

接initMapper

GMapping::OrientedPoint odom_pose;
// addScan这个函数要转到pf的核心代码了，将调用processScan
if(addScan(*scan, odom_pose))
{
  ROS_DEBUG("scan processed done !");
  // 从粒子集合中挑选最优的粒子， 以其地图坐标为基准， 计算从激光雷达到地图之间的坐标变换
  GMapping::OrientedPoint mpose = gsp_->getParticles()[gsp_->getBestParticleIndex()].pose;
  ROS_INFO("new best pose: %.3f %.3f %.3f", mpose.x, mpose.y, mpose.theta);
  tf::Transform laser_to_map = tf::Transform(tf::createQuaternionFromRPY(0, 0, mpose.theta), tf::Vector3(mpose.x, mpose.y, 0.0)).inverse();

  ROS_INFO("odom pose: %.3f %.3f %.3f", odom_pose.x, odom_pose.y, odom_pose.theta);
  tf::Transform odom_to_laser = tf::Transform(tf::createQuaternionFromRPY(0, 0, odom_pose.theta), tf::Vector3(odom_pose.x, odom_pose.y, 0.0));

  ROS_WARN("correction: %.3f %.3f %.3f", mpose.x - odom_pose.x, mpose.y - odom_pose.y, mpose.theta - odom_pose.theta);

  // 另一处的lock在 SlamGMapping::publishTransform()， 这里是获得map_to_odom_，前者是发布
  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_WARN("cannot process scan");

这里如果addScan返回false，就是未向地图添加scan，原因在processScan里，可能是机器人走过的线距离太短或者旋转过角度太少，未达到门槛；或者未达到指定的时间(period_，写死为5秒)，毕竟我们不希望机器人停着不动就更新地图，那太消耗资源了。

updateMap()得到最优的粒子的地图数据，利用占据栅格地图算法，更新地图。

addScan

把新的测量值和里程计坐标报告给建图引擎gsp_，让其更新粒子集。 根据激光雷达的安装方式，对角度进行修改。

然后将ROS的激光雷达采集的信息转换成gmapping能看懂的格式。设置和激光数据时间戳匹配的机器人的位姿。调用processscan函数

// 加入一个激光雷达的数据, 里面会调用 processScan()函数,这个函数被laserCallback()函数调用
// gmap_pose为刚定义的里程计的累积位姿
bool SlamGMapping::addScan(const sensor_msgs::LaserScan&  scan, GMapping::OrientedPoint&  gmap_pose)
{
    //得到与激光的时间戳相对应的机器人的里程计的位姿
    if(!getOdomPose(gmap_pose, scan.header.stamp))
        return false;

    //检测是否所有帧的数据都是相等的，如果不相等就return
    if(scan.ranges.size() != gsp_laser_beam_count_)
        return false;

    double* ranges_double = new double[scan.ranges.size()];

    // 如果激光是反着装的，这激光的顺序需要反过来，这段代码省略
    if (do_reverse_range_)
    {
        ......
    }
    else
    {
        for(unsigned int i=0; i < scan.ranges.size(); i++)
        {
            // mapper那里无法过滤掉
            // 排除掉所有激光距离小于range_min的值，它们很可能是噪声数据
            if(scan.ranges[i] < scan.range_min)
                ranges_double[i] = (double)scan.range_max;
            else
                ranges_double[i] = (double)scan.ranges[i];
        }
    }
    // 把ROS的激光雷达数据信息 转换为 GMapping算法看得懂的形式
    // 激光传感器gsp_laser_ 在initMapper里定义，ranges数据赋值给m_dists，size给m_beams
    GMapping::RangeReading reading(scan.ranges.size(),
                                   ranges_double,
                                   gsp_laser_,
                                   scan.header.stamp.toSec());
    // 上面的初始化是进行深拷贝，因此申请的内存可以直接释放。
    delete[] ranges_double;

    //设置和激光数据的时间戳匹配的机器人的位姿   不是雷达位姿吗???
    reading.setPose(gmap_pose);

    //调用gmapping算法进行处理
    return gsp_->processScan(reading);
}

gmap_pose应当是不同时间戳时，雷达扫描扇形的中心(包含角度)在odom坐标系中的坐标。

问题：

• reading.setPose(gmap_pose); 解释存疑

最后就是核心算法所在的processScan