Gmapping Slam主要代码分析

目录

main主函数
startLiveSlam函数
laserCallback激光处理回调函数
initMapper初始化地图函数
addScan激光数据处理函数
processScan函数
drawFromMotion运动模型数据更新函数
scanMatch扫描匹配函数
optimize位姿优化函数
score函数
likelihoodAndScore函数
invalidateActiveArea函数
computeActiveArea计算有效区域函数
updateTreeWeights更新粒子权重函数
normalize归一化函数
resetTree重置粒子树函数
propagateWeights计算权重函数
registerScan地图状态更新函数
resample粒子重采样函数
updateMap地图更新函数

Gmapping主要流程

main函数

#include <ros/ros.h>

#include "slam_gmapping.h"

int
main(int argc, char** argv)
{
   
  ros::init(argc, argv, "slam_gmapping");//**初始化slam节点**

  SlamGMapping gn;
  gn.startLiveSlam();//开始slam进程
  ros::spin();

  return(0);
}

startLiveSlam函数

/*订阅一些主题 发布一些主题*/
void SlamGMapping::startLiveSlam()
{
   
    entropy_publisher_ = private_nh_.advertise<std_msgs::Float64>("entropy", 1, true);
    sst_ = node_.advertise<nav_msgs::OccupancyGrid>("map", 1, true);
    sstm_ = node_.advertise<nav_msgs::MapMetaData>("map_metadata", 1, true);
    ss_ = node_.advertiseService("dynamic_map", &SlamGMapping::mapCallback, this);
    {
   
        //订阅激光数据 同时和odom_frame之间的转换同步
        scan_filter_sub_ = new message_filters::Subscriber<sensor_msgs::LaserScan>(node_, "scan", 5);
        scan_filter_ = new tf::MessageFilter<sensor_msgs::LaserScan>(*scan_filter_sub_, tf_, odom_frame_, 5);
        scan_filter_->registerCallback(boost::bind(&SlamGMapping::laserCallback, this, _1));
        std::cout <<"Subscribe LaserScan & odom!!!"<<std::endl;
    }
    /*发布转换关系的线程*/
    transform_thread_ = new boost::thread(boost::bind(&SlamGMapping::publishLoop, this, transform_publish_period_));
}

laserCallback函数

/*
 * 接受到激光雷达数据的回调函数 在这里面调用addScan()函数
 * 如果addScan()函数调用成功，也就是说激光数据被成功的插入到地图中后，
 * 如果到了地图更新的时间，则对地图进行更新，通过调用updateMap()函数来进行相应的操作。
 *
 * laserCallback()->addScan()->gmapping::processScan()
 *                ->updateMap()
 *
*/
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
    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");
}

initMapper函数

/**
 * @brief SlamGMapping::initMapper  gmapping算法的初始化
 * 这个函数在收到的第一帧激光雷达数据的时候会被调用一次，之后就再也不会被调用了。
 * 这个函数的功能主要就是对gmapping算法中需要的一些参数进行赋值，即对gmapping算法进行初始化
 * 这个参数一开始由gmapping wrapper读取，但是在这个函数里面真正的会赋值给gmapping算法
 * 1.判断激光雷达是否是水平放置的，如果不是 则报错。
 * 2.假设激光雷达数据的角度是对称的 & 递增的 为每个激光束分配角度。
 * 3.为gmapping算法设置各种需要的参数。
 *
 *
 * @param scan
 * @return
 */
