main函数
ros::init(argc, argv, "slam_gmapping");
SlamGMapping gn;
gn.startLiveSlam();
ros::spin();
return(0);
starLiveSlam函数
// 初始化并启动实时SLAM过程
void SlamGMapping::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);
// 使用TF过滤扫描数据，确保数据以正确的参考系发布
scan_filter_ = new tf::MessageFilter<sensor_msgs::LaserScan>(*scan_filter_sub_, tf_, odom_frame_, 5);
// 注册回调函数，激光雷达数据到来时调用
scan_filter_->registerCallback(boost::bind(&SlamGMapping::laserCallback, this, _1));
// 启动一个线程，定期发布变换信息 发布map->odom的tf信息
transform_thread_ = new boost::thread(boost::bind(&SlamGMapping::publishLoop, this, transform_publish_period_));
}
SlamGMapping::laserCallback函数
地图初始化initMapper(*scan)
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
if(!got_first_scan_)
{
// 如果初始化失败，则直接返回
if(!initMapper(*scan))
return;
// 标记已经接收到第一个激光雷达数据
got_first_scan_ = true;
}
// 定义一个全局位姿变量，用于存储里程计数据
GMapping::OrientedPoint odom_pose;
// 添加激光雷达数据到mapper中，并返回全局位姿
if(addScan(*scan, odom_pose))
{
// 输出调试信息，表示激光雷达数据处理成功
// 获取当前最优粒子的位姿
GMapping::OrientedPoint mpose = gsp_->getParticles()[gsp_->getBestParticleIndex()].pose;
// 输出新的最优位姿和里程计位姿，以及两者的差值
// 计算激光雷达坐标系到地图坐标系的转换
tf::Transform laser_to_map = tf::Transform(tf::_to_laser = tf::Transform(tfcreateQuaternionFromRPY(0,::createQuaternionFromRPY(0 0, mpose.theta), tf, 0, odom_pose.theta),::Vector3(mpose.x, m tf::Vector3(odom_pose.xpose.y, 0.0))., odom_pose.y, 0.inverse()));
// 计算里程0));
// 锁定互计坐标系到激光雷达坐标系斥锁，更新地图到里程计的转换
// tf::Transform odom的转换，然后解锁
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;
// 输出调试信息，表示地图已经更新
}
} else
// 输出调试信息，表示无法处理当前的激光雷达数据
}
SlamGMapping::initMapper函数
bool
SlamGMapping::initMapper(const sensor_msgs::LaserScan& scan)
{
// 设置激光帧ID
laser_frame_ = scan.header.frame_id;
// 获取激光相对于基座的姿态
tf::Stamped<tf::Pose> ident; //用于表示一个带有时间戳的姿态（pose）
tf::Stamped<tf::Transform> laser_pose;
ident.setIdentity();
ident.frame_id_ = laser_frame_;
ident.stamp_ = scan.header.stamp;
try
{
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米，并将其转换到激光坐标系
tf::Vector3 v;
v.setValue(0, 0, 1 + laser_pose.getOrigin().z());
tf::Stamped<tf::Vector3> 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不考虑翻滚和俯仰。因此，检查传感器是否正确对齐。
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::Pose>(tf::Transform(tf::createQuaternionFromRPY(0,0,angle_center),
tf::Vector3(0,0,0)), ros::Time::now(), laser_frame_);
ROS_INFO("Laser is mounted upwards.");
}
else
{
do_reverse_range_ = scan.angle_min < scan.angle_max;
centered_laser_pose_ = tf::Stamped<tf::Pose>(tf::Transform(tf::createQuaternionFromRPY(M_PI,0,-angle_center),
tf::Vector3(0,0,0)), ros::Time::now(), laser_frame_);
ROS_INFO("Laser is mounted upside down.");
}
// 计算激光的角度，从-x到x，基本上是对称的并且按递增顺序排列
// 设置数据大小
laser_angles_.resize(scan.ranges.size());
// 确保角度从中心开始 -x到x ：-90 到 90
double theta = - std::fabs(scan.angle_min - scan.angle_max)/2;
for(unsigned int i=0; i<scan.ranges.size(); ++i)
{
laser_angles_[i]=theta;
// 以激光雷达角度增量为步长递增角度值，取绝对值以确保角度增量总是正的。
theta += std::fabs(scan.angle_increment);
}
// laser_angles_[0]-laser_angles_[2*theta]:90-180
ROS_DEBUG("Laser angles in laser-frame: min: %.3f max: %.3f inc: %.3f", scan.angle_min, scan.angle_max,
scan.angle_increment);
ROS_DEBUG("Laser angles in top-down centered laser-frame: min: %.3f max: %.3f inc: %.3f", laser_angles_.front(),
laser_angles_.back(), std::fabs(scan.angle_increment));
GMapping::OrientedPoint gmap_pose(0, 0, 0);
// 设置maxRange和maxUrange，如果没有设置则使用默认值
ros::NodeHandle private_nh_("~");
if(!private_nh_.getParam("maxRange", maxRange_))
maxRange_ = scan.range_max - 0.01;
if(!private_nh_.getParam("maxUrange", maxUrange_))
maxUrange_ = maxRange_;
// 激光必须命名为"FLASER"
gsp_laser_ = new GMapping::RangeSensor("FLASER",
gsp_laser_beam_count_,
fabs(scan.angle_increment),
gmap_pose,
0.0,
maxRange_);
ROS_ASSERT(gsp_laser_);
// 创建一个GMapping::SensorMap类型的对象smap，用于存储传感器映射
GMapping::SensorMap smap;
// 将激光传感器的信息插入到smap中，包括传感器的名称和对应的对象
smap.insert(make_pair(gsp_laser_->getName(), gsp_laser_));
// 设置gsp_对象的传感器映射为smap，确保GMapping模块能够识别和使用激光传感器
gsp_->setSensorMap(smap);
// 创建一个新的GMapping::OdometrySensor对象gsp_odom_，用于处理里程计数据
// odom_frame_参数用于指定里程计传感器的参考框架
gsp_odom_ = new GMapping::OdometrySensor(odom_frame_);
// 断言gsp_odom_对象创建成功，确保后续使用不会出现空指针问题
ROS_ASSERT(gsp_odom_);
// 获取初始姿态
GMapping::OrientedPoint initialPose;
if(!getOdomPose(initialPose, scan.header.stamp))
{
ROS_WARN("Unable to determine inital pose of laser! Starting point will be set to zero.");
initialPose = GMapping::OrientedPoint(0.0, 0.0, 0.0);
}
// 设置匹配参数，用于调整匹配过程的灵敏度和范围
gsp_->setMatchingParameters(maxUrange_, maxRange_, sigma_,
kernelSize_, lstep_, astep_, iterations_,
lsigma_, ogain_, lskip_);
// 设置运动模型参数，用于描述机器人运动的不确定性
gsp_->setMotionModelParameters(srr_, srt_, str_, stt_);
// 设置更新距离参数，决定何时进行线性和角位移更新
gsp_->setUpdateDistances(linearUpdate_, angularUpdate_, resampleThreshold_);
// 设置时间更新周期，控制地图更新的频率
gsp_->setUpdatePeriod(temporalUpdate_);
// 禁止自动生成地图，因为地图更新将基于其他考虑进行
gsp_->setgenerateMap(false);
// GMapping::GridSlamProcessor* gsp_;
// GMapping::RangeSensor* gsp_laser_;
// 初始化网格SLAM处理器，设置地图和机器人的初始状态
gsp_->GridSlamProcessor::init(particles_, xmin_, ymin_, xmax_, ymax_,
delta_, initialPose);
// 设置局部采样范围和步长，用于控制局部匹配的精度和范围
gsp_->setllsamplerange(llsamplerange_);
gsp_->setllsamplestep(llsamplestep_);
gsp_->setlasamplerange(lasamplerange_);
gsp_->setlasamplestep(lasamplestep_);
// 设置最小得分阈值，用于评估匹配质量并决定是否更新地图
gsp_->setminimumScore(minimum_score_);
// 调用采样函数一次以设置种子 高斯采样函数
GMapping::sampleGaussian(1,seed_);
ROS_INFO("Initialization complete");
return true;
}
gmapping参数初始化传入—gridslamprocessor.cpp
// 其他省略
void GridSlamProcessor::setMatchingParameters (double urange, double range, double sigma, int kernsize, double lopt, double aopt,
int iterations, double likelihoodSigma, double likelihoodGain, unsigned int likelihoodSkip){
m_obsSigmaGain=likelihoodGain;
m_matcher.setMatchingParameters(urange, range, sigma, kernsize, lopt, aopt, iterations, likelihoodSigma, likelihoodSkip);
if (m_infoStream)
m_infoStream << " -maxUrange "<< urange
<< " -maxUrange "<< range
<< " -sigma "<< sigma
<< " -kernelSize "<< kernsize
<< " -lstep " << lopt
<< " -lobsGain " << m_obsSigmaGain
<< " -astep " << aopt << endl;
}
高斯随机采样初始化
// 调用采样函数一次以设置种子 高斯采样函数
GMapping::sampleGaussian(1,seed_);
stat.cpp:utils/stat.cpp
/**
* 生成高斯分布的随机数
* @param sigma 标准差
* @return 生成的高斯分布随机数
*/
double pf_ran_gaussian(double sigma)
{
double x1, x2, w;
double r;
// 生成满足高斯分布的随机数对
do
{
// 生成均匀分布的随机数，并转换到[-1, 1]区间
do { r = drand48(); } while (r == 0.0);
x1 = 2.0 * r - 1.0;
do { r = drand48(); } while (r == 0.0);
x2 = 2.0 * drand48() - 1.0;
w = x1*x1 + x2*x2;
} while(w > 1.0 || w==0.0);
// 根据Box-Muller变换公式计算高斯分布随机数
return(sigma * x2 * sqrt(-2.0*log(w)/w));
}
/**
* 根据给定的标准差和种子采样高斯分布随机数
* @param sigma 标准差
* @param S 随机数生成器的种子
* @return 生成的高斯分布随机数
*/
double sampleGaussian(double sigma, unsigned long int S) {
// 初始化随机数生成器
/*
static gsl_rng * r = NULL;
if(r==NULL) {
gsl_rng_env_setup();
r = gsl_rng_alloc (gsl_rng_default);
}
*/
// 设置随机数生成器的种子
if (S!=0)
{
//gsl_rng_set(r, S);
srand48(S);
}
// 处理标准差为0的特殊情况
if (sigma==0)
return 0;
// 使用自定义函数生成高斯分布随机数
//return gsl_ran_gaussian (r,sigma);
return pf_ran_gaussian (sigma);
}
addScan函数
// 将激光雷达数据添加到SLAM后端GMapping中
// 参数: scan - 激光雷达数据, gmap_pose - 在GMapping中的姿态估计结果
// 返回值: bool - 添加成功与否
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需要一个双精度数组来表示激光雷达数据...
double* ranges_double = new double[scan.ranges.size()];
// 如果角度增量为负，我们需要反转读数的顺序
if (do_reverse_range_)
{
ROS_DEBUG("Inverting scan");
int num_ranges = scan.ranges.size();
// 反转读数，并过滤掉短的读数
for(int i=0; i < num_ranges; i++)
{
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++)
{
if(scan.ranges[i] < scan.range_min)
ranges_double[i] = (double)scan.range_max;
else
ranges_double[i] = (double)scan.ranges[i];
}
}
// 创建一个GMapping::RangeReading对象来表示这次激光雷达扫描
GMapping::RangeReading reading(scan.ranges.size(),
ranges_double,
gsp_laser_,
scan.header.stamp.toSec());
// 由于RangeReading构造函数深拷贝了数组，所以我们不再需要保留这个数组
delete[] ranges_double;
// 设置激光雷达扫描的姿态
reading.setPose(gmap_pose);
// 在处理激光雷达数据前打印调试信息
ROS_DEBUG("processing scan");
// 将激光雷达数据传递给GMapping处理
return gsp_->processScan(reading);
}
processScan函数：openslam_gmapping/gridfastslam/gridslamprocessor.cpp
bool GridSlamProcessor::processScan(const RangeReading & reading, int adaptParticles){
/** 从读取中检索位置，并计算里程计 */
OrientedPoint relPose = reading.getPose(); // 获取当前扫描的相对位姿
if (!m_count){ // 如果是第一次处理扫描
m_lastPartPose = m_odoPose = relPose; // 初始化上一次粒子位姿和里程计位姿
}
// 将读取的状态写入并使用运动模型更新所有粒子
for (ParticleVector::iterator it = m_particles.begin(); it != m_particles.end(); it++){
OrientedPoint& pose(it->pose);
pose = m_motionModel.drawFromMotion(it->pose, relPose, m_odoPose); // 根据运动模型更新粒子位姿
}
// 更新输出文件
if (m_outputStream.is_open()){
m_outputStream << setiosflags(ios::fixed) << setprecision(6);
m_outputStream << "ODOM ";
m_outputStream << setiosflags(ios::fixed) << setprecision(3) << m_odoPose.x << " " << m_odoPose.y << " ";
m_outputStream << setiosflags(ios::fixed) << setprecision(6) << m_odoPose.theta << " ";
m_outputStream << reading.getTime();
m_outputStream << endl;
}
if (m_outputStream.is_open()){
m_outputStream << setiosflags(ios::fixed) << setprecision(6);
m_outputStream << "ODO_UPDATE " << m_particles.size() << " ";
for (ParticleVector::iterator it = m_particles.begin(); it != m_particles.end(); it++){
OrientedPoint& pose(it->pose);
m_outputStream << setiosflags(ios::fixed) << setprecision(3) << pose.x << " " << pose.y << " ";
m_outputStream << setiosflags(ios::fixed) << setprecision(6) << pose.theta << " " << it-> weight << " ";
}
m_outputStream << reading.getTime();
m_outputStream << endl;
}
// 调用回调函数
onOdometryUpdate();
// 累积机器人的平移和旋转
OrientedPoint move = relPose - m_odoPose;
move.theta = atan2(sin(move.theta), cos(move.theta));
m_linearDistance += sqrt(move * move);
m_angularDistance += fabs(move.theta);
// 如果机器人跳跃则发出警告
if (m_linearDistance > 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_odoPose.theta << endl;
cerr << "New Odometry Pose (reported from observation)= " << relPose.x << " " << relPose.y
<< " " << relPose.theta << endl;
cerr << "***********************************************************************" << endl;
cerr << "** The Odometry has a big jump here. This is probably a bug in the **" << endl;
cerr << "** odometry/laser input. We continue now, but the result is probably **" << endl;
cerr << "** crap or can lead to a core dump since the map doesn't fit.... C&G **" << endl;
cerr << "***********************************************************************" << endl;
}
m_odoPose = relPose; // 更新里程计位姿
bool processed = false;
// 只有在机器人行驶了一定距离后才处理扫描
if (!m_count
|| m_linearDistance > m_linearThresholdDistance
|| m_angularDistance > m_angularThresholdDistance){
if (m_outputStream.is_open()){
m_outputStream << setiosflags(ios::fixed) << setprecision(6);
m_outputStream << "FRAME " << m_readingCount;
m_outputStream << " " << m_linearDistance;
m_outputStream << " " << m_angularDistance << endl;
}
if (m_infoStream)
m_infoStream << "update frame " << m_readingCount << endl
<< "update ld=" << m_linearDistance << " ad=" << m_angularDistance << endl;
cerr << "Laser Pose= " << reading.getPose().x << " " << reading.getPose().y
<< " " << reading.getPose().theta << endl;
// 将读取转换为扫描匹配器可用的形式
assert(reading.size() == m_beams);
double * plainReading = new double[m_beams];
for(unsigned int i = 0; i < m_beams; i++){
plainReading[i] = reading[i];
}
m_infoStream << "m_count " << m_count << endl;
if (m_count > 0){
scanMatch(plainReading); // 进行扫描匹配
if (m_outputStream.is_open()){
m_outputStream << "LASER_READING " << reading.size() << " ";
m_outputStream << setiosflags(ios::fixed) << setprecision(2);
for (RangeReading::const_iterator b = reading.begin(); b != reading.end(); b++){
m_outputStream << *b << " ";
}
OrientedPoint p = reading.getPose();
m_outputStream << setiosflags(ios::fixed) << setprecision(6);
m_outputStream << p.x << " " << p.y << " " << p.theta << " " << reading.getTime() << endl;
m_outputStream << "SM_UPDATE " << m_particles.size() << " ";
for (ParticleVector::const_iterator it = m_particles.begin(); it != m_particles.end(); it++){
const OrientedPoint& pose = it->pose;
m_outputStream << setiosflags(ios::fixed) << setprecision(3) << pose.x << " " << pose.y << " ";
m_outputStream << setiosflags(ios::fixed) << setprecision(6) << pose.theta << " " << it-> weight << " ";
}
m_outputStream << endl;
}
onScanmatchUpdate(); // 调用扫描匹配更新回调
updateTreeWeights(false); // 更新树权重
if (m_infoStream){
m_infoStream << "neff= " << m_neff << endl;
}
if (m_outputStream.is_open()){
m_outputStream << setiosflags(ios::fixed) << setprecision(6);
m_outputStream << "NEFF " << m_neff << endl;
}
resample(plainReading, adaptParticles); // 重采样
} else {
m_infoStream << "Registering First Scan" << endl;
for (ParticleVector::iterator it = m_particles.begin(); it != m_particles.end(); it++){
m_matcher.invalidateActiveArea();
m_matcher.computeActiveArea(it->map, it->pose, plainReading);
m_matcher.registerScan(it->map, it->pose, plainReading);
// cyr: 不再需要，粒子一开始就指向根节点！
TNode* node = new TNode(it->pose, 0., it->node, 0);
node->reading = 0;
it->node = node;
}
}
// 更新树权重
updateTreeWeights(false);
delete [] plainReading; // 释放内存
m_lastPartPose = m_odoPose; // 更新上一次粒子位姿
m_linearDistance = 0; // 重置线性距离
m_angularDistance = 0; // 重置角度距离
m_count++; // 增加计数
processed = true; // 设置处理标志
// 为下一次迭代准备
for (ParticleVector::iterator it = m_particles.begin(); it != m_particles.end(); it++){
it->previousPose = it->pose;
}
}
if (m_outputStream.is_open())
m_outputStream << flush;
m_readingCount++; // 增加读取计数
return processed; // 返回处理标志
}
scanMatch函数
// 在网格SLAM处理器中执行扫描匹配
// @param plainReading: 未处理的测距仪读数
inline void GridSlamProcessor::scanMatch(const double* plainReading){
// 初始化得分总和
double sumScore=0;
// 遍历所有粒子
for (ParticleVector::iterator it=m_particles.begin(); it!=m_particles.end(); it++){
// 存储校正后的位姿
OrientedPoint corrected;
// 存储匹配得分、似然和分数
double score, l, s;
// 优化校正后的位姿，并返回匹配得分
score=m_matcher.optimize(corrected, it->map, it->pose, plainReading);
// 如果匹配得分高于最小得分阈值，则更新粒子的位姿
if (score>m_minimumScore){
it->pose=corrected;
} else {
// 匹配失败时，使用里程计数据，并记录相关信息
if (m_infoStream){
m_infoStream << "Scan Matching Failed, using odometry. Likelihood=" << l <<std::endl;
m_infoStream << "lp:" << m_lastPartPose.x << " " << m_lastPartPose.y << " "<< m_lastPartPose.theta <<std::endl;
m_infoStream << "op:" << m_odoPose.x << " " << m_odoPose.y << " "<< m_odoPose.theta <<std::endl;
}
}
// 计算粒子的似然和分数
m_matcher.likelihoodAndScore(s, l, it->map, it->pose, plainReading);
// 更新得分总和和粒子的权重
sumScore+=score;
it->weight+=l;
it->weightSum+=l;
// 为选择性复制活跃区域做准备，通过分离将被更新的区域
m_matcher.invalidateActiveArea();
m_matcher.computeActiveArea(it->map, it->pose, plainReading);
}
// 记录平均扫描匹配得分
if (m_infoStream)
m_infoStream << "Average Scan Matching Score=" << sumScore/m_particles.size() << std::endl;
}
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
