/*
* slam_gmapping
* Copyright (c) 2008, Willow Garage, Inc.
*
* THE WORK (AS DEFINED BELOW) IS PROVIDED UNDER THE TERMS OF THIS CREATIVE
* COMMONS PUBLIC LICENSE ("CCPL" OR "LICENSE"). THE WORK IS PROTECTED BY
* COPYRIGHT AND/OR OTHER APPLICABLE LAW. ANY USE OF THE WORK OTHER THAN AS
* AUTHORIZED UNDER THIS LICENSE OR COPYRIGHT LAW IS PROHIBITED.
*
* BY EXERCISING ANY RIGHTS TO THE WORK PROVIDED HERE, YOU ACCEPT AND AGREE TO
* BE BOUND BY THE TERMS OF THIS LICENSE. THE LICENSOR GRANTS YOU THE RIGHTS
* CONTAINED HERE IN CONSIDERATION OF YOUR ACCEPTANCE OF SUCH TERMS AND
* CONDITIONS.
*
*/
/* Author: Brian Gerkey */
/* Modified by: Charles DuHadway */
/**
@mainpage slam_gmapping
@htmlinclude manifest.html
@b slam_gmapping is a wrapper around the GMapping SLAM library. It reads laser
scans and odometry and computes a map. This map can be
written to a file using e.g.
"rosrun map_server map_saver static_map:=dynamic_map"
<hr>
@section topic ROS topics
Subscribes to (name/type):
- @b "scan"/<a href="../../sensor_msgs/html/classstd__msgs_1_1LaserScan.html">sensor_msgs/LaserScan</a> : data from a laser range scanner
- @b "/tf": odometry from the robot
Publishes to (name/type):
- @b "/tf"/tf/tfMessage: position relative to the map
@section services
- @b "~dynamic_map" : returns the map
@section parameters ROS parameters
Reads the following parameters from the parameter server
Parameters used by our GMapping wrapper:
- @b "~throttle_scans": @b [int] throw away every nth laser scan
- @b "~base_frame": @b [string] the tf frame_id to use for the robot base pose
- @b "~map_frame": @b [string] the tf frame_id where the robot pose on the map is published
- @b "~odom_frame": @b [string] the tf frame_id from which odometry is read
- @b "~map_update_interval": @b [double] time in seconds between two recalculations of the map
Parameters used by GMapping itself:
Laser Parameters:
- @b "~/maxRange" @b [double] maximum range of the laser scans. Rays beyond this range get discarded completely. (default: maximum laser range minus 1 cm, as received in the the first LaserScan message)
- @b "~/maxUrange" @b [double] maximum range of the laser scanner that is used for map building (default: same as maxRange)
- @b "~/sigma" @b [double] standard deviation for the scan matching process (cell)
- @b "~/kernelSize" @b [int] search window for the scan matching process
- @b "~/lstep" @b [double] initial search step for scan matching (linear)
- @b "~/astep" @b [double] initial search step for scan matching (angular)
- @b "~/iterations" @b [int] number of refinement steps in the scan matching. The final "precision" for the match is lstep*2^(-iterations) or astep*2^(-iterations), respectively.
- @b "~/lsigma" @b [double] standard deviation for the scan matching process (single laser beam)
- @b "~/ogain" @b [double] gain for smoothing the likelihood
- @b "~/lskip" @b [int] take only every (n+1)th laser ray for computing a match (0 = take all rays)
- @b "~/minimumScore" @b [double] minimum score for considering the outcome of the scanmatching good. Can avoid 'jumping' pose estimates in large open spaces when using laser scanners with limited range (e.g. 5m). (0 = default. Scores go up to 600+, try 50 for example when experiencing 'jumping' estimate issues)
Motion Model Parameters (all standard deviations of a gaussian noise model)
- @b "~/srr" @b [double] linear noise component (x and y)
- @b "~/stt" @b [double] angular noise component (theta)
- @b "~/srt" @b [double] linear -> angular noise component
- @b "~/str" @b [double] angular -> linear noise component
Others:
- @b "~/linearUpdate" @b [double] the robot only processes new measurements if the robot has moved at least this many meters
- @b "~/angularUpdate" @b [double] the robot only processes new measurements if the robot has turned at least this many rads
- @b "~/resampleThreshold" @b [double] threshold at which the particles get resampled. Higher means more frequent resampling.
- @b "~/particles" @b [int] (fixed) number of particles. Each particle represents a possible trajectory that the robot has traveled
Likelihood sampling (used in scan matching)
- @b "~/llsamplerange" @b [double] linear range
- @b "~/lasamplerange" @b [double] linear step size
- @b "~/llsamplestep" @b [double] linear range
- @b "~/lasamplestep" @b [double] angular step size
Initial map dimensions and resolution:
- @b "~/xmin" @b [double] minimum x position in the map [m]
- @b "~/ymin" @b [double] minimum y position in the map [m]
- @b "~/xmax" @b [double] maximum x position in the map [m]
- @b "~/ymax" @b [double] maximum y position in the map [m]
- @b "~/delta" @b [double] size of one pixel [m]
*/
#include "slam_gmapping.h"
#include <iostream>
#include <time.h>
#include "ros/ros.h"
#include "ros/console.h"
#include "nav_msgs/MapMetaData.h"
#include "gmapping/sensor/sensor_range/rangesensor.h"
#include "gmapping/sensor/sensor_odometry/odometrysensor.h"
#include <rosbag/bag.h>
#include <rosbag/view.h>
#include <boost/foreach.hpp>
#define foreach BOOST_FOREACH
// compute linear index for given map coords
#define MAP_IDX(sx, i, j) ((sx) * (j) + (i))
SlamGMapping::SlamGMapping():
map_to_odom_(tf::Transform(tf::createQuaternionFromRPY( 0, 0, 0 ), tf::Point(0, 0, 0 ))),
laser_count_(0), private_nh_("~"), scan_filter_sub_(NULL), scan_filter_(NULL), transform_thread_(NULL)
{
seed_ = time(NULL);
init();
}
SlamGMapping::SlamGMapping(ros::NodeHandle& nh, ros::NodeHandle& pnh):
map_to_odom_(tf::Transform(tf::createQuaternionFromRPY( 0, 0, 0 ), tf::Point(0, 0, 0 ))),
laser_count_(0),node_(nh), private_nh_(pnh), scan_filter_sub_(NULL), scan_filter_(NULL), transform_thread_(NULL)
{
seed_ = time(NULL);
init();
}
SlamGMapping::SlamGMapping(long unsigned int seed, long unsigned int max_duration_buffer):
map_to_odom_(tf::Transform(tf::createQuaternionFromRPY( 0, 0, 0 ), tf::Point(0, 0, 0 ))),
laser_count_(0) ,//激光雷达计数
private_nh_("~"), //私有的句柄
scan_filter_sub_(NULL),//激光雷达订阅
scan_filter_(NULL), //激光雷达滤波器
transform_thread_(NULL),//变换的线程，
seed_(seed), tf_(ros::Duration(max_duration_buffer))
{
init();
}
void SlamGMapping::init()
{
// log4cxx::Logger::getLogger(ROSCONSOLE_DEFAULT_NAME)->setLevel(ros::console::g_level_lookup[ros::console::levels::Debug]);
// The library is pretty chatty
//gsp_ = new GMapping::GridSlamProcessor(std::cerr);
gsp_ = new GMapping::GridSlamProcessor();
ROS_ASSERT(gsp_);
//gmapping这个命名空间是在basic GridFastSLAM算法的基础上，每个粒子都有其自己的地图和机器人的位姿。
tfB_ = new tf::TransformBroadcaster();
ROS_ASSERT(tfB_);
gsp_laser_ = NULL;
gsp_odom_ = NULL;
got_first_scan_ = false;
got_map_ = false;
//让其里程计的数据为NULL，空指针
// Parameters used by our GMapping wrapper
if(!private_nh_.getParam("throttle_scans", throttle_scans_))
throttle_scans_ = 1;
if(!private_nh_.getParam("base_frame", base_frame_))
base_frame_ = "base_link";
if(!private_nh_.getParam("map_frame", map_frame_))
map_frame_ = "map";
if(!private_nh_.getParam("odom_frame", odom_frame_))
odom_frame_ = "odom";
//throttle_scans表示处理的容忍度吧，最小的容忍度。如果设置很高，就会漏掉更多的点
//设置参数的名字，数字和string类型，都可以，然后对这个参数进行设置
private_nh_.param("transform_publish_period", transform_publish_period_, 0.05);
double tmp;
if(!private_nh_.getParam("map_update_interval", tmp))
tmp = 5.0;
map_update_interval_.fromSec(tmp);
// Parameters used by GMapping itself
maxUrange_ = 0.0; maxRange_ = 0.0; // preliminary default, will be set in initMapper()
if(!private_nh_.getParam("minimumScore", minimum_score_))
minimum_score_ = 0;//每束激光雷达扫描的可能性
if(!private_nh_.getParam("sigma", sigma_))
sigma_ = 0.05;//用于暴力匹配
if(!private_nh_.getParam("kernelSize", kernelSize_))
kernelSize_ = 1;//寻找关联的
if(!private_nh_.getParam("lstep", lstep_))
lstep_ = 0.05;//平移优化的步长
if(!private_nh_.getParam("astep", astep_))
astep_ = 0.05;//旋转优化的步长
if(!private_nh_.getParam("iterations", iterations_))
iterations_ = 5;//旋转优化的步长
if(!private_nh_.getParam("lsigma", lsigma_))
lsigma_ = 0.075;//平移优化的步长
if(!private_nh_.getParam("ogain", ogain_))
ogain_ = 3.0;//为了重新采样而评估的增益
if(!private_nh_.getParam("lskip", lskip_))
lskip_ = 0;//每束激光雷达跳过的激光束
if(!private_nh_.getParam("srr", srr_))
srr_ = 0.1;//srr里程计的平移的/平移
if(!private_nh_.getParam("srt", srt_))
srt_ = 0.2;//srt里程计的平移/旋转
if(!private_nh_.getParam("str", str_))
str_ = 0.1;//stt里程计的旋转/旋转
if(!private_nh_.getParam("stt", stt_))
stt_ = 0.2;//str里程计的旋转/平移
if(!private_nh_.getParam("linearUpdate", linearUpdate_))
linearUpdate_ = 1.0;
if(!private_nh_.getParam("angularUpdate", angularUpdate_))
angularUpdate_ = 0.5;
if(!private_nh_.getParam("temporalUpdate", temporalUpdate_))
temporalUpdate_ = -1.0;
if(!private_nh_.getParam("resampleThreshold", resampleThreshold_))
resampleThreshold_ = 0.5;
if(!private_nh_.getParam("particles", particles_))
particles_ = 30;
if(!private_nh_.getParam("xmin", xmin_))
xmin_ = -100.0;
if(!private_nh_.getParam("ymin", ymin_))
ymin_ = -100.0;
if(!private_nh_.getParam("xmax", xmax_))
xmax_ = 100.0;
if(!private_nh_.getParam("ymax", ymax_))
ymax_ = 100.0;
if(!private_nh_.getParam("delta", delta_))
delta_ = 0.05;
if(!private_nh_.getParam("occ_thresh", occ_thresh_))
occ_thresh_ = 0.25;
if(!private_nh_.getParam("llsamplerange", llsamplerange_))
llsamplerange_ = 0.01;
if(!private_nh_.getParam("llsamplestep", llsamplestep_))
llsamplestep_ = 0.01;
if(!private_nh_.getParam("lasamplerange", lasamplerange_))
lasamplerange_ = 0.005;
if(!private_nh_.getParam("lasamplestep", lasamplestep_))
lasamplestep_ = 0.005;
if(!private_nh_.getParam("tf_delay", tf_delay_))
tf_delay_ = transform_publish_period_;
}
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);//订阅的话题
scan_filter_ = new tf::MessageFilter<sensor_msgs::LaserScan>(*scan_filter_sub_, tf_, odom_frame_, 5);
// 赋值
scan_filter_->registerCallback(boost::bind(&SlamGMapping::laserCallback, this, _1));
//从这里进入同一个文件的laserCallback函数
transform_thread_ = new boost::thread(boost::bind(&SlamGMapping::publishLoop, this, transform_publish_period_));
//开始一个线程
}
void SlamGMapping::startReplay(const std::string & bag_fname, std::string scan_topic)
{
double transform_publish_period;
ros::NodeHandle private_nh_("~");
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);
rosbag::Bag bag;
bag.open(bag_fname, rosbag::bagmode::Read);
std::vector<std::string> topics;
topics.push_back(std::string("/tf"));
topics.push_back(scan_topic);
rosbag::View viewall(bag, rosbag::TopicQuery(topics));
// Store up to 5 messages and there error message (if they cannot be processed right away)
std::queue<std::pair<sensor_msgs::LaserScan::ConstPtr, std::string> > s_queue;
foreach(rosbag::MessageInstance const m, viewall)
{
tf::tfMessage::ConstPtr cur_tf = m.instantiate<tf::tfMessage>();
if (cur_tf != NULL) {
for (size_t i = 0; i < cur_tf->transforms.size(); ++i)
{
geometry_msgs::TransformStamped transformStamped;
tf::StampedTransform stampedTf;
transformStamped = cur_tf->transforms[i];
tf::transformStampedMsgToTF(transformStamped, stampedTf);
tf_.setTransform(stampedTf);
}
}
sensor_msgs::LaserScan::ConstPtr s = m.instantiate<sensor_msgs::LaserScan>();
if (s != NULL) {
if (!(ros::Time(s->header.stamp)).is_zero())
{
s_queue.push(std::make_pair(s, ""));
}
// Just like in live processing, only process the latest 5 scans
if (s_queue.size() > 5) {
ROS_WARN_STREAM("Dropping old scan: " << s_queue.front().second);
s_queue.pop();
}
// ignoring un-timestamped tf data
}
// Only process a scan if it has tf data
while (!s_queue.empty())
{
try
{
tf::StampedTransform t;
tf_.lookupTransform(s_queue.front().first->header.frame_id, odom_frame_, s_queue.front().first->header.stamp, t);
this->laserCallback(s_queue.front().first);
s_queue.pop();
}
// If tf does not have the data yet
catch(tf2::TransformException& e)
{
// Store the error to display it if we cannot process the data after some time
s_queue.front().second = std::string(e.what());
break;
}
}
}
bag.close();
}
void SlamGMapping::publishLoop(double transform_publish_period){
if(transform_publish_period == 0)
return;
ros::Rate r(1.0 / transform_publish_period);
while(ros::ok()){
publishTransform();
r.sleep();
}
}
SlamGMapping::~SlamGMapping()//析构函数
{
if(transform_thread_){
transform_thread_->join();
delete transform_thread_;
}
delete gsp_;
if(gsp_laser_)
delete gsp_laser_;
if(gsp_odom_)
delete gsp_odom_;
if (scan_filter_)
delete scan_filter_;
if (scan_filter_sub_)
delete scan_filter_sub_;
}
bool
SlamGMapping::getOdomPose(GMapping::OrientedPoint& gmap_pose, const ros::Time& t)
{
// Get the pose of the centered laser at the right time
centered_laser_pose_.stamp_ = t;
// Get the laser's pose that is centered
tf::Stamped<tf::Transform> odom_pose;
try
{
tf_.transformPose(odom_frame_, centered_laser_pose_, odom_pose);
}
catch(tf::TransformException e)
{
ROS_WARN("Failed to compute odom pose, skipping scan (%s)", e.what());
return false;
}
double yaw = tf::getYaw(odom_pose.getRotation());
gmap_pose = GMapping::OrientedPoint(odom_pose.getOrigin().x(),
odom_pose.getOrigin().y(),
yaw);laser_count_++;
return true;
}
bool
SlamGMapping::initMapper(const sensor_msgs::LaserScan& scan)
{
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_;//这是frameid
ident.stamp_ = scan.header.stamp;//这是ID
try//这里用来抛出一个异常，其实就是查看着两个TF之间的关系
{
tf_.transformPose(base_frame_, ident, laser_pose);//两个TF之间的关系
}
catch(tf::TransformException e)//这里输出一个e
{
ROS_WARN("Failed to compute laser pose, aborting initialization (%s)",
e.what());
return false;
}
//创造一个点在激光雷达laser_pose是他的位姿。然后就是在原点
// 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<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();//对gmapping 进行计算数值
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; //如果最小的值比最大的值大的话，那么返回一个bool值
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
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 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
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);
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;
}
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()];
// 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
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());
// ...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);
/*
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); // 从这里进入particle filter
}
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))// 进入initMapper 函数 为bool类型函数
return;
got_first_scan_ = true;
}
GMapping::OrientedPoint odom_pose;
if(addScan(*scan, odom_pose)) // 进入addScan函数
{
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");
}
double
SlamGMapping::computePoseEntropy()
{
double weight_total=0.0;
for(std::vector<GMapping::GridSlamProcessor::Particle>::const_iterator it = gsp_->getParticles().begin();
it != gsp_->getParticles().end();
++it)
{
weight_total += it->weight;
}
double entropy = 0.0;
for(std::vector<GMapping::GridSlamProcessor::Particle>::const_iterator it = gsp_->getParticles().begin();
it != gsp_->getParticles().end();
++it)
{
if(it->weight/weight_total > 0.0)
entropy += it->weight/weight_total * log(it->weight/weight_total);
}
return -entropy;
}
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);
/* 取最优粒子，根据权重和weightSum 判断（最大） */
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;
}
void SlamGMapping::publishTransform()
{
map_to_odom_mutex_.lock();
ros::Time tf_expiration = ros::Time::now() + ros::Duration(tf_delay_);
tfB_->sendTransform( tf::StampedTransform (map_to_odom_, tf_expiration, map_frame_, odom_frame_));
map_to_odom_mutex_.unlock();
}
ROS上slam_gmapping代码详解
猜你喜欢
转载自blog.csdn.net/qq_25368751/article/details/80635848
今日推荐
周排行