bool SlamGMapping::initMapper(const sensor_msgs::LaserScan& scan)
{
   

    //得到激光雷达相对于车身坐标系(base_link)的位姿
    laser_frame_ = scan.header.frame_id;
    // Get the laser's pose, relative to base.
    tf::Stamped<tf::Pose> ident;
    tf::Stamped<tf::Transform> laser_pose;
    ident.setIdentity();
    ident.frame_id_ = laser_frame_;
    ident.stamp_ = scan.header.stamp;
    try
    {
   
        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;
    }

    // create a point 1m above the laser position and transform it into the laser-frame
    // 创造一个比激光雷达高1m的一个点，然后把这个点转换到激光雷达坐标系 这个点主要用来检测激光雷达和车身的位姿关系
    // 来判断激光雷达是否是倾斜的 判断条件：如果激光雷达是水平放置的，那么转换回去之后，z的坐标也依然是1
    tf::Vector3 v;
    v.setValue(0, 0, 1 + laser_pose.getOrigin().z());
    tf::Stamped<tf::Vector3> 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::Pose>(tf::Transform(tf::createQuaternionFromRPY(0,0,angle_center),
                                                                   tf::Vector3(0,0,0)), ros::Time::now(), laser_frame_);
        ROS_INFO("Laser is mounted upwards.");
    }
    else
    {
   
        do_reverse_range_ = scan.angle_min < scan.angle_max;
        centered_laser_pose_ = tf::Stamped<tf::Pose>(tf::Transform(tf::createQuaternionFromRPY(M_PI,0,-angle_center),
                                                                   tf::Vector3(0,0,0)), ros::Time::now(), laser_frame_);
        ROS_INFO("Laser is mounted upside down.");
    }


    // Compute the angles of the laser from -x to x, basically symmetric and in increasing order
    // 这里认为激光雷达数据的角度是对称的，且是从小到大递增的。
    // 因此根据激光雷达数据中的angle_min,angle_max,angle_increment等数据为每个激光束分配角度。
    laser_angles_.resize(scan.ranges.size());
    // Make sure angles are started so that they are centered
    double theta = - std::fabs(scan.angle_min - scan.angle_max)/2;
    for(unsigned int i=0; i<scan.ranges.size(); ++i)
    {
   
        laser_angles_[i]=theta;
        theta += std::fabs(scan.angle_increment);
    }


    ROS_DEBUG("Laser angles in laser-frame: min: %.3f max: %.3f inc: %.3f", scan.angle_min, scan.angle_max,
              scan.angle_increment);
    ROS_DEBUG("Laser angles in top-down centered laser-frame: min: %.3f max: %.3f inc: %.3f", laser_angles_.front(),
              laser_angles_.back(), std::fabs(scan.angle_increment));


    GMapping::OrientedPoint gmap_pose(0, 0, 0);

    // setting maxRange and maxUrange here so we can set a reasonable default
    // 设置激光雷达的最大观测距离和最大使用距离
    ros::NodeHandle private_nh_("~");
    if(!private_nh_.getParam("maxRange", maxRange_))
        maxRange_ = scan.range_max - 0.01;
    if(!private_nh_.getParam("maxUrange", maxUrange_))
        maxUrange_ = maxRange_;

    // The laser must be called "FLASER".
    // We pass in the absolute value of the computed angle increment, on the
    // assumption that GMapping requires a positive angle increment.  If the
    // actual increment is negative, we'll swap the order of ranges before
    // feeding each scan to GMapping.
    // 根据上面得到的激光雷达的数据信息 来初始化一个激光传感器
    gsp_laser_ = new GMapping::RangeSensor("FLASER",
                                           gsp_laser_beam_count_,
                                           fabs(scan.angle_increment),
                                           gmap_pose,
                                           0.0,
                                           maxRange_);
    ROS_ASSERT(gsp_laser_);

    //为gmapping算法设置sensormap
    GMapping::SensorMap smap;
    smap.insert(make_pair(gsp_laser_->getName(), gsp_laser_));
    gsp_->setSensorMap(smap);

    //初始化里程计传感器
    gsp_odom_ = new GMapping::OdometrySensor(odom_frame_);
    ROS_ASSERT(gsp_odom_);


    /// @todo Expose setting an initial pose
    /// 得到里程计的初始位姿，如果没有 则把初始位姿设置为(0,0,0)
    GMapping::OrientedPoint initialPose;
    if(!getOdomPose(initialPose, scan.header.stamp))
    {
   
        ROS_WARN("Unable to determine inital pose of laser! Starting point will be set to zero.");
        initialPose = GMapping::OrientedPoint(0.0, 0.0, 0.0);
    }

    //为gmapping算法设置各种参数
    gsp_->setMatchingParameters(maxUrange_, maxRange_, sigma_,
                                kernelSize_, lstep_, astep_, iterations_,
                                lsigma_, ogain_, lskip_);

    gsp_->setMotionModelParameters(srr_, srt_, str_, stt_);
    gsp_->setUpdateDistances(linearUpdate_, angularUpdate_, resampleThreshold_);
    gsp_->setUpdatePeriod(temporalUpdate_);
    gsp_->setgenerateMap(false);
    gsp_->GridSlamProcessor::init(particles_, xmin_, ymin_, xmax_, ymax_,
                                  delta_, initialPose);

    gsp_->setllsamplerange(llsamplerange_);
    gsp_->setllsamplestep(llsamplestep_);

    /// @todo Check these calls; in the gmapping gui, they use
    /// llsamplestep and llsamplerange intead of lasamplestep and
    /// lasamplerange.  It was probably a typo, but who knows.
    gsp_->setlasamplerange(lasamplerange_);
    gsp_->setlasamplestep(lasamplestep_);
    gsp_->setminimumScore(minimum_score_);

    // Call the sampling function once to set the seed.
    GMapping::sampleGaussian(1,seed_);

    ROS_INFO("Initialization complete");

    return true;
}

addScan激光数据处理函数

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

    //检测是否所有帧的数据都是相等的 如果不相等就进行计算 不知道为什么 感觉完全没必要啊。
    //特别是对于champion_nav_msgs的LaserScan来说 激光束的数量经常变
    if(scan.ranges.size() != gsp_laser_beam_count_)
        return false;</