Gmapping code analysis [easy to understand]

Hello everyone , I meet you again , I'm your friend, Quan Jun .

Preface ：

Just recently be used GMapping, The code needs to be changed , But I always forget after reading it , Then just write a blog record Coming down can also help people who want to know GMapping Code students .

The code entry is still main function , but GMapping A lot of code is useless , So it's not There is no need to look one by one , It can be said that the author of the code has strong code ability, but the code style is not flattering . This is it Don't take everyone to file the code one by one , Just for a few main Function .

Before looking at the code , Readers had better choose one IDE Look at code , Because it will involve a lot of jumps , It's too tired to find it by hand .yi

main.cpp

int
main(int argc, char** argv)
{
  ros::init(argc, argv, "slam_gmapping");

  SlamGMapping gn;
  gn.startLiveSlam();
  ros::spin();

  return(0);
}

Copy

Code entry , The main function is to generate a class object , Class object calls member functions startLiveSlam(). Explain ros There is no more basic knowledge here , Readers learn by themselves ros Just go .

startLiveSlam()

/* Subscribe to some topics   Post some topics */
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);

    // Subscribe to laser data   At the same time with odom_frame Conversion synchronization between 
    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;
 

    /* The thread that publishes the transformation relationship */
    transform_thread_ = new boost::thread(boost::bind(&SlamGMapping::publishLoop, this, transform_publish_period_));
}

Copy

The function of this function is to publish four messages , Subscribe to a topic , It will involve boost Some knowledge of the Library , Just search it on the Internet . Next, go to the callback function laserCallback in .

laserCallback（）

void SlamGMapping::laserCallback(const sensor_msgs::LaserScan::ConstPtr& scan)
{

    laser_count_++;
    if ((laser_count_ % throttle_scans_) != 0)//throttle_scans_ Realize frequency reduction , If the laser frequency is high and 
        return;                              // The computing power of the processor is limited , It can reduce the frequency of processing laser data 

    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))// Initialize the first frame of laser data 
            return;
        got_first_scan_ = true;
    }

    GMapping::OrientedPoint odom_pose;
    if(addScan(*scan, odom_pose))// After initialization , Execute when there is laser data addScan function 
    {
        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 there is no map, it must be updated directly , If there is a map, it will take time , Just updated the map */
        if(!got_map_ || (scan->header.stamp - last_map_update) > map_update_interval_)
        {
            /* How often do I update the map */
            updateMap(*scan);
            last_map_update = scan->header.stamp;
            ROS_DEBUG("Updated the map");
        }
    }
    else
        ROS_DEBUG("cannot process scan");
}

Copy

To be continued

Publisher ： Full stack programmer stack length , Reprint please indicate the source ：https://javaforall.cn/148589.html Link to the original text ：https://javaforall.cn