gmapping原理及代码解析（一）

此次记录Gmapping学习的过程，笔者能力尚缺，欢迎大家一起交流啊～

一、gmapping代码解析

我们先来看看Gmapping里的main.cpp

#include #include "slam_gmapping.h"int
main(int argc, char** argv)
{ros::init(argc, argv, "slam_gmapping");SlamGMapping gn;gn.startLiveSlam();ros::spin();return(0);
}

里面涉及到startLiveSlam()函数，转到此函数，此函数在slam_gmapping.h中定义，在slam_gmapping.cpp中实现：

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_filter_->registerCallback(boost::bind(&SlamGMapping::laserCallback, this, _1));transform_thread_ = new boost::thread(boost::bind(&SlamGMapping::publishLoop, this, transform_publish_period_));
}

可以看出，此函数注册了相关的回调函数和发布者，我们进入雷达数据的回调函数SlamGMapping::laserCallback，也是在slam_gmapping.cpp中：

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);//第一帧激光数据时不初始化mapper// We can't initialize the mapper until we've got the first scanif(!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");}} elseROS_DEBUG("cannot process scan");
}

在此函数中，突出重要的是addscan()函数,这个函数需要scan（激光数据）和odom_pose（里程计位姿）两个参数：

bool
SlamGMapping::addScan(const sensor_msgs::LaserScan& scan, GMapping::OrientedPoint& gmap_pose)
{if(!getOdomPose(gmap_pose, scan.header.stamp))return false;if(scan.ranges.size() != gsp_laser_beam_count_)return false;// GMapping wants an array of doubles...double* ranges_double = new double[scan.ranges.size()];   //dynamic array// If the angle increment is negative, we have to invert the order of the readings.if (do_reverse_range_){ROS_DEBUG("Inverting scan");int num_ranges = scan.ranges.size();for(int i=0; i < num_ranges; i++){// Must filter out short readings, because the mapper won't  ?过滤掉短读数,i he i-1这样关系是因为雷达reverseif(scan.ranges[num_ranges - i - 1] < scan.range_min)ranges_double[i] = (double)scan.range_max;elseranges_double[i] = (double)scan.ranges[num_ranges - i - 1];}} else {for(unsigned int i=0; i < scan.ranges.size(); i++){// Must filter out short readings, because the mapper won't  //可是为什么要等于最大值呢if(scan.ranges[i] < scan.range_min)ranges_double[i] = (double)scan.range_max;elseranges_double[i] = (double)scan.ranges[i];}}GMapping::RangeReading reading(scan.ranges.size(),ranges_double,gsp_laser_,scan.header.stamp.toSec());//但是它在rangereading构造函数中深度复制它们，因此我们不需要保留数组。// ...but it deep copies them in RangeReading constructor, so we don't// need to keep our array around.delete[] ranges_double;reading.setPose(gmap_pose);   //inline void setPose(const OrientedPoint& pose) {m_pose=pose;}/*ROS_DEBUG("scanpose (%.3f): %.3f %.3f %.3f\n",scan.header.stamp.toSec(),gmap_pose.x,gmap_pose.y,gmap_pose.theta);*/ROS_DEBUG("processing scan");return gsp_->processScan(reading);
}

