slam_gmapping.cpp
Go to the documentation of this file.
00001 /*
00002  * slam_gmapping
00003  * Copyright (c) 2008, Willow Garage, Inc.
00004  *
00005  * THE WORK (AS DEFINED BELOW) IS PROVIDED UNDER THE TERMS OF THIS CREATIVE
00006  * COMMONS PUBLIC LICENSE ("CCPL" OR "LICENSE"). THE WORK IS PROTECTED BY
00007  * COPYRIGHT AND/OR OTHER APPLICABLE LAW. ANY USE OF THE WORK OTHER THAN AS
00008  * AUTHORIZED UNDER THIS LICENSE OR COPYRIGHT LAW IS PROHIBITED.
00009  * 
00010  * BY EXERCISING ANY RIGHTS TO THE WORK PROVIDED HERE, YOU ACCEPT AND AGREE TO
00011  * BE BOUND BY THE TERMS OF THIS LICENSE. THE LICENSOR GRANTS YOU THE RIGHTS
00012  * CONTAINED HERE IN CONSIDERATION OF YOUR ACCEPTANCE OF SUCH TERMS AND
00013  * CONDITIONS.
00014  *
00015  */
00016 
00017 /* Author: Brian Gerkey */
00018 /* Modified by: Charles DuHadway */
00019 
00020 
00108 #include "slam_gmapping.h"
00109 
00110 #include <iostream>
00111 
00112 #include <time.h>
00113 
00114 #include "ros/ros.h"
00115 #include "ros/console.h"
00116 #include "nav_msgs/MapMetaData.h"
00117 
00118 #include "gmapping/sensor/sensor_range/rangesensor.h"
00119 #include "gmapping/sensor/sensor_odometry/odometrysensor.h"
00120 
00121 #include <rosbag/bag.h>
00122 #include <rosbag/view.h>
00123 #include <boost/foreach.hpp>
00124 #define foreach BOOST_FOREACH
00125 
00126 // compute linear index for given map coords
00127 #define MAP_IDX(sx, i, j) ((sx) * (j) + (i))
00128 
00129 SlamGMapping::SlamGMapping():
00130   map_to_odom_(tf::Transform(tf::createQuaternionFromRPY( 0, 0, 0 ), tf::Point(0, 0, 0 ))),
00131   laser_count_(0), private_nh_("~"), scan_filter_sub_(NULL), scan_filter_(NULL), transform_thread_(NULL)
00132 {
00133   seed_ = time(NULL);
00134   init();
00135 }
00136 
00137 SlamGMapping::SlamGMapping(ros::NodeHandle& nh, ros::NodeHandle& pnh):
00138   map_to_odom_(tf::Transform(tf::createQuaternionFromRPY( 0, 0, 0 ), tf::Point(0, 0, 0 ))),
00139   laser_count_(0),node_(nh), private_nh_(pnh), scan_filter_sub_(NULL), scan_filter_(NULL), transform_thread_(NULL)
00140 {
00141   seed_ = time(NULL);
00142   init();
00143 }
00144 
00145 SlamGMapping::SlamGMapping(long unsigned int seed, long unsigned int max_duration_buffer):
00146   map_to_odom_(tf::Transform(tf::createQuaternionFromRPY( 0, 0, 0 ), tf::Point(0, 0, 0 ))),
00147   laser_count_(0), private_nh_("~"), scan_filter_sub_(NULL), scan_filter_(NULL), transform_thread_(NULL),
00148   seed_(seed), tf_(ros::Duration(max_duration_buffer))
00149 {
00150   init();
00151 }
00152 
00153 
00154 void SlamGMapping::init()
00155 {
00156   // log4cxx::Logger::getLogger(ROSCONSOLE_DEFAULT_NAME)->setLevel(ros::console::g_level_lookup[ros::console::levels::Debug]);
00157 
00158   // The library is pretty chatty
00159   //gsp_ = new GMapping::GridSlamProcessor(std::cerr);
00160   gsp_ = new GMapping::GridSlamProcessor();
00161   ROS_ASSERT(gsp_);
00162 
00163   tfB_ = new tf::TransformBroadcaster();
00164   ROS_ASSERT(tfB_);
00165 
00166   gsp_laser_ = NULL;
00167   gsp_odom_ = NULL;
00168 
00169   got_first_scan_ = false;
00170   got_map_ = false;
00171   
00172 
00173   
00174   // Parameters used by our GMapping wrapper
00175   if(!private_nh_.getParam("throttle_scans", throttle_scans_))
00176     throttle_scans_ = 1;
00177   if(!private_nh_.getParam("base_frame", base_frame_))
00178     base_frame_ = "base_link";
00179   if(!private_nh_.getParam("map_frame", map_frame_))
00180     map_frame_ = "map";
00181   if(!private_nh_.getParam("odom_frame", odom_frame_))
00182     odom_frame_ = "odom";
00183 
00184   private_nh_.param("transform_publish_period", transform_publish_period_, 0.05);
00185 
00186   double tmp;
00187   if(!private_nh_.getParam("map_update_interval", tmp))
00188     tmp = 5.0;
00189   map_update_interval_.fromSec(tmp);
00190   
00191   // Parameters used by GMapping itself
00192   maxUrange_ = 0.0;  maxRange_ = 0.0; // preliminary default, will be set in initMapper()
00193   if(!private_nh_.getParam("minimumScore", minimum_score_))
00194     minimum_score_ = 0;
00195   if(!private_nh_.getParam("sigma", sigma_))
00196     sigma_ = 0.05;
00197   if(!private_nh_.getParam("kernelSize", kernelSize_))
00198     kernelSize_ = 1;
00199   if(!private_nh_.getParam("lstep", lstep_))
00200     lstep_ = 0.05;
00201   if(!private_nh_.getParam("astep", astep_))
00202     astep_ = 0.05;
00203   if(!private_nh_.getParam("iterations", iterations_))
00204     iterations_ = 5;
00205   if(!private_nh_.getParam("lsigma", lsigma_))
00206     lsigma_ = 0.075;
00207   if(!private_nh_.getParam("ogain", ogain_))
00208     ogain_ = 3.0;
00209   if(!private_nh_.getParam("lskip", lskip_))
00210     lskip_ = 0;
00211   if(!private_nh_.getParam("srr", srr_))
00212     srr_ = 0.1;
00213   if(!private_nh_.getParam("srt", srt_))
00214     srt_ = 0.2;
00215   if(!private_nh_.getParam("str", str_))
00216     str_ = 0.1;
00217   if(!private_nh_.getParam("stt", stt_))
00218     stt_ = 0.2;
00219   if(!private_nh_.getParam("linearUpdate", linearUpdate_))
00220     linearUpdate_ = 1.0;
00221   if(!private_nh_.getParam("angularUpdate", angularUpdate_))
00222     angularUpdate_ = 0.5;
00223   if(!private_nh_.getParam("temporalUpdate", temporalUpdate_))
00224     temporalUpdate_ = -1.0;
00225   if(!private_nh_.getParam("resampleThreshold", resampleThreshold_))
00226     resampleThreshold_ = 0.5;
00227   if(!private_nh_.getParam("particles", particles_))
00228     particles_ = 30;
00229   if(!private_nh_.getParam("xmin", xmin_))
00230     xmin_ = -100.0;
00231   if(!private_nh_.getParam("ymin", ymin_))
00232     ymin_ = -100.0;
00233   if(!private_nh_.getParam("xmax", xmax_))
00234     xmax_ = 100.0;
00235   if(!private_nh_.getParam("ymax", ymax_))
00236     ymax_ = 100.0;
00237   if(!private_nh_.getParam("delta", delta_))
00238     delta_ = 0.05;
00239   if(!private_nh_.getParam("occ_thresh", occ_thresh_))
00240     occ_thresh_ = 0.25;
00241   if(!private_nh_.getParam("llsamplerange", llsamplerange_))
00242     llsamplerange_ = 0.01;
00243   if(!private_nh_.getParam("llsamplestep", llsamplestep_))
00244     llsamplestep_ = 0.01;
00245   if(!private_nh_.getParam("lasamplerange", lasamplerange_))
00246     lasamplerange_ = 0.005;
00247   if(!private_nh_.getParam("lasamplestep", lasamplestep_))
00248     lasamplestep_ = 0.005;
00249     
00250   if(!private_nh_.getParam("tf_delay", tf_delay_))
00251     tf_delay_ = transform_publish_period_;
00252 
00253 }
00254 
00255 
00256 void SlamGMapping::startLiveSlam()
00257 {
00258   entropy_publisher_ = private_nh_.advertise<std_msgs::Float64>("entropy", 1, true);
00259   sst_ = node_.advertise<nav_msgs::OccupancyGrid>("map", 1, true);
00260   sstm_ = node_.advertise<nav_msgs::MapMetaData>("map_metadata", 1, true);
00261   ss_ = node_.advertiseService("dynamic_map", &SlamGMapping::mapCallback, this);
00262   scan_filter_sub_ = new message_filters::Subscriber<sensor_msgs::LaserScan>(node_, "scan", 5);
00263   scan_filter_ = new tf::MessageFilter<sensor_msgs::LaserScan>(*scan_filter_sub_, tf_, odom_frame_, 5);
00264   scan_filter_->registerCallback(boost::bind(&SlamGMapping::laserCallback, this, _1));
00265 
00266   transform_thread_ = new boost::thread(boost::bind(&SlamGMapping::publishLoop, this, transform_publish_period_));
00267 }
00268 
00269 void SlamGMapping::startReplay(const std::string & bag_fname, std::string scan_topic)
00270 {
00271   double transform_publish_period;
00272   ros::NodeHandle private_nh_("~");
00273   entropy_publisher_ = private_nh_.advertise<std_msgs::Float64>("entropy", 1, true);
00274   sst_ = node_.advertise<nav_msgs::OccupancyGrid>("map", 1, true);
00275   sstm_ = node_.advertise<nav_msgs::MapMetaData>("map_metadata", 1, true);
00276   ss_ = node_.advertiseService("dynamic_map", &SlamGMapping::mapCallback, this);
00277   
00278   rosbag::Bag bag;
00279   bag.open(bag_fname, rosbag::bagmode::Read);
00280   
00281   std::vector<std::string> topics;
00282   topics.push_back(std::string("/tf"));
00283   topics.push_back(scan_topic);
00284   rosbag::View viewall(bag, rosbag::TopicQuery(topics));
00285 
00286   // Store up to 5 messages and there error message (if they cannot be processed right away)
00287   std::queue<std::pair<sensor_msgs::LaserScan::ConstPtr, std::string> > s_queue;
00288   foreach(rosbag::MessageInstance const m, viewall)
00289   {
00290     tf::tfMessage::ConstPtr cur_tf = m.instantiate<tf::tfMessage>();
00291     if (cur_tf != NULL) {
00292       for (size_t i = 0; i < cur_tf->transforms.size(); ++i)
00293       {
00294         geometry_msgs::TransformStamped transformStamped;
00295         tf::StampedTransform stampedTf;
00296         transformStamped = cur_tf->transforms[i];
00297         tf::transformStampedMsgToTF(transformStamped, stampedTf);
00298         tf_.setTransform(stampedTf);
00299       }
00300     }
00301 
00302     sensor_msgs::LaserScan::ConstPtr s = m.instantiate<sensor_msgs::LaserScan>();
00303     if (s != NULL) {
00304       if (!(ros::Time(s->header.stamp)).is_zero())
00305       {
00306         s_queue.push(std::make_pair(s, ""));
00307       }
00308       // Just like in live processing, only process the latest 5 scans
00309       if (s_queue.size() > 5) {
00310         ROS_WARN_STREAM("Dropping old scan: " << s_queue.front().second);
00311         s_queue.pop();
00312       }
00313       // ignoring un-timestamped tf data 
00314     }
00315 
00316     // Only process a scan if it has tf data
00317     while (!s_queue.empty())
00318     {
00319       try
00320       {
00321         tf::StampedTransform t;
00322         tf_.lookupTransform(s_queue.front().first->header.frame_id, odom_frame_, s_queue.front().first->header.stamp, t);
00323         this->laserCallback(s_queue.front().first);
00324         s_queue.pop();
00325       }
00326       // If tf does not have the data yet
00327       catch(tf2::TransformException& e)
00328       {
00329         // Store the error to display it if we cannot process the data after some time
00330         s_queue.front().second = std::string(e.what());
00331         break;
00332       }
00333     }
00334   }
00335 
00336   bag.close();
00337 }
00338 
00339 void SlamGMapping::publishLoop(double transform_publish_period){
00340   if(transform_publish_period == 0)
00341     return;
00342 
00343   ros::Rate r(1.0 / transform_publish_period);
00344   while(ros::ok()){
00345     publishTransform();
00346     r.sleep();
00347   }
00348 }
00349 
00350 SlamGMapping::~SlamGMapping()
00351 {
00352   if(transform_thread_){
00353     transform_thread_->join();
00354     delete transform_thread_;
00355   }
00356 
00357   delete gsp_;
00358   if(gsp_laser_)
00359     delete gsp_laser_;
00360   if(gsp_odom_)
00361     delete gsp_odom_;
00362   if (scan_filter_)
00363     delete scan_filter_;
00364   if (scan_filter_sub_)
00365     delete scan_filter_sub_;
00366 }
00367 
00368 bool
00369 SlamGMapping::getOdomPose(GMapping::OrientedPoint& gmap_pose, const ros::Time& t)
00370 {
00371   // Get the pose of the centered laser at the right time
00372   centered_laser_pose_.stamp_ = t;
00373   // Get the laser's pose that is centered
00374   tf::Stamped<tf::Transform> odom_pose;
00375   try
00376   {
00377     tf_.transformPose(odom_frame_, centered_laser_pose_, odom_pose);
00378   }
00379   catch(tf::TransformException e)
00380   {
00381     ROS_WARN("Failed to compute odom pose, skipping scan (%s)", e.what());
00382     return false;
00383   }
00384   double yaw = tf::getYaw(odom_pose.getRotation());
00385 
00386   gmap_pose = GMapping::OrientedPoint(odom_pose.getOrigin().x(),
00387                                       odom_pose.getOrigin().y(),
00388                                       yaw);
00389   return true;
00390 }
00391 
00392 bool
00393 SlamGMapping::initMapper(const sensor_msgs::LaserScan& scan)
00394 {
00395   laser_frame_ = scan.header.frame_id;
00396   // Get the laser's pose, relative to base.
00397   tf::Stamped<tf::Pose> ident;
00398   tf::Stamped<tf::Transform> laser_pose;
00399   ident.setIdentity();
00400   ident.frame_id_ = laser_frame_;
00401   ident.stamp_ = scan.header.stamp;
00402   try
00403   {
00404     tf_.transformPose(base_frame_, ident, laser_pose);
00405   }
00406   catch(tf::TransformException e)
00407   {
00408     ROS_WARN("Failed to compute laser pose, aborting initialization (%s)",
00409              e.what());
00410     return false;
00411   }
00412 
00413   // create a point 1m above the laser position and transform it into the laser-frame
00414   tf::Vector3 v;
00415   v.setValue(0, 0, 1 + laser_pose.getOrigin().z());
00416   tf::Stamped<tf::Vector3> up(v, scan.header.stamp,
00417                                       base_frame_);
00418   try
00419   {
00420     tf_.transformPoint(laser_frame_, up, up);
00421     ROS_DEBUG("Z-Axis in sensor frame: %.3f", up.z());
00422   }
00423   catch(tf::TransformException& e)
00424   {
00425     ROS_WARN("Unable to determine orientation of laser: %s",
00426              e.what());
00427     return false;
00428   }
00429   
00430   // gmapping doesnt take roll or pitch into account. So check for correct sensor alignment.
00431   if (fabs(fabs(up.z()) - 1) > 0.001)
00432   {
00433     ROS_WARN("Laser has to be mounted planar! Z-coordinate has to be 1 or -1, but gave: %.5f",
00434                  up.z());
00435     return false;
00436   }
00437 
00438   gsp_laser_beam_count_ = scan.ranges.size();
00439 
00440   double angle_center = (scan.angle_min + scan.angle_max)/2;
00441 
00442   if (up.z() > 0)
00443   {
00444     do_reverse_range_ = scan.angle_min > scan.angle_max;
00445     centered_laser_pose_ = tf::Stamped<tf::Pose>(tf::Transform(tf::createQuaternionFromRPY(0,0,angle_center),
00446                                                                tf::Vector3(0,0,0)), ros::Time::now(), laser_frame_);
00447     ROS_INFO("Laser is mounted upwards.");
00448   }
00449   else
00450   {
00451     do_reverse_range_ = scan.angle_min < scan.angle_max;
00452     centered_laser_pose_ = tf::Stamped<tf::Pose>(tf::Transform(tf::createQuaternionFromRPY(M_PI,0,-angle_center),
00453                                                                tf::Vector3(0,0,0)), ros::Time::now(), laser_frame_);
00454     ROS_INFO("Laser is mounted upside down.");
00455   }
00456 
00457   // Compute the angles of the laser from -x to x, basically symmetric and in increasing order
00458   laser_angles_.resize(scan.ranges.size());
00459   // Make sure angles are started so that they are centered
00460   double theta = - std::fabs(scan.angle_min - scan.angle_max)/2;
00461   for(unsigned int i=0; i<scan.ranges.size(); ++i)
00462   {
00463     laser_angles_[i]=theta;
00464     theta += std::fabs(scan.angle_increment);
00465   }
00466 
00467   ROS_DEBUG("Laser angles in laser-frame: min: %.3f max: %.3f inc: %.3f", scan.angle_min, scan.angle_max,
00468             scan.angle_increment);
00469   ROS_DEBUG("Laser angles in top-down centered laser-frame: min: %.3f max: %.3f inc: %.3f", laser_angles_.front(),
00470             laser_angles_.back(), std::fabs(scan.angle_increment));
00471 
00472   GMapping::OrientedPoint gmap_pose(0, 0, 0);
00473 
00474   // setting maxRange and maxUrange here so we can set a reasonable default
00475   ros::NodeHandle private_nh_("~");
00476   if(!private_nh_.getParam("maxRange", maxRange_))
00477     maxRange_ = scan.range_max - 0.01;
00478   if(!private_nh_.getParam("maxUrange", maxUrange_))
00479     maxUrange_ = maxRange_;
00480 
00481   // The laser must be called "FLASER".
00482   // We pass in the absolute value of the computed angle increment, on the
00483   // assumption that GMapping requires a positive angle increment.  If the
00484   // actual increment is negative, we'll swap the order of ranges before
00485   // feeding each scan to GMapping.
00486   gsp_laser_ = new GMapping::RangeSensor("FLASER",
00487                                          gsp_laser_beam_count_,
00488                                          fabs(scan.angle_increment),
00489                                          gmap_pose,
00490                                          0.0,
00491                                          maxRange_);
00492   ROS_ASSERT(gsp_laser_);
00493 
00494   GMapping::SensorMap smap;
00495   smap.insert(make_pair(gsp_laser_->getName(), gsp_laser_));
00496   gsp_->setSensorMap(smap);
00497 
00498   gsp_odom_ = new GMapping::OdometrySensor(odom_frame_);
00499   ROS_ASSERT(gsp_odom_);
00500 
00501 
00503   GMapping::OrientedPoint initialPose;
00504   if(!getOdomPose(initialPose, scan.header.stamp))
00505   {
00506     ROS_WARN("Unable to determine inital pose of laser! Starting point will be set to zero.");
00507     initialPose = GMapping::OrientedPoint(0.0, 0.0, 0.0);
00508   }
00509 
00510   gsp_->setMatchingParameters(maxUrange_, maxRange_, sigma_,
00511                               kernelSize_, lstep_, astep_, iterations_,
00512                               lsigma_, ogain_, lskip_);
00513 
00514   gsp_->setMotionModelParameters(srr_, srt_, str_, stt_);
00515   gsp_->setUpdateDistances(linearUpdate_, angularUpdate_, resampleThreshold_);
00516   gsp_->setUpdatePeriod(temporalUpdate_);
00517   gsp_->setgenerateMap(false);
00518   gsp_->GridSlamProcessor::init(particles_, xmin_, ymin_, xmax_, ymax_,
00519                                 delta_, initialPose);
00520   gsp_->setllsamplerange(llsamplerange_);
00521   gsp_->setllsamplestep(llsamplestep_);
00525   gsp_->setlasamplerange(lasamplerange_);
00526   gsp_->setlasamplestep(lasamplestep_);
00527   gsp_->setminimumScore(minimum_score_);
00528 
00529   // Call the sampling function once to set the seed.
00530   GMapping::sampleGaussian(1,seed_);
00531 
00532   ROS_INFO("Initialization complete");
00533 
00534   return true;
00535 }
00536 
00537 bool
00538 SlamGMapping::addScan(const sensor_msgs::LaserScan& scan, GMapping::OrientedPoint& gmap_pose)
00539 {
00540   if(!getOdomPose(gmap_pose, scan.header.stamp))
00541      return false;
00542   
00543   if(scan.ranges.size() != gsp_laser_beam_count_)
00544     return false;
00545 
00546   // GMapping wants an array of doubles...
00547   double* ranges_double = new double[scan.ranges.size()];
00548   // If the angle increment is negative, we have to invert the order of the readings.
00549   if (do_reverse_range_)
00550   {
00551     ROS_DEBUG("Inverting scan");
00552     int num_ranges = scan.ranges.size();
00553     for(int i=0; i < num_ranges; i++)
00554     {
00555       // Must filter out short readings, because the mapper won't
00556       if(scan.ranges[num_ranges - i - 1] < scan.range_min)
00557         ranges_double[i] = (double)scan.range_max;
00558       else
00559         ranges_double[i] = (double)scan.ranges[num_ranges - i - 1];
00560     }
00561   } else 
00562   {
00563     for(unsigned int i=0; i < scan.ranges.size(); i++)
00564     {
00565       // Must filter out short readings, because the mapper won't
00566       if(scan.ranges[i] < scan.range_min)
00567         ranges_double[i] = (double)scan.range_max;
00568       else
00569         ranges_double[i] = (double)scan.ranges[i];
00570     }
00571   }
00572 
00573   GMapping::RangeReading reading(scan.ranges.size(),
00574                                  ranges_double,
00575                                  gsp_laser_,
00576                                  scan.header.stamp.toSec());
00577 
00578   // ...but it deep copies them in RangeReading constructor, so we don't
00579   // need to keep our array around.
00580   delete[] ranges_double;
00581 
00582   reading.setPose(gmap_pose);
00583 
00584   /*
00585   ROS_DEBUG("scanpose (%.3f): %.3f %.3f %.3f\n",
00586             scan.header.stamp.toSec(),
00587             gmap_pose.x,
00588             gmap_pose.y,
00589             gmap_pose.theta);
00590             */
00591   ROS_DEBUG("processing scan");
00592 
00593   return gsp_->processScan(reading);
00594 }
00595 
00596 void
00597 SlamGMapping::laserCallback(const sensor_msgs::LaserScan::ConstPtr& scan)
00598 {
00599   laser_count_++;
00600   if ((laser_count_ % throttle_scans_) != 0)
00601     return;
00602 
00603   static ros::Time last_map_update(0,0);
00604 
00605   // We can't initialize the mapper until we've got the first scan
00606   if(!got_first_scan_)
00607   {
00608     if(!initMapper(*scan))
00609       return;
00610     got_first_scan_ = true;
00611   }
00612 
00613   GMapping::OrientedPoint odom_pose;
00614 
00615   if(addScan(*scan, odom_pose))
00616   {
00617     ROS_DEBUG("scan processed");
00618 
00619     GMapping::OrientedPoint mpose = gsp_->getParticles()[gsp_->getBestParticleIndex()].pose;
00620     ROS_DEBUG("new best pose: %.3f %.3f %.3f", mpose.x, mpose.y, mpose.theta);
00621     ROS_DEBUG("odom pose: %.3f %.3f %.3f", odom_pose.x, odom_pose.y, odom_pose.theta);
00622     ROS_DEBUG("correction: %.3f %.3f %.3f", mpose.x - odom_pose.x, mpose.y - odom_pose.y, mpose.theta - odom_pose.theta);
00623 
00624     tf::Transform laser_to_map = tf::Transform(tf::createQuaternionFromRPY(0, 0, mpose.theta), tf::Vector3(mpose.x, mpose.y, 0.0)).inverse();
00625     tf::Transform odom_to_laser = tf::Transform(tf::createQuaternionFromRPY(0, 0, odom_pose.theta), tf::Vector3(odom_pose.x, odom_pose.y, 0.0));
00626 
00627     map_to_odom_mutex_.lock();
00628     map_to_odom_ = (odom_to_laser * laser_to_map).inverse();
00629     map_to_odom_mutex_.unlock();
00630 
00631     if(!got_map_ || (scan->header.stamp - last_map_update) > map_update_interval_)
00632     {
00633       updateMap(*scan);
00634       last_map_update = scan->header.stamp;
00635       ROS_DEBUG("Updated the map");
00636     }
00637   } else
00638     ROS_DEBUG("cannot process scan");
00639 }
00640 
00641 double
00642 SlamGMapping::computePoseEntropy()
00643 {
00644   double weight_total=0.0;
00645   for(std::vector<GMapping::GridSlamProcessor::Particle>::const_iterator it = gsp_->getParticles().begin();
00646       it != gsp_->getParticles().end();
00647       ++it)
00648   {
00649     weight_total += it->weight;
00650   }
00651   double entropy = 0.0;
00652   for(std::vector<GMapping::GridSlamProcessor::Particle>::const_iterator it = gsp_->getParticles().begin();
00653       it != gsp_->getParticles().end();
00654       ++it)
00655   {
00656     if(it->weight/weight_total > 0.0)
00657       entropy += it->weight/weight_total * log(it->weight/weight_total);
00658   }
00659   return -entropy;
00660 }
00661 
00662 void
00663 SlamGMapping::updateMap(const sensor_msgs::LaserScan& scan)
00664 {
00665   ROS_DEBUG("Update map");
00666   boost::mutex::scoped_lock map_lock (map_mutex_);
00667   GMapping::ScanMatcher matcher;
00668 
00669   matcher.setLaserParameters(scan.ranges.size(), &(laser_angles_[0]),
00670                              gsp_laser_->getPose());
00671 
00672   matcher.setlaserMaxRange(maxRange_);
00673   matcher.setusableRange(maxUrange_);
00674   matcher.setgenerateMap(true);
00675 
00676   GMapping::GridSlamProcessor::Particle best =
00677           gsp_->getParticles()[gsp_->getBestParticleIndex()];
00678   std_msgs::Float64 entropy;
00679   entropy.data = computePoseEntropy();
00680   if(entropy.data > 0.0)
00681     entropy_publisher_.publish(entropy);
00682 
00683   if(!got_map_) {
00684     map_.map.info.resolution = delta_;
00685     map_.map.info.origin.position.x = 0.0;
00686     map_.map.info.origin.position.y = 0.0;
00687     map_.map.info.origin.position.z = 0.0;
00688     map_.map.info.origin.orientation.x = 0.0;
00689     map_.map.info.origin.orientation.y = 0.0;
00690     map_.map.info.origin.orientation.z = 0.0;
00691     map_.map.info.origin.orientation.w = 1.0;
00692   } 
00693 
00694   GMapping::Point center;
00695   center.x=(xmin_ + xmax_) / 2.0;
00696   center.y=(ymin_ + ymax_) / 2.0;
00697 
00698   GMapping::ScanMatcherMap smap(center, xmin_, ymin_, xmax_, ymax_, 
00699                                 delta_);
00700 
00701   ROS_DEBUG("Trajectory tree:");
00702   for(GMapping::GridSlamProcessor::TNode* n = best.node;
00703       n;
00704       n = n->parent)
00705   {
00706     ROS_DEBUG("  %.3f %.3f %.3f",
00707               n->pose.x,
00708               n->pose.y,
00709               n->pose.theta);
00710     if(!n->reading)
00711     {
00712       ROS_DEBUG("Reading is NULL");
00713       continue;
00714     }
00715     matcher.invalidateActiveArea();
00716     matcher.computeActiveArea(smap, n->pose, &((*n->reading)[0]));
00717     matcher.registerScan(smap, n->pose, &((*n->reading)[0]));
00718   }
00719 
00720   // the map may have expanded, so resize ros message as well
00721   if(map_.map.info.width != (unsigned int) smap.getMapSizeX() || map_.map.info.height != (unsigned int) smap.getMapSizeY()) {
00722 
00723     // NOTE: The results of ScanMatcherMap::getSize() are different from the parameters given to the constructor
00724     //       so we must obtain the bounding box in a different way
00725     GMapping::Point wmin = smap.map2world(GMapping::IntPoint(0, 0));
00726     GMapping::Point wmax = smap.map2world(GMapping::IntPoint(smap.getMapSizeX(), smap.getMapSizeY()));
00727     xmin_ = wmin.x; ymin_ = wmin.y;
00728     xmax_ = wmax.x; ymax_ = wmax.y;
00729     
00730     ROS_DEBUG("map size is now %dx%d pixels (%f,%f)-(%f, %f)", smap.getMapSizeX(), smap.getMapSizeY(),
00731               xmin_, ymin_, xmax_, ymax_);
00732 
00733     map_.map.info.width = smap.getMapSizeX();
00734     map_.map.info.height = smap.getMapSizeY();
00735     map_.map.info.origin.position.x = xmin_;
00736     map_.map.info.origin.position.y = ymin_;
00737     map_.map.data.resize(map_.map.info.width * map_.map.info.height);
00738 
00739     ROS_DEBUG("map origin: (%f, %f)", map_.map.info.origin.position.x, map_.map.info.origin.position.y);
00740   }
00741 
00742   for(int x=0; x < smap.getMapSizeX(); x++)
00743   {
00744     for(int y=0; y < smap.getMapSizeY(); y++)
00745     {
00747       GMapping::IntPoint p(x, y);
00748       double occ=smap.cell(p);
00749       assert(occ <= 1.0);
00750       if(occ < 0)
00751         map_.map.data[MAP_IDX(map_.map.info.width, x, y)] = -1;
00752       else if(occ > occ_thresh_)
00753       {
00754         //map_.map.data[MAP_IDX(map_.map.info.width, x, y)] = (int)round(occ*100.0);
00755         map_.map.data[MAP_IDX(map_.map.info.width, x, y)] = 100;
00756       }
00757       else
00758         map_.map.data[MAP_IDX(map_.map.info.width, x, y)] = 0;
00759     }
00760   }
00761   got_map_ = true;
00762 
00763   //make sure to set the header information on the map
00764   map_.map.header.stamp = ros::Time::now();
00765   map_.map.header.frame_id = tf_.resolve( map_frame_ );
00766 
00767   sst_.publish(map_.map);
00768   sstm_.publish(map_.map.info);
00769 }
00770 
00771 bool 
00772 SlamGMapping::mapCallback(nav_msgs::GetMap::Request  &req,
00773                           nav_msgs::GetMap::Response &res)
00774 {
00775   boost::mutex::scoped_lock map_lock (map_mutex_);
00776   if(got_map_ && map_.map.info.width && map_.map.info.height)
00777   {
00778     res = map_;
00779     return true;
00780   }
00781   else
00782     return false;
00783 }
00784 
00785 void SlamGMapping::publishTransform()
00786 {
00787   map_to_odom_mutex_.lock();
00788   ros::Time tf_expiration = ros::Time::now() + ros::Duration(tf_delay_);
00789   tfB_->sendTransform( tf::StampedTransform (map_to_odom_, tf_expiration, map_frame_, odom_frame_));
00790   map_to_odom_mutex_.unlock();
00791 }


gmapping
Author(s): Brian Gerkey
autogenerated on Wed Aug 9 2017 02:37:49