这一篇先讲我对gmapping源码的理解(难免有错,欢迎指正,相互学习)。
参考论文:
Improved Techniques for Grid Mapping with Rao-Blackwellized Particle Filters
参考博客:
http://blog.csdn.net/heyijia0327/article/details/40899819 pf原理讲解
http://blog.csdn.net/u010545732/article/details/17462941 pf代码实现
http://www.cnblogs.com/yhlx125/p/5634128.html gmapping分析
http://wenku.baidu.com/view/3a67461550e2524de4187e4d.html?from=search gmapping 分析
其他参考 :
http://ishare.iask.sina.com.cn/f/24615049.html
从ros官网上下载 slam_gmapping 包以及在openslam ( http://openslam.org/ )上下载openslam_gmapping包。
为了方便的阅读源码,这里强力推荐一款源码阅读软件 understand (聪明的你一定找的到资源),可以方便实现各种跳转与生成图、表、树,流程等。
废话不多说了,开始看源码,对于我这种c++都没有过关的菜鸟,看着大几千行的c++的代码,简直是身体和精神上的蹂躏。
先说说 slam_gmapping 包与openslam_gmapping包
进入slam_gmapping 的main.cpp文件的关系,slam_gmapping 是openslam_gampping在ros下的二次封装,你可以直接用这个包,而真正的核心代码实现都在openslam_gampping里面。
进入代码
先用understand 看看代码的调用关系。 (调用太复杂了,图太大,我就截取了一小部分)
转到
gn.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);
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));
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_ (默认值 1)帧数据计算一次,限流作用*/
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_)
{
/*一些重要参数的初始化,将slam里的参数传递到 openslam 里 ,设定坐标系,坐标原点,以及采样函数随机种子的初始化,等等
里面还调用了 GridSlamProcessor::init ,这个初始化了粒子数,子地图大小 */
if(!initMapper(*scan))
return;
got_first_scan_ = true;
}
GMapping::OrientedPoint odom_pose;
/**
pay attention: addScan这个函数*要转到pf的核心代码了 ,将调用processScan
*/
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