【ROS】slam_gmapping.cpp 详注
/*
* slam_gmapping
* Copyright (c) 2008, Willow Garage, Inc.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions are met:
* * Redistributions of source code must retain the above copyright notice,
* this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in the
* documentation and/or other materials provided with the distribution.
* * Neither the names of Stanford University or Willow Garage, Inc. nor the names of its
* contributors may be used to endorse or promote products derived from
* this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
* ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
* LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
* CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
* SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
* INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
* CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
* ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
*/
/* 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"
@section topic ROS topics
Subscribes to (name/type):
- @b "scan"/sensor_msgs/LaserScan : 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": 处理的扫描数据门限，默认每次处理1个扫描数据（可以设置更大跳过一些扫描数据） @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" (float, default: 0.05)，endpoint匹配标准差 @b [double] standard deviation for the scan matching process (cell)
- @b "~/kernelSize"(int, default: 1)用于查找对应的kernel size @b [int] search window for the scan matching process
- @b "~/lstep" (float, default: 0.05)，平移优化步长 @b [double] initial search step for scan matching (linear)
- @b "~/astep" (float, default: 0.05)，旋转优化步长 @b [double] initial search step for scan matching (angular)
- @b "~/iterations"(int, default: 5)，扫描匹配迭代步数 @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" (float, default: 0.075)，用于扫描匹配概率的激光标准差 @b [double] standard deviation for the scan matching process (single laser beam)
- @b "~/ogain" (float, default: 3.0)，似然估计为平滑重采样影响使用的gain @b [double] gain for smoothing the likelihood
- @b "~/lskip"(int, default: 0)，每次扫描跳过的光束数. @b [int] take only every (n+1)th laser ray for computing a match (0 = take all rays)
- @b "~/minimumScore" (float, default: 0.0)，为获得好的扫描匹配输出结果，用于避免在大空间范围使用有限距离的激光扫描仪（如5m）出现的jumping pose estimates问题。 当 Scores高达600+，如果出现了该问题可以考虑设定值50。
@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" (float, default: 0.1)，平移时里程误差作为平移函数(rho/rho) @b [double] linear noise component (x and y)
- @b "~/stt" (float, default: 0.2)，旋转时的里程误差作为旋转函数 (theta/theta) @b [double] angular noise component (theta)
- @b "~/srt" (float, default: 0.2)，平移时的里程误差作为旋转函数 (rho/theta) @b [double] linear -> angular noise component
- @b "~/str" (float, default: 0.1)，旋转时的里程误差作为平移函数 (theta/rho) @b [double] angular -> linear noise component
Others:
- @b "~/linearUpdate" (float, default: 1.0)，机器人每旋转这么远的距离处理一次扫描 @b [double] the robot only processes new measurements if the robot has moved at least this many meters
- @b "~/angularUpdate" (float, default: 0.5)，每次机器人旋转这个角度时都要进行扫描 @b [double] the robot only processes new measurements if the robot has turned at least this many rads
- @b "~/resampleThreshold" （float, default: 0.5)，基于重采样门限的Neff @b [double] threshold at which the particles get resampled. Higher means more frequent resampling.
- @b "~/particles" (int, default: 30)，滤波器中粒子数目 @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" (float, default: -100.0)，地图初始尺寸，对应最小x值 @b [double] minimum x position in the map [m]
- @b "~/ymin" (float, default: -100.0)，地图初始尺寸，对应最小y值 @b [double] minimum y position in the map [m]
- @b "~/xmax" (float, default: 100.0)，地图初始尺寸，对应最大x值 @b [double] maximum x position in the map [m]
- @b "~/ymax" (float, default: 100.0)，地图初始尺寸，对应最大y值 @b [double] maximum y position in the map [m]
- @b "~/delta" (float, default: 0.05)，地图分辨率 @b [double] size of one pixel [m]
*/
#include "slam_gmapping.h"
#include
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; //delete
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);
}
void
SlamGMapping::laserCallback(const sensor_msgs::LaserScan::ConstPtr& scan)
{
laser_count_++; //默认值为1
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);
//transform坐标
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 地图可能已经展开，所以也要调整ros消息的大小
// width!=sizex 或 height ！= sizey
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();
}