同时定位与制图-SlamGMapping

class SlamGMappingNodelet : public nodelet::Nodelet
{
  public:
    SlamGMappingNodelet()  {}
 
    ~SlamGMappingNodelet() {}
 
    virtual void onInit()
    {
      NODELET_INFO_STREAM("Initialising Slam GMapping nodelet...");
      sg_.reset(new SlamGMapping(getNodeHandle(), getPrivateNodeHandle()));
      NODELET_INFO_STREAM("Starting live SLAM...");
//节点启动
      sg_->startLiveSlam();
    }
 
  private:  
    boost::shared_ptr sg_;
};
 
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);
  scan_filter_sub_ = new message_filters::Subscriber(node_, "scan", 5);
  scan_filter_ = new tf::MessageFilter(*scan_filter_sub_, tf_, odom_frame_, 5);
//注册一个回调函数，每收到“scan”消息就调用“laserCallback”函数处理
  scan_filter_->registerCallback(boost::bind(&SlamGMapping::laserCallback, this, _1));
 
  transform_thread_ = new boost::thread(boost::bind(&SlamGMapping::publishLoop, this, transform_publish_period_));
}
 
void
SlamGMapping::laserCallback(const sensor_msgs::LaserScan::ConstPtr& scan)
{
  laser_count_++;
//限流，每throttle_scans_次扫描才处理一次
  if ((laser_count_ % throttle_scans_) != 0)
    return;
 
  static ros::Time last_map_update(0,0);
 
//第一次扫描需要初始化Mapper
  // We can't initialize the mapper until we've got the first scan
  if(!got_first_scan_)
  {
    if(!initMapper(*scan))
      return;
    got_first_scan_ = true;
  }
 
  GMapping::OrientedPoint odom_pose;
//传入激光扫描数据和里程计位置，添加一次扫描
  if(addScan(*scan, odom_pose))
  {
    ROS_DEBUG("scan processed");
//取最优的粒子位置作为雷达位置
    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);
 
    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_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");
}

bool
SlamGMapping::initMapper(const sensor_msgs::LaserScan& scan)
{
  laser_frame_ = scan.header.frame_id;
  // Get the laser's pose, relative to base.
  tf::Stamped ident;
  tf::Stamped laser_pose;
  ident.setIdentity();
  ident.frame_id_ = laser_frame_;
  ident.stamp_ = scan.header.stamp;
  try
  {
//获取激光雷达相对小车的真实位置，这才是所有雷达信号的0点
    tf_.transformPose(base_frame_, ident, laser_pose);
  }
  catch(tf::TransformException e)
  {
    ROS_WARN("Failed to compute laser pose, aborting initialization (%s)",
             e.what());
    return false;
  }
 
//获取雷达上方1米的坐标，用于判断雷达是否安装在小车上
  // create a point 1m above the laser position and transform it into the laser-frame
  tf::Vector3 v;
  v.setValue(0, 0, 1 + laser_pose.getOrigin().z());
  tf::Stamped up(v, scan.header.stamp,
                                      base_frame_);
  try
  {
    tf_.transformPoint(laser_frame_, up, up);
    ROS_DEBUG("Z-Axis in sensor frame: %.3f", up.z());
  }
  catch(tf::TransformException& e)
  {
    ROS_WARN("Unable to determine orientation of laser: %s",
             e.what());
    return false;
  }
 
  // gmapping doesnt take roll or pitch into account. So check for correct sensor alignment.
  if (fabs(fabs(up.z()) - 1) > 0.001)
  {
    ROS_WARN("Laser has to be mounted planar! Z-coordinate has to be 1 or -1, but gave: %.5f",
                 up.z());
    return false;
  }
//扫描激光束数量
  gsp_laser_beam_count_ = scan.ranges.size();
//扫描激光束的中间角度
  double angle_center = (scan.angle_min + scan.angle_max)/2;
 
  if (up.z() > 0)
  {
//雷达正着装的时候，如果小角大于大角，说明扫描顺序是反的。
    do_reverse_range_ = scan.angle_min > scan.angle_max;
    centered_laser_pose_ = tf::Stamped(tf::Transform(tf::createQuaternionFromRPY(0,0,a