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 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");
}在此函数中，突出重要的是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这样关系是因为雷达reverse
if(scan.ranges[num_ranges - i - 1] < scan.range_min)
ranges_double[i] = (double)scan.range_max;
else
ranges_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;
else
ranges_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 for
for(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 for
for (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 || refinementoptimize中的score函数，这个函数在《概率机器人》中的likehood_field_range_finder_model方法有讲：
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 ) 在源码中，类似score函数的还有likelihoodAndScore()函数：
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 ) 2.updateTreeWeights（）权重更新
void GridSlamProcessor::updateTreeWeights(bool weightsAlreadyNormalized)
{
if (!weightsAlreadyNormalized)
{
normalize();
}
resetTree();
propagateWeights(); //传播权重
}其中的normallize（）函数如下：
@desc 把粒子的权重归一化
主要功能为归一化粒子的权重，同时计算出neff
*/
inline void GridSlamProcessor::normalize()
{
//normalize the log m_weights
double 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/w
neff = 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;
} 其中，的progateWeights（）函数如下：
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;
}
3.resample（），重采样函数
/*
@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_indexs
temp.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< 到重采样结束后，processScan结束，接下来我们回到laserScanCallback()函数，在重采样结束后还要更新地图，来看一下这个函数，updateMap（）:
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 well
if(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 way
GMapping::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 thresholding
GMapping::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;
}
else
map_.map.data[MAP_IDX(map_.map.info.width, x, y)] = 0;
}
}
got_map_ = true;
//make sure to set the header information on the map
map_.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;
}
else
return false;
}
今天第一次@20190329，很粗糙，等之后再修改