同时定位与制图-SlamGMapping

本文链接地址: 同时定位与制图-SlamGMapping

Content:

1. 节点的启动
2. 初始化Mapper
3. 处理一次扫描
4. 扫描更新
5. 计算当前粒子的最优位置和得分
6. 计算当前粒子的其中一个位置的得分
7. 计算当前粒子的似然和得分
8. 计算当前粒子的活动区域
9. 更新所有粒子的权重
10. 对所有粒子重采样

机器人如小车如果需要在一个陌生的环境中导航，一个首要的前提就是得由一个地图。这个地图就需要小车利用自己的传感器来制作，这儿一般使用里程计和激光雷达，依赖一定的算法就可以完成。这儿以同时定位与制图-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"); }

初始化Mapper

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