这里借读的代码是白茶清欢的gmapping手写板仅作阅读学习基于滤波器的SLAM算法-《gmapping算法的删减版》《加入激光雷达运动畸变去除》在原来gmapping源码的基础之上公众号小白学移动机器人的作者对其进行了大刀阔斧的更改。1删除几乎所有不需要的代码对代码的运行结构也进行了调整2对该代码进行详细中文注释以及对核心代码进行更改3将激光雷达运动畸变去除算法直接加入删减版的gmapping算法中算法文件在part_data文件夹目录结构下面是目录结构robotrobot-virtual-machine:~/my_slam_gmapping$ tree . ├── CMakeLists.txt ├── include │ └── my_slam_gmapping ├── launch │ └── my_slam_gmapping.launch ├── package.xml └── src ├── part_data │ └── lidar_undistortion │ ├── lidar_undistortion.cpp │ └── lidar_undistortion.h ├── part_ros │ ├── main.cpp │ ├── my_slam_gmapping.cpp │ └── my_slam_gmapping.h └── part_slam ├── grid │ ├── array2d.h │ ├── harray2d.h │ └── map.h ├── gridfastslam │ ├── gridslamprocessor.cpp │ └── gridslamprocessor.h ├── motionmodel │ ├── motionmodel.cpp │ └── motionmodel.h ├── particlefilter │ └── particlefilter.h ├── scanmatcher │ ├── gridlinetraversal.h │ ├── scanmatcher.cpp │ └── scanmatcher.h ├── sensor_range │ ├── rangereading.cpp │ └── rangereading.h └── utils ├── macro_params.h └── point.hpart_data里面的lidar_undistortion是实现雷达运动去畸变 part_slam的motionmodel是通过odom的数据推算出机器人下一个时刻的大致位置 scanmatcher是之前提到通过odom得到P1的大致位置之后通过与地地图的匹配得到P1的最优位置scanmatcher是做最优匹配的 sensor_range里的rangereading是由于每一个激光雷达激光束是角度相同的但是是有畸变的RangeReading结构体来存储每一个激光束对应的角度和距离函数讲解初始化参数main函数先从main函数讲起位于src\part_ros\main.cppint main(int argc, char ** argv) { ros::init(argc, argv, my_slam_gmapping); MySlamGMapping slamer; slamer.startLiveSlam(); ros::spin(); return 0; }初始化了一个MySlamGMapping的对象slamer.startLiveSlam()启动slam接下来看一下MySlamGMapping类MySlamGMapping类构造函数//构造函数-初始化相关变量比如指针的初始化 MySlamGMapping::MySlamGMapping(): map_to_odom_(tf::Transform(tf::createQuaternionFromRPY( 0, 0, 0 ), tf::Point(0, 0, 0 ))),//默认两个坐标系重合 private_nh_(~), scan_filter_sub_(NULL), scan_filter_(NULL), transform_thread_(NULL) { seed_ time(NULL); init(); }初始化的复制先看一下map_to_odommap_to_odom是tf transform的一个变换描述的是map到odom的变化这里初始化全为0scan_filter_sub_(NULL), scan_filter_(NULL),是我们要对输入的scan数据和odom数据进行一个同步处理message_filters::Subscribersensor_msgs::LaserScan* scan_filter_sub_; tf::MessageFiltersensor_msgs::LaserScan* scan_filter_;scan_filter_sub_是scan数据的一个订阅器但是这个订阅器加了一个message_filters,加了一个数据过滤这个和普通的订阅器相比较就是加了一个过滤的功能scan_filter_是核心过滤器后面会讲transform_thread_boost::thread* transform_thread_; //发布转换关系的线程他是一个线程变量实时发布map到odom的变换的线程在MySlamGMapping::startLiveSlam()里/*发布map到odom的转换关系的线程*/ transform_thread_ new boost::thread(boost::bind(MySlamGMapping::publishLoop, this, transform_publish_period_));这里transform_thread_是由做发布变换的工作的实例化了一个线程实时发布回到构造函数看到有一个seed是高斯噪声的随机种子最后进入初始化init函数init初始化函数//slamgmapping的初始化主要用来读取配置文件中写入的参数以及初始化一些对象 void MySlamGMapping::init() { if(!private_nh_.getParam(map_frame, map_frame_)) map_frame_ map; if(!private_nh_.getParam(odom_frame, odom_frame_)) odom_frame_ odom; if(!private_nh_.getParam(scan_topic, scan_topic_)) scan_topic_ scan; if(!private_nh_.getParam(laser_frame, laser_frame_)) laser_frame_ laser_link; //new一个激光雷达运动畸变的对象 lmc_ new LidarMotionCalibrator(laser_frame_,odom_frame_); //new一个GridSlamProcessor对象也是ros和gridslam的连接 gsp_ new GMapping::GridSlamProcessor(); //这里需要跳进去看第一次不要看 //new一个TransformBroadcaster对象用来发布map和odom的关系 tfB_ new tf::TransformBroadcaster(); got_first_scan_ false; got_map_ false; private_nh_.param(transform_publish_period, transform_publish_period_, 0.05); double tmp; if(!private_nh_.getParam(map_update_interval, tmp))//地图更新的秒数间隔 tmp 5.0; map_update_interval_.fromSec(tmp); //GMapping算法本身使用的参数 maxUrange_ 0.0; maxRange_ 0.0; if(!private_nh_.getParam(particles, particles_)) particles_ 30; if(!private_nh_.getParam(xmin, xmin_)) xmin_ -100.0; if(!private_nh_.getParam(ymin, ymin_)) ymin_ -100.0; if(!private_nh_.getParam(xmax, xmax_)) xmax_ 100.0; if(!private_nh_.getParam(ymax, ymax_)) ymax_ 100.0; if(!private_nh_.getParam(delta, delta_)) delta_ 0.05; if(!private_nh_.getParam(occ_thresh, occ_thresh_)) occ_thresh_ 0.25; if(!private_nh_.getParam(minimumScore, minimum_score_)) minimum_score_ 0; if(!private_nh_.getParam(sigma, sigma_)) sigma_ 0.05; if(!private_nh_.getParam(kernelSize, kernelSize_)) kernelSize_ 1; if(!private_nh_.getParam(lstep, lstep_))//默认一个栅格距离大小变化 lstep_ delta_; if(!private_nh_.getParam(astep, astep_)) astep_ delta_; if(!private_nh_.getParam(iterations, iterations_)) iterations_ 5; if(!private_nh_.getParam(lsigma, lsigma_)) lsigma_ 0.075; if(!private_nh_.getParam(ogain, ogain_)) ogain_ 3.0; if(!private_nh_.getParam(lskip, lskip_))//计算scan与地图匹配得分时跳过的部分默认为0 lskip_ 0; if(!private_nh_.getParam(srr, srr_)) srr_ 0.1; if(!private_nh_.getParam(srt, srt_)) srt_ 0.2; if(!private_nh_.getParam(str, str_)) str_ 0.1; if(!private_nh_.getParam(stt, stt_)) stt_ 0.2; if(!private_nh_.getParam(linearUpdate, linearUpdate_)) linearUpdate_ 1.0; if(!private_nh_.getParam(angularUpdate, angularUpdate_)) angularUpdate_ 0.5; if(!private_nh_.getParam(temporalUpdate, temporalUpdate_)) temporalUpdate_ -1.0; if(!private_nh_.getParam(resampleThreshold, resampleThreshold_)) resampleThreshold_ 0.5; if(!private_nh_.getParam(tf_delay, tf_delay_)) tf_delay_ transform_publish_period_; ROS_DEBUG(MySlamGMapping::init finish); }前面是做参数初始化map、odom、laser坐标系的初始化以及scan话题初始化//new一个激光雷达运动畸变的对象 lmc_ new LidarMotionCalibrator(laser_frame_,odom_frame_);初始化对象去畸变对象、tfB_是用来发布map到odom之间的tf变换与上面map_to_odom的区别是什么呢?tf::Transform map_to_odom_存坐标变换数据的 “容器” tf::TransformBroadcaster *tfB_广播器负责把变换发给整个 ROS 系统 作用只有一个 把你存在 tf::Transform 里的坐标变换打包成 ROS 消息发布到 /tf 话题上。 其他节点雷达、导航、可视化 Rviz订阅 /tf 就能读取坐标系关系。两者核心区别一句话分清 map_to_odom_ 数据盒子 只存平移、旋转数值纯内存变量其他节点看不到。 tfB_ 快递员 把盒子里的数据打包广播让全系统所有节点读取坐标系变换。got_first_scan_表示是否得到了第一帧我们要决定是否来进行地图初始化地图需要第一帧来进行初始化got_map_表示是否拿到了第一帧初始化好的地图如果没有拿到第一帧以及第一帧初始化好的地图的话后续的帧都不会进行处理而是反复的等待地图private_nh_.param(transform_publish_period, transform_publish_period_, 0.05);transform_publish_period_表示的是时间差多久发布一次map到odom的tf变换double tmp; if(!private_nh_.getParam(map_update_interval, tmp))//地图更新的秒数间隔 tmp 5.0; map_update_interval_.fromSec(tmp);这里的map_update_interval表示的是地图更新的时间间隔单位是秒后面就是地图相关参数从launch的参数读取地图参数particles初始化粒子每一次得到一个大致位置odom发出的的时候我们就要在这个大致位置附近初始化很多有可能的位姿这里每一个位姿称之为一个粒子。每一次初始化多少个粒子就代表的这个意思。地图尺寸xmin ymin xmax ymax后面会进行扩展在launch里面所以设置的是5m到-5m之间地图分辨率delta最终形成的栅格地图的分辨率每一个像素就是一个栅格每一个栅格表示物理世界的表示多少米occ_thresh占用概率超过这个概率才是被占用下面是scan_match相关参数scan_match参数minimumScore匹配阈值sigma粒子得分每一个粒子就是每一个位置表示每一个位置和地图的匹配程度得分kernelSize搜索框在找到最优粒子位置的时候会向左右前后四个方向进行位置偏移会不会得到最优位置这个是偏移程度linearUpdate angularUpdate更新的情况当前激光帧和现有地图距离比较远的时候才进行地图更新初始化讲完了下面讲解SLAM相关的函数开始实时SLAMstartLiveSlam函数()void MySlamGMapping::startLiveSlam() { sst_ node_.advertisenav_msgs::OccupancyGrid(map, 1, true); sstm_ node_.advertisenav_msgs::MapMetaData(map_metadata, 1, true); ss_ node_.advertiseService(dynamic_map, MySlamGMapping::mapCallback, this); { //用message_filters来订阅scan_topic_进而初始化scan_filter_ scan_filter_sub_ new message_filters::Subscribersensor_msgs::LaserScan(node_, scan_topic_, 5); //tf::MessageFilter订阅激光数据同时和odom_frame之间转换时间同步 scan_filter_ new tf::MessageFiltersensor_msgs::LaserScan(*scan_filter_sub_, tf_, odom_frame_, 5); //scan_filter_注册回调函数laserCallback scan_filter_-registerCallback(boost::bind(MySlamGMapping::laserCallback, this, _1)); ROS_DEBUG(Start Subscribe LaserScan odom!!!); } /*发布map到odom的转换关系的线程*/ transform_thread_ new boost::thread(boost::bind(MySlamGMapping::publishLoop, this, transform_publish_period_)); ROS_DEBUG(Start transform_thread ); }发布地图map_pub_ nh_.advertisenav_msgs::OccupancyGrid(map, 1, true); map_info_pub_ nh_.advertisenav_msgs::MapMetaData(map_metadata, 1, true);gmapping最终输出的建图结果就是一张栅格地图图片一个对应的yaml配置文件如下:可以理解第一个发布的是pgm格式的第二个发布的是yaml格式类型下面是代码段Scan和Odom同步收到一帧LaserScan后filter 会拿着这帧激光的时间戳去 TF 里查询在该时刻激光坐标系 → odom 坐标系 的 TF 变换是否已经可用TF 存在、查询成功 → 放行消息触发回调TF 还没收到 / 延迟、查不到变换 →缓存消息等待一段时间超时直接丢弃不进回调{ //用message_filters来订阅scan_topic_进而初始化scan_filter_ scan_filter_sub_ new message_filters::Subscribersensor_msgs::LaserScan(node_, scan_topic_, 5); //tf::MessageFilter订阅激光数据同时和odom_frame之间转换时间同步 scan_filter_ new tf::MessageFiltersensor_msgs::LaserScan(*scan_filter_sub_, tf_, odom_frame_, 5); //scan_filter_注册回调函数laserCallback scan_filter_-registerCallback(boost::bind(MySlamGMapping::laserCallback, this, _1)); ROS_DEBUG(Start Subscribe LaserScan odom!!!); }message_filter的使用message_filters tf::MessageFilter做时间 TF 坐标同步经典用法首先是实例化了一个订阅订阅scan话题跟不同的订阅不同的是增加了一个消息过滤message_filter消息过滤器message_filters类似一个消息缓存当消息到达消息过滤器的时候可能并不会立即输出而是在稍后的时间点里满足一定条件下输出。所以这里第一个是订阅器的实例化绑定了雷达scan话题第二个是过滤器的实例化同步处理对订阅的scgn话题按照指定tf对消息进行过滤 (话题订阅器监听坐标变换消息将会被转换到的目的帧queue_size)同步上了就进入第三行的回调函数实时发布map-odomnew boost::thread对线程实例化/*发布map到odom的转换关系的线程*/ transform_thread_ new boost::thread(boost::bind(MySlamGMapping::publishLoop, this, transform_publish_period_));void MySlamGMapping::publishLoop(double transform_publish_period)//发布map-odom的转换关系 void MySlamGMapping::publishLoop(double transform_publish_period) { //发布时间间隔不正确直接返回 if(transform_publish_period 0) return; ros::Rate r(1.0 / transform_publish_period); while(ros::ok()) { publishTransform(); //发布 r.sleep(); //延时r ms } }while循环持续发布MySlamGMapping::publishLoop(double transform_publish_period)//发布map到odom的转换关系 void MySlamGMapping::publishTransform() { // 加锁因为会对 map_to_odom_ 内容进行更新 map_to_odom_mutex_.lock(); //默认情况下 tf_delay_ transform_publish_period_; //默认情况下ros::Duration(tf_delay_)时间长度等于 r.sleep();的时间长度 ros::Time tf_expiration ros::Time::now() ros::Duration(tf_delay_);//这个没搞明白为啥要加这一点时间感觉没有必要 // tf::StampedTransform是ROS中用于表示带有时间戳的坐标变换的类。 // 坐标的具体变换这个变换的时间戳表示这个变换发生的时间父坐标系名称子坐标系名称 tfB_-sendTransform( tf::StampedTransform (map_to_odom_, tf_expiration, map_frame_, odom_frame_)); map_to_odom_mutex_.unlock(); }持续发布map_to_odom_发布前需要上锁操作接下来就看回调函数回调函数--Scan和Odom的同步MySlamGMapping::laserCallback(const sensor_msgs::LaserScan::ConstPtr scan)首先拿到激光雷达数据需要进行一个运动去畸变处理//每当到达一帧scan数据就将调用laserCallback函数 void MySlamGMapping::laserCallback(const sensor_msgs::LaserScan::ConstPtr scan) { //激光雷达数据运动畸变处理部分 ros::Time startTime, endTime; //一帧scan的时间戳就代表一帧数据的开始时间 startTime scan-header.stamp;// 一帧scan的时间戳就代表一帧数据的开始时间(第一个激光束的时间) sensor_msgs::LaserScan laserScanMsg *scan;// 拷贝scan数据 int beamNum laserScanMsg.ranges.size();// 激光束数量 // endTime startTime 每束之间的时间差*激光束数量 //根据激光时间分割和激光束个数的乘积startTime得到endTime最后一束激光束的时间 endTime startTime ros::Duration(laserScanMsg.time_increment * beamNum); laser_ranges_.clear(); laser_angles_.clear(); //拷贝scan数据到laser_ranges_,laser_angles_ double lidar_dist,lidar_angle; for(int i 0; i beamNum;i) { lidar_dist laserScanMsg.ranges[i];//单位米 lidar_angle laserScanMsg.angle_min laserScanMsg.angle_increment * i;//单位弧度 laser_ranges_.push_back(lidar_dist); laser_angles_.push_back(lidar_angle); } //激光雷达运动畸变去除 lmc_-lidarCalibration(laser_ranges_,laser_angles_,startTime,endTime,tf_); ------------------ }首先定义两个变量startTime endTime表示激光束的开始和结束时间一帧scan的时间戳就是开始时间可以直接获取laserScanMsg 是拷贝的激光数据beamNum 是一帧scan里的激光束数量这也可以从scan里面拿到endTime是计算出的结束时间每一个激光束是有时间差的拿时间差*beamNumstd::vectordouble laser_ranges_; //存储每一个激光点距离 std::vectordouble laser_angles_; //存储每一个激光点的角度把scan帧数据放入两个容器存放的是每一个激光束信息一个是距离一个是角度雷达运动去畸变lidarCalibration//激光雷达运动畸变去除 lmc_-lidarCalibration(laser_ranges_,laser_angles_,startTime,endTime,tf_);在头文件定义了lmcLidarMotionCalibrator* lmc_; //激光雷达运动畸变去除对象在初始化的时候实例化了lmc_这里看一下类LidarMotionCalibrator//构造函数 LidarMotionCalibrator::LidarMotionCalibrator(std::string scan_frame_name,std::string odom_name) { scan_frame_name_ scan_frame_name; odom_name_ odom_name; }实现去畸变//激光雷达运动畸变去除函数 void LidarMotionCalibrator::lidarCalibration(std::vectordouble ranges,std::vectordouble angles,ros::Time startTime,ros::Time endTime,tf::TransformListener * tf_) { //激光束的数量 int beamNumber ranges.size(); //分段时间间隔单位us int interpolation_time_duration 5 * 1000;//单位us tf::Stampedtf::Pose frame_base_pose; //基准坐标系原点位姿 tf::Stampedtf::Pose frame_start_pose; tf::Stampedtf::Pose frame_mid_pose; double start_time startTime.toSec() * 1000 * 1000; //*1000*1000转化时间单位为us double end_time endTime.toSec() * 1000 * 1000; double time_inc (end_time - start_time) / beamNumber; //每相邻两束激光数据的时间间隔单位us //得到start_time时刻laser_link在里程计坐标下的位姿存放到frame_start_pose if(!getLaserPose(frame_start_pose, ros::Time(start_time /1000000.0), tf_)) { ROS_WARN(Not Start Pose,Can not Calib); return ; } //分段个数计数 int cnt 0; //当前插值的段的起始坐标 int start_index 0; //默认基准坐标系就是第一个位姿的坐标系 frame_base_pose frame_start_pose; for(int i 0; i beamNumber; i) { //按照分割时间分段分割时间大小为interpolation_time_duration double mid_time start_time time_inc * (i - start_index); //这里的mid_time、start_time多次重复利用 if(mid_time - start_time interpolation_time_duration || (i beamNumber - 1)) { cnt; //得到临时结束点的laser_link在里程计坐标系下的位姿存放到frame_mid_pose if(!getLaserPose(frame_mid_pose, ros::Time(mid_time/1000000.0), tf_)) { ROS_ERROR(Mid %d Pose Error,cnt); return ; } //计算该分段需要插值的个数 int interp_count i 1 - start_index ; //对本分段的激光点进行运动畸变的去除 lidarMotionCalibration(frame_base_pose, //对于一帧激光雷达数据传入参数基准坐标系是不变的 frame_start_pose, //每一次的传入都代表新分段的开始位姿第一个分段根据时间戳在tf树上获得其他分段都为上一段的结束点传递 frame_mid_pose, //每一次的传入都代表新分段的结束位姿根据时间戳在tf树上获得 ranges, //引用对象需要被修改的距离数组 angles, //引用对象需要被修改的角度数组 start_index, //每一次的传入都代表新分段的开始序号 interp_count); //每一次的传入都代表该新分段需要线性插值的个数 //更新时间 start_time mid_time; start_index i; frame_start_pose frame_mid_pose; //将上一分段的结束位姿传递为下一分段的开始位姿 } } }