目录
main主函数
startLiveSlam函数
laserCallback激光处理回调函数
initMapper初始化地图函数
addScan激光数据处理函数
processScan函数
drawFromMotion运动模型数据更新函数
scanMatch扫描匹配函数
optimize位姿优化函数
score函数
likelihoodAndScore函数
invalidateActiveArea函数
computeActiveArea计算有效区域函数
updateTreeWeights更新粒子权重函数
normalize归一化函数
resetTree重置粒子树函数
propagateWeights计算权重函数
registerScan地图状态更新函数
resample粒子重采样函数
updateMap地图更新函数
Gmapping主要流程
main函数
#include <ros/ros.h>
#include "slam_gmapping.h"
int
main(int argc, char** argv)
{
ros::init(argc, argv, "slam_gmapping");//**初始化slam节点**
SlamGMapping gn;
gn.startLiveSlam();//开始slam进程
ros::spin();
return(0);
}
startLiveSlam函数
/*订阅一些主题 发布一些主题*/
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);
{
//订阅激光数据 同时和odom_frame之间的转换同步
scan_filter_sub_ = new message_filters::Subscriber<sensor_msgs::LaserScan>(node_, "scan", 5);
scan_filter_ = new tf::MessageFilter<sensor_msgs::LaserScan>(*scan_filter_sub_, tf_, odom_frame_, 5);
scan_filter_->registerCallback(boost::bind(&SlamGMapping::laserCallback, this, _1));
std::cout <<"Subscribe LaserScan & odom!!!"<<std::endl;
}
/*发布转换关系的线程*/
transform_thread_ = new boost::thread(boost::bind(&SlamGMapping::publishLoop, this, transform_publish_period_));
}
laserCallback函数
/*
* 接受到激光雷达数据的回调函数 在这里面调用addScan()函数
* 如果addScan()函数调用成功,也就是说激光数据被成功的插入到地图中后,
* 如果到了地图更新的时间,则对地图进行更新,通过调用updateMap()函数来进行相应的操作。
*
* laserCallback()->addScan()->gmapping::processScan()
* ->updateMap()
*
*/
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);
// 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");
}
initMapper函数
/**
* @brief SlamGMapping::initMapper gmapping算法的初始化
* 这个函数在收到的第一帧激光雷达数据的时候会被调用一次,之后就再也不会被调用了。
* 这个函数的功能主要就是对gmapping算法中需要的一些参数进行赋值,即对gmapping算法进行初始化
* 这个参数一开始由gmapping wrapper读取,但是在这个函数里面真正的会赋值给gmapping算法
* 1.判断激光雷达是否是水平放置的,如果不是 则报错。
* 2.假设激光雷达数据的角度是对称的 & 递增的 为每个激光束分配角度。
* 3.为gmapping算法设置各种需要的参数。
*
*
* @param scan
* @return
*/
bool SlamGMapping::initMapper(const sensor_msgs::LaserScan& scan)
{
//得到激光雷达相对于车身坐标系(base_link)的位姿
laser_frame_ = scan.header.frame_id;
// Get the laser's pose, relative to base.
tf::Stamped<tf::Pose> ident;
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;
}
// create a point 1m above the laser position and transform it into the laser-frame
// 创造一个比激光雷达高1m的一个点,然后把这个点转换到激光雷达坐标系 这个点主要用来检测激光雷达和车身的位姿关系
// 来判断激光雷达是否是倾斜的 判断条件:如果激光雷达是水平放置的,那么转换回去之后,z的坐标也依然是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 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::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.");
}
// Compute the angles of the laser from -x to x, basically symmetric and in increasing order
// 这里认为激光雷达数据的角度是对称的,且是从小到大递增的。
// 因此根据激光雷达数据中的angle_min,angle_max,angle_increment等数据为每个激光束分配角度。
laser_angles_.resize(scan.ranges.size());
// Make sure angles are started so that they are centered
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);
}
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);
// setting maxRange and maxUrange here so we can set a reasonable default
// 设置激光雷达的最大观测距离和最大使用距离
ros::NodeHandle private_nh_("~");
if(!private_nh_.getParam("maxRange", maxRange_))
maxRange_ = scan.range_max - 0.01;
if(!private_nh_.getParam("maxUrange", maxUrange_))
maxUrange_ = maxRange_;
// The laser must be called "FLASER".
// We pass in the absolute value of the computed angle increment, on the
// assumption that GMapping requires a positive angle increment. If the
// actual increment is negative, we'll swap the order of ranges before
// feeding each scan to GMapping.
// 根据上面得到的激光雷达的数据信息 来初始化一个激光传感器
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
GMapping::SensorMap smap;
smap.insert(make_pair(gsp_laser_->getName(), gsp_laser_));
gsp_->setSensorMap(smap);
//初始化里程计传感器
gsp_odom_ = new GMapping::OdometrySensor(odom_frame_);
ROS_ASSERT(gsp_odom_);
/// @todo Expose setting an initial pose
/// 得到里程计的初始位姿,如果没有 则把初始位姿设置为(0,0,0)
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);
}
//为gmapping算法设置各种参数
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);
gsp_->GridSlamProcessor::init(particles_, xmin_, ymin_, xmax_, ymax_,
delta_, initialPose);
gsp_->setllsamplerange(llsamplerange_);
gsp_->setllsamplestep(llsamplestep_);
/// @todo Check these calls; in the gmapping gui, they use
/// llsamplestep and llsamplerange intead of lasamplestep and
/// lasamplerange. It was probably a typo, but who knows.
gsp_->setlasamplerange(lasamplerange_);
gsp_->setlasamplestep(lasamplestep_);
gsp_->setminimumScore(minimum_score_);
// Call the sampling function once to set the seed.
GMapping::sampleGaussian(1,seed_);
ROS_INFO("Initialization complete");
return true;
}
addScan激光数据处理函数
/*
* 加入一个激光雷达的数据 主要函数 这里面会调用processScan()函数
* 这个函数被laserCallback()函数调用
*/
bool SlamGMapping::addScan(const sensor_msgs::LaserScan& scan, GMapping::OrientedPoint& gmap_pose)
{
//得到与激光的时间戳相对应的机器人的里程计的位姿
if(!getOdomPose(gmap_pose, scan.header.stamp))
return false;
//检测是否所有帧的数据都是相等的 如果不相等就进行计算 不知道为什么 感觉完全没必要啊。
//特别是对于champion_nav_msgs的LaserScan来说 激光束的数量经常变
if(scan.ranges.size() != gsp_laser_beam_count_)
return false;
// GMapping wants an array of doubles...