addscan()函数只是将获取到的scan消息作一下处理，过滤掉无效值，将处理过的数据传入processScan()函数,这个函数如果在ros上安装了gmapping包，可以在ros的安装路径下找到，如笔者的就在/opt/ros/kinetic/include/gmapping/gridfastslam中找到此函数的相关的头文件，或者可以之间去openslam下载源代码。;

 bool GridSlamProcessor::processScan(const RangeReading & reading, int adaptParticles){/**retireve the position from the reading, and compute the odometry*//*得到当前的里程计的位置*/OrientedPoint relPose=reading.getPose();//relPose.y = m_odoPose.y;/*m_count表示这个函数被调用的次数 如果是第0次调用,则所有的位姿都是一样的*/if (!m_count){m_lastPartPose=m_odoPose=relPose;}//write the state of the reading and update all the particles using the motion model/*对于每一个粒子，都从里程计运动模型中采样，得到车子的初步估计位置  这一步对应于   里程计的更新 */int tmp_size = m_particles.size();//这个for循环显然可以用OpenMP进行并行化
//#pragma omp parallel forfor(int i = 0; i < tmp_size;i++){OrientedPoint& pose(m_particles[i].pose);pose = m_motionModel.drawFromMotion(m_particles[i],relPose,m_odoPose);}//invoke the callback/*回调函数  实际上什么都没做*/onOdometryUpdate();// accumulate the robot translation and rotation/*根据两次里程计的数据 计算出来机器人的线性位移和角度位移的累积值 m_odoPose表示上一次的里程计位姿  relPose表示新的里程计的位姿*/OrientedPoint move=relPose-m_odoPose;move.theta=atan2(sin(move.theta), cos(move.theta));//统计机器人在进行激光雷达更新之前 走了多远的距离 以及　平移了多少的角度m_linearDistance+=sqrt(move*move);m_angularDistance+=fabs(move.theta);//    cerr <<"linear Distance:"<m_distanceThresholdCheck){cerr << "***********************************************************************" << endl;cerr << "********** Error: m_distanceThresholdCheck overridden!!!! *************" << endl;cerr << "m_distanceThresholdCheck=" << m_distanceThresholdCheck << endl;cerr << "Old Odometry Pose= " << m_odoPose.x << " " << m_odoPose.y << " " <=m_linearThresholdDistance || m_angularDistance>=m_angularThresholdDistance|| (period_ >= 0.0 && (reading.getTime() - last_update_time_) > period_)){last_update_time_ = reading.getTime();std::cout <(reading.getSensor()),reading.getTime());}//ros的激光数据else{reading_copy = new RangeReading(beam_number,&(reading.m_dists[0]),static_cast(reading.getSensor()),reading.getTime());}/*如果不是第一帧数据*/if (m_count>0){/*为每个粒子进行scanMatch，计算出来每个粒子的最优位姿，同时计算改最优位姿的得分和似然  对应于gmapping论文中的用最近的一次测量计算proposal的算法这里面除了进行scanMatch之外，还对粒子进行了权重的计算，并计算了粒子的有效区域 但不进行内存分配 内存分配在resample()函数中这个函数在gridslamprocessor.hxx里面。*/scanMatch(plainReading);//至此 关于proposal的更新完毕了，接下来是计算权重onScanmatchUpdate();/*由于scanMatch中对粒子的权重进行了更新，那么这个时候各个粒子的轨迹上的累计权重都需要重新计算这个函数即更新各个粒子的轨迹上的累计权重是更新GridSlamProcessor::updateTreeWeights(bool weightsAlreadyNormalized) 函数在gridslamprocessor_tree.cpp里面实现*/updateTreeWeights(false);/** 粒子重采样  根据neff的大小来进行重采样  不但进行了重采样，也对地图进行更新* GridSlamProcessor::resample 函数在gridslamprocessor.hxx里面实现*/std::cerr<<"plainReading:"<map, it->pose, plainReading);m_matcher.registerScan(it->map, it->pose, plainReading);//m_matcher.registerScan(it->lowResolutionMap,it->pose,plainReading);//为每个粒子创建路径的第一个节点。该节点的权重为0,父节点为it->node(这个时候为NULL)。//因为第一个节点就是轨迹的根，所以没有父节点TNode* node=new	TNode(it->pose, 0., it->node,  0);node->reading = reading_copy;it->node=node;}}//		cerr  << "Tree: normalizing, resetting and propagating weights at the end..." ;//进行重采样之后，粒子的权重又会发生变化，因此需要再次更新粒子轨迹的累计权重//GridSlamProcessor::updateTreeWeights(bool weightsAlreadyNormalized) 函数在gridslamprocessor_tree.cpp里面实现updateTreeWeights(false);//		cerr  << ".done!" <previousPose=it->pose;}}m_readingCount++;return processed;}std::ofstream& GridSlamProcessor::outputStream(){return m_outputStream;}std::ostream& GridSlamProcessor::infoStream(){return m_infoStream;}

processScan函数主要有以下几个功能：1.利用运动模型更新里程计分布。2.利用最近的一次观测来提高proposal分布。(scan-match).3.利用proposal分布+激光雷达数据来确定各个粒子的权重.4.对粒子进行重采样。里面包含了几个主要函数，如下：

1.scanmatch():为每一个粒子进行扫描匹配，扫描匹配即为在里程计的基础上，通过优化求得位姿，在gridslamprocessor.hxx中

inline void GridSlamProcessor::scanMatch(const double* plainReading)
{// sample a new pose from each scan in the reference/*每个粒子都要进行scan-match*/double sumScore=0;int particle_number = m_particles.size();//用openMP的方式来进行并行化，因此这里不能用迭代器 只能用下标的方式进行访问//并行话之后会把里面的循环均匀的分到各个不同的核里面去。//#pragma omp parallel forfor (int i = 0; i < particle_number;i++){OrientedPoint corrected;double score, l, s;/*进行scan-match 计算粒子的最优位姿 调用scanmatcher.cpp里面的函数 --这是gmapping本来的做法*/score=m_matcher.optimize(corrected, m_particles[i].map, m_particles[i].pose, plainReading);/*矫正成功则更新位姿*/if (score>m_minimumScore){m_particles[i].pose = corrected;}/*扫描匹配不上 则使用里程计的数据 使用里程计数据不进行更新  因为在进行扫描匹配之前 里程计已经更新过了*/else{//输出信息 这个在并行模式下可以会出现错位if (m_infoStream){m_infoStream << "Scan Matching Failed, using odometry. Likelihood=" << l </*
/ zq commit
@desc	根据地图、激光数据、位姿迭代求解一个最优的新的位姿出来
这个函数是真正被调用来进行scan-match的函数
@param	pnew		新的最优位置
@param  map			地图
@param	init		初始位置
@param  readings	激光数据
*/
double ScanMatcher::optimize(OrientedPoint& pnew, const ScanMatcherMap& map, const OrientedPoint& init,const double* readings) const
{double bestScore=-1;/*计算当前位置的得分*/OrientedPoint currentPose=init;double currentScore=score(map, currentPose, readings);/*所有时的步进增量*/double adelta=m_optAngularDelta, ldelta=m_optLinearDelta;/*精确搜索的次数*/unsigned int refinement=0;/*搜索的方向*/enum Move{Front, Back, Left, Right, TurnLeft, TurnRight, Done};//enum Move{Front, Back, Left, Right, TurnLeft, TurnRight, Done};int c_iterations=0;do{/*如果这一次(currentScore)算出来比上一次(bestScore)差，则有可能是走太多了，要减少搜索步长 这个策略跟LM有点像*/if (bestScore>=currentScore){refinement++;adelta*=.5;ldelta*=.5;}bestScore=currentScore;OrientedPoint bestLocalPose=currentPose;OrientedPoint localPose=currentPose;/*把8个方向都搜索一次  得到这8个方向里面最好的一个位姿和对应的得分*/Move move=Front;do{localPose=currentPose;switch(move){case Front:localPose.x+=ldelta;move=Back;break;case Back:localPose.x-=ldelta;move=Left;break;case Left:localPose.y-=ldelta;move=Right;break;case Right:localPose.y+=ldelta;move=TurnLeft;break;case TurnLeft:localPose.theta+=adelta;move=TurnRight;break;case TurnRight:localPose.theta-=adelta;move=Done;break;default:;}//计算当前的位姿和初始位姿的区别 区别越大增益越小double odo_gain=1;//计算当前位姿的角度和初始角度的区别 如果里程计比较可靠的话//那么进行匹配的时候就需要对离初始位姿比较远的位姿施加惩罚if (m_angularOdometryReliability>0.){double dth=init.theta-localPose.theta; 	dth=atan2(sin(dth), cos(dth)); 	dth*=dth;odo_gain*=exp(-m_angularOdometryReliability*dth);}//计算线性距离的区别 线性距离也是一样if (m_linearOdometryReliability>0.){double dx=init.x-localPose.x;double dy=init.y-localPose.y;double drho=dx*dx+dy*dy;odo_gain*=exp(-m_linearOdometryReliability*drho);}/*计算得分=增益*score*/double localScore=odo_gain*score(map, localPose, readings);/*如果得分更好，则更新*/if (localScore>currentScore){currentScore=localScore;bestLocalPose=localPose;}c_iterations++;} while(move!=Done);/* 把当前位置设置为目前最优的位置  如果8个值都被差了的话，那么这个值不会更新*/currentPose=bestLocalPose;}while (currentScore>bestScore || refinement

inline double ScanMatcher::score(const ScanMatcherMap& map, const OrientedPoint& p, const double* readings) const{double s=0;const double * angle=m_laserAngles+m_initialBeamsSkip;OrientedPoint lp=p;lp.x+=cos(p.theta)*m_laserPose.x-sin(p.theta)*m_laserPose.y;lp.y+=sin(p.theta)*m_laserPose.x+cos(p.theta)*m_laserPose.y;lp.theta+=m_laserPose.theta;unsigned int skip=0;double freeDelta=map.getDelta()*m_freeCellRatio;for (const double* r=readings+m_initialBeamsSkip; rm_likelihoodSkip?0:skip;if (*r>m_usableRange) continue;if (skip) continue;Point phit=lp;phit.x+=*r*cos(lp.theta+*angle);phit.y+=*r*sin(lp.theta+*angle);IntPoint iphit=map.world2map(phit);Point pfree=lp;pfree.x+=(*r-map.getDelta()*freeDelta)*cos(lp.theta+*angle);pfree.y+=(*r-map.getDelta()*freeDelta)*sin(lp.theta+*angle);pfree=pfree-phit;IntPoint ipfree=map.world2map(pfree);bool found=false;Point bestMu(0.,0.);for (int xx=-m_kernelSize; xx<=m_kernelSize; xx++)for (int yy=-m_kernelSize; yy<=m_kernelSize; yy++){IntPoint pr=iphit+IntPoint(xx,yy);IntPoint pf=pr+ipfree;//AccessibilityState s=map.storage().cellState(pr);//if (s&Inside && s&Allocated){const PointAccumulator& cell=map.cell(pr);const PointAccumulator& fcell=map.cell(pf);if (((double)cell )> m_fullnessThreshold && ((double)fcell )

inline unsigned int ScanMatcher::likelihoodAndScore(double& s, double& l, const ScanMatcherMap& map, const OrientedPoint& p, const double* readings) const{using namespace std;l=0;s=0;const double * angle=m_laserAngles+m_initialBeamsSkip;OrientedPoint lp=p;lp.x+=cos(p.theta)*m_laserPose.x-sin(p.theta)*m_laserPose.y;lp.y+=sin(p.theta)*m_laserPose.x+cos(p.theta)*m_laserPose.y;lp.theta+=m_laserPose.theta;double noHit=nullLikelihood/(m_likelihoodSigma);unsigned int skip=0;unsigned int c=0;double freeDelta=map.getDelta()*m_freeCellRatio;for (const double* r=readings+m_initialBeamsSkip; rm_likelihoodSkip?0:skip;if (*r>m_usableRange) continue;if (skip) continue;Point phit=lp;phit.x+=*r*cos(lp.theta+*angle);phit.y+=*r*sin(lp.theta+*angle);IntPoint iphit=map.world2map(phit);Point pfree=lp;pfree.x+=(*r-freeDelta)*cos(lp.theta+*angle);pfree.y+=(*r-freeDelta)*sin(lp.theta+*angle);pfree=pfree-phit;IntPoint ipfree=map.world2map(pfree);bool found=false;Point bestMu(0.,0.);for (int xx=-m_kernelSize; xx<=m_kernelSize; xx++)for (int yy=-m_kernelSize; yy<=m_kernelSize; yy++){IntPoint pr=iphit+IntPoint(xx,yy);IntPoint pf=pr+ipfree;//AccessibilityState s=map.storage().cellState(pr);//if (s&Inside && s&Allocated){const PointAccumulator& cell=map.cell(pr);const PointAccumulator& fcell=map.cell(pf);if (((double)cell )>m_fullnessThreshold && ((double)fcell )

void  GridSlamProcessor::updateTreeWeights(bool weightsAlreadyNormalized)
{if (!weightsAlreadyNormalized) {normalize();}resetTree();propagateWeights();  //传播权重
}

@desc 把粒子的权重归一化
主要功能为归一化粒子的权重，同时计算出neff
*/
inline void GridSlamProcessor::normalize()
{//normalize the log m_weightsdouble gain=1./(m_obsSigmaGain*m_particles.size());/*求所有粒子中的最大的权重*/double lmax= -std::numeric_limits::max();for (ParticleVector::iterator it=m_particles.begin(); it!=m_particles.end(); it++){lmax=it->weight>lmax?it->weight:lmax;}//cout << "!!!!!!!!!!! maxwaight= "<< lmax << endl;/*权重以最大权重为中心的高斯分布*/m_weights.clear();double wcum=0;m_neff=0;for (std::vector::iterator it=m_particles.begin(); it!=m_particles.end(); it++){m_weights.push_back(exp(gain*(it->weight-lmax)));wcum+=m_weights.back();//cout << "l=" << it->weight<< endl;}/*计算有效粒子数 和 归一化权重权重=wi/wneff = 1/w*w*/m_neff=0;for (std::vector::iterator it=m_weights.begin(); it!=m_weights.end(); it++){*it=*it/wcum;double w=*it;m_neff+=w*w;}m_neff=1./m_neff;}

double GridSlamProcessor::propagateWeights()
{// don't calls this function directly, use updateTreeWeights(..) !// all nodes must be resetted to zero and weights normalized// the accumulated weight of the root// 求所有根节点的累计权重之和double lastNodeWeight=0;// sum of the weights in the leafs// 所有叶子节点的权重 也就是m_weights数组里面所有元素的和double aw=0;std::vector::iterator w=m_weights.begin();for (ParticleVector::iterator it=m_particles.begin(); it!=m_particles.end(); it++){//求解所有叶子节点的累计权重double weight=*w;aw+=weight;//叶子节点的子节点累计权重就等于自己的权重 因为它没有子节点//每一个粒子的路径都是从叶子节点开始的，得到了叶子节点，就得到了路径TNode * n=it->node;n->accWeight=weight;lastNodeWeight+=propagateWeight(n->parent,n->accWeight);w++;}if (fabs(aw-1.0) > 0.0001 || fabs(lastNodeWeight-1.0) > 0.0001){cerr << "ERROR: ";cerr << "root->accWeight=" << lastNodeWeight << "    sum_leaf_weights=" << aw << endl;assert(0);         }return lastNodeWeight;
}

/*
@desc 粒子滤波器重采样。分为两步：
1.需要重采样，则所有保留下来的粒子的轨迹都加上一个新的节点，然后进行地图更新。
2.不需要冲采样，则所有的粒子的轨迹都加上一个新的节点，然后进行地图的更新在重采样完毕之后，会调用registerScan函数来更新地图
*/
inline bool GridSlamProcessor::resample(const double* plainReading, int adaptSize, const RangeReading* reading)
{bool hasResampled = false;/*备份老的粒子的轨迹  即保留叶子节点 在增加新节点的时候使用*/TNodeVector oldGeneration;for (unsigned int i=0; i resampler;m_indexes=resampler.resampleIndexes(m_weights, adaptSize);if (m_outputStream.is_open()){m_outputStream << "RESAMPLE "<< m_indexes.size() << " ";for (std::vector::const_iterator it=m_indexes.begin(); it!=m_indexes.end(); it++){m_outputStream << *it <<  " ";}m_outputStream << std::endl;}onResampleUpdate();//BEGIN: BUILDING TREE//重采样之后的粒子ParticleVector temp;unsigned int j=0;//要删除的粒子下标std::vector deletedParticles;  		//this is for deleteing the particles which have been resampled away.//枚举每一个要被保留的粒子for (unsigned int i=0; ireading=reading;//这个要保留下来的粒子，要保留的粒子的下标为m_indexstemp.push_back(p);temp.back().node=node;temp.back().previousIndex=m_indexes[i];}while(jreading = reading;m_particles[i].node = node;//更新各个例子的地图m_matcher.invalidateActiveArea();m_matcher.registerScan(m_particles[i].map, m_particles[i].pose, plainReading);m_particles[i].previousIndex = i;}std::cerr<

void
SlamGMapping::updateMap(const sensor_msgs::LaserScan& scan)
{ROS_DEBUG("Update map");boost::mutex::scoped_lock map_lock (map_mutex_);GMapping::ScanMatcher matcher;matcher.setLaserParameters(scan.ranges.size(), &(laser_angles_[0]),gsp_laser_->getPose());matcher.setlaserMaxRange(maxRange_);matcher.setusableRange(maxUrange_);matcher.setgenerateMap(true);GMapping::GridSlamProcessor::Particle best =gsp_->getParticles()[gsp_->getBestParticleIndex()];std_msgs::Float64 entropy;entropy.data = computePoseEntropy();if(entropy.data > 0.0)entropy_publisher_.publish(entropy);if(!got_map_) {map_.map.info.resolution = delta_;map_.map.info.origin.position.x = 0.0;map_.map.info.origin.position.y = 0.0;map_.map.info.origin.position.z = 0.0;map_.map.info.origin.orientation.x = 0.0;map_.map.info.origin.orientation.y = 0.0;map_.map.info.origin.orientation.z = 0.0;map_.map.info.origin.orientation.w = 1.0;} GMapping::Point center;center.x=(xmin_ + xmax_) / 2.0;center.y=(ymin_ + ymax_) / 2.0;GMapping::ScanMatcherMap smap(center, xmin_, ymin_, xmax_, ymax_, delta_);ROS_DEBUG("Trajectory tree:");for(GMapping::GridSlamProcessor::TNode* n = best.node;n;n = n->parent){ROS_DEBUG("  %.3f %.3f %.3f",n->pose.x,n->pose.y,n->pose.theta);if(!n->reading){ROS_DEBUG("Reading is NULL");continue;}matcher.invalidateActiveArea();matcher.computeActiveArea(smap, n->pose, &((*n->reading)[0]));matcher.registerScan(smap, n->pose, &((*n->reading)[0]));}// the map may have expanded, so resize ros message as wellif(map_.map.info.width != (unsigned int) smap.getMapSizeX() || map_.map.info.height != (unsigned int) smap.getMapSizeY()) {// NOTE: The results of ScanMatcherMap::getSize() are different from the parameters given to the constructor//       so we must obtain the bounding box in a different wayGMapping::Point wmin = smap.map2world(GMapping::IntPoint(0, 0));GMapping::Point wmax = smap.map2world(GMapping::IntPoint(smap.getMapSizeX(), smap.getMapSizeY()));xmin_ = wmin.x; ymin_ = wmin.y;xmax_ = wmax.x; ymax_ = wmax.y;ROS_DEBUG("map size is now %dx%d pixels (%f,%f)-(%f, %f)", smap.getMapSizeX(), smap.getMapSizeY(),xmin_, ymin_, xmax_, ymax_);map_.map.info.width = smap.getMapSizeX();map_.map.info.height = smap.getMapSizeY();map_.map.info.origin.position.x = xmin_;map_.map.info.origin.position.y = ymin_;map_.map.data.resize(map_.map.info.width * map_.map.info.height);ROS_DEBUG("map origin: (%f, %f)", map_.map.info.origin.position.x, map_.map.info.origin.position.y);}for(int x=0; x < smap.getMapSizeX(); x++){for(int y=0; y < smap.getMapSizeY(); y++){/// @todo Sort out the unknown vs. free vs. obstacle thresholdingGMapping::IntPoint p(x, y);double occ=smap.cell(p);assert(occ <= 1.0);if(occ < 0)map_.map.data[MAP_IDX(map_.map.info.width, x, y)] = -1;else if(occ > occ_thresh_){//map_.map.data[MAP_IDX(map_.map.info.width, x, y)] = (int)round(occ*100.0);map_.map.data[MAP_IDX(map_.map.info.width, x, y)] = 100;}elsemap_.map.data[MAP_IDX(map_.map.info.width, x, y)] = 0;}}got_map_ = true;//make sure to set the header information on the mapmap_.map.header.stamp = ros::Time::now();map_.map.header.frame_id = tf_.resolve( map_frame_ );sst_.publish(map_.map);sstm_.publish(map_.map.info);
}bool 
SlamGMapping::mapCallback(nav_msgs::GetMap::Request  &req,nav_msgs::GetMap::Response &res)
{boost::mutex::scoped_lock map_lock (map_mutex_);if(got_map_ && map_.map.info.width && map_.map.info.height){res = map_;return true;}elsereturn false;
}

本文来自互联网用户投稿，文章观点仅代表作者本人，不代表本站立场，不承担相关法律责任。如若转载，请注明出处。 如若内容造成侵权/违法违规/事实不符，请点击【内容举报】进行投诉反馈！