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 
00107 #include "slam_gmapping.h"
00108 
00109 #include <iostream>
00110 
00111 #include <time.h>
00112 
00113 #include "ros/ros.h"
00114 #include "ros/console.h"
00115 #include "nav_msgs/MapMetaData.h"
00116 
00117 #include "gmapping/sensor/sensor_range/rangesensor.h"
00118 #include "gmapping/sensor/sensor_odometry/odometrysensor.h"
00119 
00120 // compute linear index for given map coords
00121 #define MAP_IDX(sx, i, j) ((sx) * (j) + (i))
00122 
00123 SlamGMapping::SlamGMapping():
00124   map_to_odom_(tf::Transform(tf::createQuaternionFromRPY( 0, 0, 0 ), tf::Point(0, 0, 0 ))),
00125   laser_count_(0), transform_thread_(NULL)
00126 {
00127   // log4cxx::Logger::getLogger(ROSCONSOLE_DEFAULT_NAME)->setLevel(ros::console::g_level_lookup[ros::console::levels::Debug]);
00128 
00129   // The library is pretty chatty
00130   //gsp_ = new GMapping::GridSlamProcessor(std::cerr);
00131   gsp_ = new GMapping::GridSlamProcessor();
00132   ROS_ASSERT(gsp_);
00133 
00134   tfB_ = new tf::TransformBroadcaster();
00135   ROS_ASSERT(tfB_);
00136 
00137   gsp_laser_ = NULL;
00138   gsp_laser_angle_increment_ = 0.0;
00139   gsp_odom_ = NULL;
00140 
00141   got_first_scan_ = false;
00142   got_map_ = false;
00143 
00144   ros::NodeHandle private_nh_("~");
00145 
00146   // Parameters used by our GMapping wrapper
00147   if(!private_nh_.getParam("throttle_scans", throttle_scans_))
00148     throttle_scans_ = 1;
00149   if(!private_nh_.getParam("base_frame", base_frame_))
00150     base_frame_ = "base_link";
00151   if(!private_nh_.getParam("map_frame", map_frame_))
00152     map_frame_ = "map";
00153   if(!private_nh_.getParam("odom_frame", odom_frame_))
00154     odom_frame_ = "odom";
00155 
00156   double transform_publish_period;
00157   private_nh_.param("transform_publish_period", transform_publish_period, 0.05);
00158 
00159   double tmp;
00160   if(!private_nh_.getParam("map_update_interval", tmp))
00161     tmp = 5.0;
00162   map_update_interval_.fromSec(tmp);
00163   
00164   // Parameters used by GMapping itself
00165   maxUrange_ = 0.0;  maxRange_ = 0.0; // preliminary default, will be set in initMapper()
00166   if(!private_nh_.getParam("sigma", sigma_))
00167     sigma_ = 0.05;
00168   if(!private_nh_.getParam("kernelSize", kernelSize_))
00169     kernelSize_ = 1;
00170   if(!private_nh_.getParam("lstep", lstep_))
00171     lstep_ = 0.05;
00172   if(!private_nh_.getParam("astep", astep_))
00173     astep_ = 0.05;
00174   if(!private_nh_.getParam("iterations", iterations_))
00175     iterations_ = 5;
00176   if(!private_nh_.getParam("lsigma", lsigma_))
00177     lsigma_ = 0.075;
00178   if(!private_nh_.getParam("ogain", ogain_))
00179     ogain_ = 3.0;
00180   if(!private_nh_.getParam("lskip", lskip_))
00181     lskip_ = 0;
00182   if(!private_nh_.getParam("srr", srr_))
00183     srr_ = 0.1;
00184   if(!private_nh_.getParam("srt", srt_))
00185     srt_ = 0.2;
00186   if(!private_nh_.getParam("str", str_))
00187     str_ = 0.1;
00188   if(!private_nh_.getParam("stt", stt_))
00189     stt_ = 0.2;
00190   if(!private_nh_.getParam("linearUpdate", linearUpdate_))
00191     linearUpdate_ = 1.0;
00192   if(!private_nh_.getParam("angularUpdate", angularUpdate_))
00193     angularUpdate_ = 0.5;
00194   if(!private_nh_.getParam("temporalUpdate", temporalUpdate_))
00195     temporalUpdate_ = -1.0;
00196   if(!private_nh_.getParam("resampleThreshold", resampleThreshold_))
00197     resampleThreshold_ = 0.5;
00198   if(!private_nh_.getParam("particles", particles_))
00199     particles_ = 30;
00200   if(!private_nh_.getParam("xmin", xmin_))
00201     xmin_ = -100.0;
00202   if(!private_nh_.getParam("ymin", ymin_))
00203     ymin_ = -100.0;
00204   if(!private_nh_.getParam("xmax", xmax_))
00205     xmax_ = 100.0;
00206   if(!private_nh_.getParam("ymax", ymax_))
00207     ymax_ = 100.0;
00208   if(!private_nh_.getParam("delta", delta_))
00209     delta_ = 0.05;
00210   if(!private_nh_.getParam("occ_thresh", occ_thresh_))
00211     occ_thresh_ = 0.25;
00212   if(!private_nh_.getParam("llsamplerange", llsamplerange_))
00213     llsamplerange_ = 0.01;
00214   if(!private_nh_.getParam("llsamplestep", llsamplestep_))
00215     llsamplestep_ = 0.01;
00216   if(!private_nh_.getParam("lasamplerange", lasamplerange_))
00217     lasamplerange_ = 0.005;
00218   if(!private_nh_.getParam("lasamplestep", lasamplestep_))
00219     lasamplestep_ = 0.005;
00220     
00221   if(!private_nh_.getParam("tf_delay", tf_delay_))
00222     tf_delay_ = transform_publish_period;
00223 
00224   entropy_publisher_ = private_nh_.advertise<std_msgs::Float64>("entropy", 1, true);
00225   sst_ = node_.advertise<nav_msgs::OccupancyGrid>("map", 1, true);
00226   sstm_ = node_.advertise<nav_msgs::MapMetaData>("map_metadata", 1, true);
00227   ss_ = node_.advertiseService("dynamic_map", &SlamGMapping::mapCallback, this);
00228   scan_filter_sub_ = new message_filters::Subscriber<sensor_msgs::LaserScan>(node_, "scan", 5);
00229   scan_filter_ = new tf::MessageFilter<sensor_msgs::LaserScan>(*scan_filter_sub_, tf_, odom_frame_, 5);
00230   scan_filter_->registerCallback(boost::bind(&SlamGMapping::laserCallback, this, _1));
00231 
00232   transform_thread_ = new boost::thread(boost::bind(&SlamGMapping::publishLoop, this, transform_publish_period));
00233 }
00234 
00235 void SlamGMapping::publishLoop(double transform_publish_period){
00236   if(transform_publish_period == 0)
00237     return;
00238 
00239   ros::Rate r(1.0 / transform_publish_period);
00240   while(ros::ok()){
00241     publishTransform();
00242     r.sleep();
00243   }
00244 }
00245 
00246 SlamGMapping::~SlamGMapping()
00247 {
00248   if(transform_thread_){
00249     transform_thread_->join();
00250     delete transform_thread_;
00251   }
00252 
00253   delete gsp_;
00254   if(gsp_laser_)
00255     delete gsp_laser_;
00256   if(gsp_odom_)
00257     delete gsp_odom_;
00258   if (scan_filter_)
00259     delete scan_filter_;
00260   if (scan_filter_sub_)
00261     delete scan_filter_sub_;
00262 }
00263 
00264 bool
00265 SlamGMapping::getOdomPose(GMapping::OrientedPoint& gmap_pose, const ros::Time& t)
00266 {
00267   // Get the laser's pose
00268   tf::Stamped<tf::Pose> ident (tf::Transform(tf::createQuaternionFromRPY(0,0,0),
00269                                            tf::Vector3(0,0,0)), t, laser_frame_);
00270   tf::Stamped<tf::Transform> odom_pose;
00271   try
00272   {
00273     tf_.transformPose(odom_frame_, ident, odom_pose);
00274   }
00275   catch(tf::TransformException e)
00276   {
00277     ROS_WARN("Failed to compute odom pose, skipping scan (%s)", e.what());
00278     return false;
00279   }
00280   double yaw = tf::getYaw(odom_pose.getRotation());
00281 
00282   gmap_pose = GMapping::OrientedPoint(odom_pose.getOrigin().x(),
00283                                       odom_pose.getOrigin().y(),
00284                                       yaw);
00285   return true;
00286 }
00287 
00288 bool
00289 SlamGMapping::initMapper(const sensor_msgs::LaserScan& scan)
00290 {
00291   laser_frame_ = scan.header.frame_id;
00292   // Get the laser's pose, relative to base.
00293   tf::Stamped<tf::Pose> ident;
00294   tf::Stamped<tf::Transform> laser_pose;
00295   ident.setIdentity();
00296   ident.frame_id_ = laser_frame_;
00297   ident.stamp_ = scan.header.stamp;
00298   try
00299   {
00300     tf_.transformPose(base_frame_, ident, laser_pose);
00301   }
00302   catch(tf::TransformException e)
00303   {
00304     ROS_WARN("Failed to compute laser pose, aborting initialization (%s)",
00305              e.what());
00306     return false;
00307   }
00308 
00309   // create a point 1m above the laser position and transform it into the laser-frame
00310   tf::Vector3 v;
00311   v.setValue(0, 0, 1 + laser_pose.getOrigin().z());
00312   tf::Stamped<tf::Vector3> up(v, scan.header.stamp,
00313                                       base_frame_);
00314   try
00315   {
00316     tf_.transformPoint(laser_frame_, up, up);
00317     ROS_DEBUG("Z-Axis in sensor frame: %.3f", up.z());
00318   }
00319   catch(tf::TransformException& e)
00320   {
00321     ROS_WARN("Unable to determine orientation of laser: %s",
00322              e.what());
00323     return false;
00324   }
00325   
00326   // gmapping doesnt take roll or pitch into account. So check for correct sensor alignment.
00327   if (fabs(fabs(up.z()) - 1) > 0.001)
00328   {
00329     ROS_WARN("Laser has to be mounted planar! Z-coordinate has to be 1 or -1, but gave: %.5f",
00330                  up.z());
00331     return false;
00332   }
00333 
00334   gsp_laser_beam_count_ = scan.ranges.size();
00335 
00336   int orientationFactor;
00337   if (up.z() > 0)
00338   {
00339     orientationFactor = 1;
00340     ROS_INFO("Laser is mounted upwards.");
00341   }
00342   else
00343   {
00344     orientationFactor = -1;
00345     ROS_INFO("Laser is mounted upside down.");
00346   }
00347 
00348   angle_min_ = orientationFactor * scan.angle_min;
00349   angle_max_ = orientationFactor * scan.angle_max;
00350   gsp_laser_angle_increment_ = orientationFactor * scan.angle_increment;
00351   ROS_DEBUG("Laser angles top down in laser-frame: min: %.3f max: %.3f inc: %.3f", angle_min_, angle_max_, gsp_laser_angle_increment_);
00352 
00353   GMapping::OrientedPoint gmap_pose(0, 0, 0);
00354 
00355   // setting maxRange and maxUrange here so we can set a reasonable default
00356   ros::NodeHandle private_nh_("~");
00357   if(!private_nh_.getParam("maxRange", maxRange_))
00358     maxRange_ = scan.range_max - 0.01;
00359   if(!private_nh_.getParam("maxUrange", maxUrange_))
00360     maxUrange_ = maxRange_;
00361 
00362   // The laser must be called "FLASER".
00363   // We pass in the absolute value of the computed angle increment, on the
00364   // assumption that GMapping requires a positive angle increment.  If the
00365   // actual increment is negative, we'll swap the order of ranges before
00366   // feeding each scan to GMapping.
00367   gsp_laser_ = new GMapping::RangeSensor("FLASER",
00368                                          gsp_laser_beam_count_,
00369                                          fabs(gsp_laser_angle_increment_),
00370                                          gmap_pose,
00371                                          0.0,
00372                                          maxRange_);
00373   ROS_ASSERT(gsp_laser_);
00374 
00375   GMapping::SensorMap smap;
00376   smap.insert(make_pair(gsp_laser_->getName(), gsp_laser_));
00377   gsp_->setSensorMap(smap);
00378 
00379   gsp_odom_ = new GMapping::OdometrySensor(odom_frame_);
00380   ROS_ASSERT(gsp_odom_);
00381 
00382 
00384   GMapping::OrientedPoint initialPose;
00385   if(!getOdomPose(initialPose, scan.header.stamp))
00386   {
00387     ROS_WARN("Unable to determine inital pose of laser! Starting point will be set to zero.");
00388     initialPose = GMapping::OrientedPoint(0.0, 0.0, 0.0);
00389   }
00390 
00391   gsp_->setMatchingParameters(maxUrange_, maxRange_, sigma_,
00392                               kernelSize_, lstep_, astep_, iterations_,
00393                               lsigma_, ogain_, lskip_);
00394 
00395   gsp_->setMotionModelParameters(srr_, srt_, str_, stt_);
00396   gsp_->setUpdateDistances(linearUpdate_, angularUpdate_, resampleThreshold_);
00397   gsp_->setUpdatePeriod(temporalUpdate_);
00398   gsp_->setgenerateMap(false);
00399   gsp_->GridSlamProcessor::init(particles_, xmin_, ymin_, xmax_, ymax_,
00400                                 delta_, initialPose);
00401   gsp_->setllsamplerange(llsamplerange_);
00402   gsp_->setllsamplestep(llsamplestep_);
00406   gsp_->setlasamplerange(lasamplerange_);
00407   gsp_->setlasamplestep(lasamplestep_);
00408 
00409   // Call the sampling function once to set the seed.
00410   GMapping::sampleGaussian(1,time(NULL));
00411 
00412   ROS_INFO("Initialization complete");
00413 
00414   return true;
00415 }
00416 
00417 bool
00418 SlamGMapping::addScan(const sensor_msgs::LaserScan& scan, GMapping::OrientedPoint& gmap_pose)
00419 {
00420   if(!getOdomPose(gmap_pose, scan.header.stamp))
00421      return false;
00422   
00423   if(scan.ranges.size() != gsp_laser_beam_count_)
00424     return false;
00425 
00426   // GMapping wants an array of doubles...
00427   double* ranges_double = new double[scan.ranges.size()];
00428   // If the angle increment is negative, we have to invert the order of the readings.
00429   if (gsp_laser_angle_increment_ < 0)
00430   {
00431     ROS_DEBUG("Inverting scan");
00432     int num_ranges = scan.ranges.size();
00433     for(int i=0; i < num_ranges; i++)
00434     {
00435       // Must filter out short readings, because the mapper won't
00436       if(scan.ranges[i] < scan.range_min)
00437         ranges_double[i] = (double)scan.range_max;
00438       else
00439         ranges_double[i] = (double)scan.ranges[num_ranges - i - 1];
00440     }
00441   } else 
00442   {
00443     for(unsigned int i=0; i < scan.ranges.size(); i++)
00444     {
00445       // Must filter out short readings, because the mapper won't
00446       if(scan.ranges[i] < scan.range_min)
00447         ranges_double[i] = (double)scan.range_max;
00448       else
00449         ranges_double[i] = (double)scan.ranges[i];
00450     }
00451   }
00452 
00453   GMapping::RangeReading reading(scan.ranges.size(),
00454                                  ranges_double,
00455                                  gsp_laser_,
00456                                  scan.header.stamp.toSec());
00457 
00458   // ...but it deep copies them in RangeReading constructor, so we don't
00459   // need to keep our array around.
00460   delete[] ranges_double;
00461 
00462   reading.setPose(gmap_pose);
00463 
00464   /*
00465   ROS_DEBUG("scanpose (%.3f): %.3f %.3f %.3f\n",
00466             scan.header.stamp.toSec(),
00467             gmap_pose.x,
00468             gmap_pose.y,
00469             gmap_pose.theta);
00470             */
00471 
00472   return gsp_->processScan(reading);
00473 }
00474 
00475 void
00476 SlamGMapping::laserCallback(const sensor_msgs::LaserScan::ConstPtr& scan)
00477 {
00478   laser_count_++;
00479   if ((laser_count_ % throttle_scans_) != 0)
00480     return;
00481 
00482   static ros::Time last_map_update(0,0);
00483 
00484   // We can't initialize the mapper until we've got the first scan
00485   if(!got_first_scan_)
00486   {
00487     if(!initMapper(*scan))
00488       return;
00489     got_first_scan_ = true;
00490   }
00491 
00492   GMapping::OrientedPoint odom_pose;
00493   if(addScan(*scan, odom_pose))
00494   {
00495     ROS_DEBUG("scan processed");
00496 
00497     GMapping::OrientedPoint mpose = gsp_->getParticles()[gsp_->getBestParticleIndex()].pose;
00498     ROS_DEBUG("new best pose: %.3f %.3f %.3f", mpose.x, mpose.y, mpose.theta);
00499     ROS_DEBUG("odom pose: %.3f %.3f %.3f", odom_pose.x, odom_pose.y, odom_pose.theta);
00500     ROS_DEBUG("correction: %.3f %.3f %.3f", mpose.x - odom_pose.x, mpose.y - odom_pose.y, mpose.theta - odom_pose.theta);
00501 
00502     tf::Transform laser_to_map = tf::Transform(tf::createQuaternionFromRPY(0, 0, mpose.theta), tf::Vector3(mpose.x, mpose.y, 0.0)).inverse();
00503     tf::Transform odom_to_laser = tf::Transform(tf::createQuaternionFromRPY(0, 0, odom_pose.theta), tf::Vector3(odom_pose.x, odom_pose.y, 0.0));
00504 
00505     map_to_odom_mutex_.lock();
00506     map_to_odom_ = (odom_to_laser * laser_to_map).inverse();
00507     map_to_odom_mutex_.unlock();
00508 
00509     if(!got_map_ || (scan->header.stamp - last_map_update) > map_update_interval_)
00510     {
00511       updateMap(*scan);
00512       last_map_update = scan->header.stamp;
00513       ROS_DEBUG("Updated the map");
00514     }
00515   }
00516 }
00517 
00518 double
00519 SlamGMapping::computePoseEntropy()
00520 {
00521   double weight_total=0.0;
00522   for(std::vector<GMapping::GridSlamProcessor::Particle>::const_iterator it = gsp_->getParticles().begin();
00523       it != gsp_->getParticles().end();
00524       ++it)
00525   {
00526     weight_total += it->weight;
00527   }
00528   double entropy = 0.0;
00529   for(std::vector<GMapping::GridSlamProcessor::Particle>::const_iterator it = gsp_->getParticles().begin();
00530       it != gsp_->getParticles().end();
00531       ++it)
00532   {
00533     if(it->weight/weight_total > 0.0)
00534       entropy += it->weight/weight_total * log(it->weight/weight_total);
00535   }
00536   return -entropy;
00537 }
00538 
00539 void
00540 SlamGMapping::updateMap(const sensor_msgs::LaserScan& scan)
00541 {
00542   boost::mutex::scoped_lock map_lock (map_mutex_);
00543   GMapping::ScanMatcher matcher;
00544   double* laser_angles = new double[scan.ranges.size()];
00545   double theta = angle_min_;
00546   for(unsigned int i=0; i<scan.ranges.size(); i++)
00547   {
00548     if (gsp_laser_angle_increment_ < 0)
00549         laser_angles[scan.ranges.size()-i-1]=theta;
00550     else
00551         laser_angles[i]=theta;
00552     theta += gsp_laser_angle_increment_;
00553   }
00554 
00555   matcher.setLaserParameters(scan.ranges.size(), laser_angles,
00556                              gsp_laser_->getPose());
00557 
00558   delete[] laser_angles;
00559   matcher.setlaserMaxRange(maxRange_);
00560   matcher.setusableRange(maxUrange_);
00561   matcher.setgenerateMap(true);
00562 
00563   GMapping::GridSlamProcessor::Particle best =
00564           gsp_->getParticles()[gsp_->getBestParticleIndex()];
00565   std_msgs::Float64 entropy;
00566   entropy.data = computePoseEntropy();
00567   if(entropy.data > 0.0)
00568     entropy_publisher_.publish(entropy);
00569 
00570   if(!got_map_) {
00571     map_.map.info.resolution = delta_;
00572     map_.map.info.origin.position.x = 0.0;
00573     map_.map.info.origin.position.y = 0.0;
00574     map_.map.info.origin.position.z = 0.0;
00575     map_.map.info.origin.orientation.x = 0.0;
00576     map_.map.info.origin.orientation.y = 0.0;
00577     map_.map.info.origin.orientation.z = 0.0;
00578     map_.map.info.origin.orientation.w = 1.0;
00579   } 
00580 
00581   GMapping::Point center;
00582   center.x=(xmin_ + xmax_) / 2.0;
00583   center.y=(ymin_ + ymax_) / 2.0;
00584 
00585   GMapping::ScanMatcherMap smap(center, xmin_, ymin_, xmax_, ymax_, 
00586                                 delta_);
00587 
00588   ROS_DEBUG("Trajectory tree:");
00589   for(GMapping::GridSlamProcessor::TNode* n = best.node;
00590       n;
00591       n = n->parent)
00592   {
00593     ROS_DEBUG("  %.3f %.3f %.3f",
00594               n->pose.x,
00595               n->pose.y,
00596               n->pose.theta);
00597     if(!n->reading)
00598     {
00599       ROS_DEBUG("Reading is NULL");
00600       continue;
00601     }
00602     matcher.invalidateActiveArea();
00603     matcher.computeActiveArea(smap, n->pose, &((*n->reading)[0]));
00604     matcher.registerScan(smap, n->pose, &((*n->reading)[0]));
00605   }
00606 
00607   // the map may have expanded, so resize ros message as well
00608   if(map_.map.info.width != (unsigned int) smap.getMapSizeX() || map_.map.info.height != (unsigned int) smap.getMapSizeY()) {
00609 
00610     // NOTE: The results of ScanMatcherMap::getSize() are different from the parameters given to the constructor
00611     //       so we must obtain the bounding box in a different way
00612     GMapping::Point wmin = smap.map2world(GMapping::IntPoint(0, 0));
00613     GMapping::Point wmax = smap.map2world(GMapping::IntPoint(smap.getMapSizeX(), smap.getMapSizeY()));
00614     xmin_ = wmin.x; ymin_ = wmin.y;
00615     xmax_ = wmax.x; ymax_ = wmax.y;
00616     
00617     ROS_DEBUG("map size is now %dx%d pixels (%f,%f)-(%f, %f)", smap.getMapSizeX(), smap.getMapSizeY(),
00618               xmin_, ymin_, xmax_, ymax_);
00619 
00620     map_.map.info.width = smap.getMapSizeX();
00621     map_.map.info.height = smap.getMapSizeY();
00622     map_.map.info.origin.position.x = xmin_;
00623     map_.map.info.origin.position.y = ymin_;
00624     map_.map.data.resize(map_.map.info.width * map_.map.info.height);
00625 
00626     ROS_DEBUG("map origin: (%f, %f)", map_.map.info.origin.position.x, map_.map.info.origin.position.y);
00627   }
00628 
00629   for(int x=0; x < smap.getMapSizeX(); x++)
00630   {
00631     for(int y=0; y < smap.getMapSizeY(); y++)
00632     {
00634       GMapping::IntPoint p(x, y);
00635       double occ=smap.cell(p);
00636       assert(occ <= 1.0);
00637       if(occ < 0)
00638         map_.map.data[MAP_IDX(map_.map.info.width, x, y)] = -1;
00639       else if(occ > occ_thresh_)
00640       {
00641         //map_.map.data[MAP_IDX(map_.map.info.width, x, y)] = (int)round(occ*100.0);
00642         map_.map.data[MAP_IDX(map_.map.info.width, x, y)] = 100;
00643       }
00644       else
00645         map_.map.data[MAP_IDX(map_.map.info.width, x, y)] = 0;
00646     }
00647   }
00648   got_map_ = true;
00649 
00650   //make sure to set the header information on the map
00651   map_.map.header.stamp = ros::Time::now();
00652   map_.map.header.frame_id = tf_.resolve( map_frame_ );
00653 
00654   sst_.publish(map_.map);
00655   sstm_.publish(map_.map.info);
00656 }
00657 
00658 bool 
00659 SlamGMapping::mapCallback(nav_msgs::GetMap::Request  &req,
00660                           nav_msgs::GetMap::Response &res)
00661 {
00662   boost::mutex::scoped_lock map_lock (map_mutex_);
00663   if(got_map_ && map_.map.info.width && map_.map.info.height)
00664   {
00665     res = map_;
00666     return true;
00667   }
00668   else
00669     return false;
00670 }
00671 
00672 void SlamGMapping::publishTransform()
00673 {
00674   map_to_odom_mutex_.lock();
00675   ros::Time tf_expiration = ros::Time::now() + ros::Duration(tf_delay_);
00676   tfB_->sendTransform( tf::StampedTransform (map_to_odom_, tf_expiration, map_frame_, odom_frame_));
00677   map_to_odom_mutex_.unlock();
00678 }


gmapping
Author(s): Giorgio Grisetti, Cyrill Stachniss, Wolfram Burgard; ROS wrapper by Brian Gerkey
autogenerated on Mon Oct 6 2014 07:42:23