slam_gmapping.cpp
Go to the documentation of this file.
1 /*
2  * slam_gmapping
3  * Copyright (c) 2008, Willow Garage, Inc.
4  *
5  * THE WORK (AS DEFINED BELOW) IS PROVIDED UNDER THE TERMS OF THIS CREATIVE
6  * COMMONS PUBLIC LICENSE ("CCPL" OR "LICENSE"). THE WORK IS PROTECTED BY
7  * COPYRIGHT AND/OR OTHER APPLICABLE LAW. ANY USE OF THE WORK OTHER THAN AS
8  * AUTHORIZED UNDER THIS LICENSE OR COPYRIGHT LAW IS PROHIBITED.
9  *
10  * BY EXERCISING ANY RIGHTS TO THE WORK PROVIDED HERE, YOU ACCEPT AND AGREE TO
11  * BE BOUND BY THE TERMS OF THIS LICENSE. THE LICENSOR GRANTS YOU THE RIGHTS
12  * CONTAINED HERE IN CONSIDERATION OF YOUR ACCEPTANCE OF SUCH TERMS AND
13  * CONDITIONS.
14  *
15  */
16 
17 /* Author: Brian Gerkey */
18 /* Modified by: Charles DuHadway */
19 
20 
108 #include "slam_gmapping.h"
109 
110 #include <iostream>
111 
112 #include <time.h>
113 
114 #include "ros/ros.h"
115 #include "ros/console.h"
116 #include "nav_msgs/MapMetaData.h"
117 
120 
121 #include <rosbag/bag.h>
122 #include <rosbag/view.h>
123 #include <boost/foreach.hpp>
124 #define foreach BOOST_FOREACH
125 
126 // compute linear index for given map coords
127 #define MAP_IDX(sx, i, j) ((sx) * (j) + (i))
128 
130  map_to_odom_(tf::Transform(tf::createQuaternionFromRPY( 0, 0, 0 ), tf::Point(0, 0, 0 ))),
131  laser_count_(0), private_nh_("~"), scan_filter_sub_(NULL), scan_filter_(NULL), transform_thread_(NULL)
132 {
133  seed_ = time(NULL);
134  init();
135 }
136 
138  map_to_odom_(tf::Transform(tf::createQuaternionFromRPY( 0, 0, 0 ), tf::Point(0, 0, 0 ))),
140 {
141  seed_ = time(NULL);
142  init();
143 }
144 
145 SlamGMapping::SlamGMapping(long unsigned int seed, long unsigned int max_duration_buffer):
148  seed_(seed), tf_(ros::Duration(max_duration_buffer))
149 {
150  init();
151 }
152 
153 
155 {
156  // log4cxx::Logger::getLogger(ROSCONSOLE_DEFAULT_NAME)->setLevel(ros::console::g_level_lookup[ros::console::levels::Debug]);
157 
158  // The library is pretty chatty
159  //gsp_ = new GMapping::GridSlamProcessor(std::cerr);
161  ROS_ASSERT(gsp_);
162 
164  ROS_ASSERT(tfB_);
165 
166  gsp_laser_ = NULL;
167  gsp_odom_ = NULL;
168 
169  got_first_scan_ = false;
170  got_map_ = false;
171 
172 
173 
174  // Parameters used by our GMapping wrapper
175  if(!private_nh_.getParam("throttle_scans", throttle_scans_))
176  throttle_scans_ = 1;
177  if(!private_nh_.getParam("base_frame", base_frame_))
178  base_frame_ = "base_link";
179  if(!private_nh_.getParam("map_frame", map_frame_))
180  map_frame_ = "map";
181  if(!private_nh_.getParam("odom_frame", odom_frame_))
182  odom_frame_ = "odom";
183 
184  private_nh_.param("transform_publish_period", transform_publish_period_, 0.05);
185 
186  double tmp;
187  if(!private_nh_.getParam("map_update_interval", tmp))
188  tmp = 5.0;
190 
191  // Parameters used by GMapping itself
192  maxUrange_ = 0.0; maxRange_ = 0.0; // preliminary default, will be set in initMapper()
193  if(!private_nh_.getParam("minimumScore", minimum_score_))
194  minimum_score_ = 0;
195  if(!private_nh_.getParam("sigma", sigma_))
196  sigma_ = 0.05;
197  if(!private_nh_.getParam("kernelSize", kernelSize_))
198  kernelSize_ = 1;
199  if(!private_nh_.getParam("lstep", lstep_))
200  lstep_ = 0.05;
201  if(!private_nh_.getParam("astep", astep_))
202  astep_ = 0.05;
203  if(!private_nh_.getParam("iterations", iterations_))
204  iterations_ = 5;
205  if(!private_nh_.getParam("lsigma", lsigma_))
206  lsigma_ = 0.075;
207  if(!private_nh_.getParam("ogain", ogain_))
208  ogain_ = 3.0;
209  if(!private_nh_.getParam("lskip", lskip_))
210  lskip_ = 0;
211  if(!private_nh_.getParam("srr", srr_))
212  srr_ = 0.1;
213  if(!private_nh_.getParam("srt", srt_))
214  srt_ = 0.2;
215  if(!private_nh_.getParam("str", str_))
216  str_ = 0.1;
217  if(!private_nh_.getParam("stt", stt_))
218  stt_ = 0.2;
219  if(!private_nh_.getParam("linearUpdate", linearUpdate_))
220  linearUpdate_ = 1.0;
221  if(!private_nh_.getParam("angularUpdate", angularUpdate_))
222  angularUpdate_ = 0.5;
223  if(!private_nh_.getParam("temporalUpdate", temporalUpdate_))
224  temporalUpdate_ = -1.0;
225  if(!private_nh_.getParam("resampleThreshold", resampleThreshold_))
226  resampleThreshold_ = 0.5;
227  if(!private_nh_.getParam("particles", particles_))
228  particles_ = 30;
229  if(!private_nh_.getParam("xmin", xmin_))
230  xmin_ = -100.0;
231  if(!private_nh_.getParam("ymin", ymin_))
232  ymin_ = -100.0;
233  if(!private_nh_.getParam("xmax", xmax_))
234  xmax_ = 100.0;
235  if(!private_nh_.getParam("ymax", ymax_))
236  ymax_ = 100.0;
237  if(!private_nh_.getParam("delta", delta_))
238  delta_ = 0.05;
239  if(!private_nh_.getParam("occ_thresh", occ_thresh_))
240  occ_thresh_ = 0.25;
241  if(!private_nh_.getParam("llsamplerange", llsamplerange_))
242  llsamplerange_ = 0.01;
243  if(!private_nh_.getParam("llsamplestep", llsamplestep_))
244  llsamplestep_ = 0.01;
245  if(!private_nh_.getParam("lasamplerange", lasamplerange_))
246  lasamplerange_ = 0.005;
247  if(!private_nh_.getParam("lasamplestep", lasamplestep_))
248  lasamplestep_ = 0.005;
249 
250  if(!private_nh_.getParam("tf_delay", tf_delay_))
252 
253 }
254 
255 
257 {
258  entropy_publisher_ = private_nh_.advertise<std_msgs::Float64>("entropy", 1, true);
259  sst_ = node_.advertise<nav_msgs::OccupancyGrid>("map", 1, true);
260  sstm_ = node_.advertise<nav_msgs::MapMetaData>("map_metadata", 1, true);
261  ss_ = node_.advertiseService("dynamic_map", &SlamGMapping::mapCallback, this);
264  scan_filter_->registerCallback(boost::bind(&SlamGMapping::laserCallback, this, _1));
265 
266  transform_thread_ = new boost::thread(boost::bind(&SlamGMapping::publishLoop, this, transform_publish_period_));
267 }
268 
269 void SlamGMapping::startReplay(const std::string & bag_fname, std::string scan_topic)
270 {
271  double transform_publish_period;
273  entropy_publisher_ = private_nh_.advertise<std_msgs::Float64>("entropy", 1, true);
274  sst_ = node_.advertise<nav_msgs::OccupancyGrid>("map", 1, true);
275  sstm_ = node_.advertise<nav_msgs::MapMetaData>("map_metadata", 1, true);
276  ss_ = node_.advertiseService("dynamic_map", &SlamGMapping::mapCallback, this);
277 
278  rosbag::Bag bag;
279  bag.open(bag_fname, rosbag::bagmode::Read);
280 
281  std::vector<std::string> topics;
282  topics.push_back(std::string("/tf"));
283  topics.push_back(scan_topic);
284  rosbag::View viewall(bag, rosbag::TopicQuery(topics));
285 
286  // Store up to 5 messages and there error message (if they cannot be processed right away)
287  std::queue<std::pair<sensor_msgs::LaserScan::ConstPtr, std::string> > s_queue;
288  foreach(rosbag::MessageInstance const m, viewall)
289  {
290  tf::tfMessage::ConstPtr cur_tf = m.instantiate<tf::tfMessage>();
291  if (cur_tf != NULL) {
292  for (size_t i = 0; i < cur_tf->transforms.size(); ++i)
293  {
294  geometry_msgs::TransformStamped transformStamped;
295  tf::StampedTransform stampedTf;
296  transformStamped = cur_tf->transforms[i];
297  tf::transformStampedMsgToTF(transformStamped, stampedTf);
298  tf_.setTransform(stampedTf);
299  }
300  }
301 
302  sensor_msgs::LaserScan::ConstPtr s = m.instantiate<sensor_msgs::LaserScan>();
303  if (s != NULL) {
304  if (!(ros::Time(s->header.stamp)).is_zero())
305  {
306  s_queue.push(std::make_pair(s, ""));
307  }
308  // Just like in live processing, only process the latest 5 scans
309  if (s_queue.size() > 5) {
310  ROS_WARN_STREAM("Dropping old scan: " << s_queue.front().second);
311  s_queue.pop();
312  }
313  // ignoring un-timestamped tf data
314  }
315 
316  // Only process a scan if it has tf data
317  while (!s_queue.empty())
318  {
319  try
320  {
322  tf_.lookupTransform(s_queue.front().first->header.frame_id, odom_frame_, s_queue.front().first->header.stamp, t);
323  this->laserCallback(s_queue.front().first);
324  s_queue.pop();
325  }
326  // If tf does not have the data yet
327  catch(tf2::TransformException& e)
328  {
329  // Store the error to display it if we cannot process the data after some time
330  s_queue.front().second = std::string(e.what());
331  break;
332  }
333  }
334  }
335 
336  bag.close();
337 }
338 
339 void SlamGMapping::publishLoop(double transform_publish_period){
340  if(transform_publish_period == 0)
341  return;
342 
343  ros::Rate r(1.0 / transform_publish_period);
344  while(ros::ok()){
346  r.sleep();
347  }
348 }
349 
351 {
352  if(transform_thread_){
353  transform_thread_->join();
354  delete transform_thread_;
355  }
356 
357  delete gsp_;
358  if(gsp_laser_)
359  delete gsp_laser_;
360  if(gsp_odom_)
361  delete gsp_odom_;
362  if (scan_filter_)
363  delete scan_filter_;
364  if (scan_filter_sub_)
365  delete scan_filter_sub_;
366 }
367 
368 bool
370 {
371  // Get the pose of the centered laser at the right time
372  centered_laser_pose_.stamp_ = t;
373  // Get the laser's pose that is centered
374  tf::Stamped<tf::Transform> odom_pose;
375  try
376  {
378  }
379  catch(tf::TransformException e)
380  {
381  ROS_WARN("Failed to compute odom pose, skipping scan (%s)", e.what());
382  return false;
383  }
384  double yaw = tf::getYaw(odom_pose.getRotation());
385 
386  gmap_pose = GMapping::OrientedPoint(odom_pose.getOrigin().x(),
387  odom_pose.getOrigin().y(),
388  yaw);
389  return true;
390 }
391 
392 bool
393 SlamGMapping::initMapper(const sensor_msgs::LaserScan& scan)
394 {
395  laser_frame_ = scan.header.frame_id;
396  // Get the laser's pose, relative to base.
397  tf::Stamped<tf::Pose> ident;
398  tf::Stamped<tf::Transform> laser_pose;
399  ident.setIdentity();
400  ident.frame_id_ = laser_frame_;
401  ident.stamp_ = scan.header.stamp;
402  try
403  {
404  tf_.transformPose(base_frame_, ident, laser_pose);
405  }
406  catch(tf::TransformException e)
407  {
408  ROS_WARN("Failed to compute laser pose, aborting initialization (%s)",
409  e.what());
410  return false;
411  }
412 
413  // create a point 1m above the laser position and transform it into the laser-frame
414  tf::Vector3 v;
415  v.setValue(0, 0, 1 + laser_pose.getOrigin().z());
416  tf::Stamped<tf::Vector3> up(v, scan.header.stamp,
417  base_frame_);
418  try
419  {
421  ROS_DEBUG("Z-Axis in sensor frame: %.3f", up.z());
422  }
423  catch(tf::TransformException& e)
424  {
425  ROS_WARN("Unable to determine orientation of laser: %s",
426  e.what());
427  return false;
428  }
429 
430  // gmapping doesnt take roll or pitch into account. So check for correct sensor alignment.
431  if (fabs(fabs(up.z()) - 1) > 0.001)
432  {
433  ROS_WARN("Laser has to be mounted planar! Z-coordinate has to be 1 or -1, but gave: %.5f",
434  up.z());
435  return false;
436  }
437 
438  gsp_laser_beam_count_ = scan.ranges.size();
439 
440  double angle_center = (scan.angle_min + scan.angle_max)/2;
441 
442  if (up.z() > 0)
443  {
444  do_reverse_range_ = scan.angle_min > scan.angle_max;
447  ROS_INFO("Laser is mounted upwards.");
448  }
449  else
450  {
451  do_reverse_range_ = scan.angle_min < scan.angle_max;
454  ROS_INFO("Laser is mounted upside down.");
455  }
456 
457  // Compute the angles of the laser from -x to x, basically symmetric and in increasing order
458  laser_angles_.resize(scan.ranges.size());
459  // Make sure angles are started so that they are centered
460  double theta = - std::fabs(scan.angle_min - scan.angle_max)/2;
461  for(unsigned int i=0; i<scan.ranges.size(); ++i)
462  {
463  laser_angles_[i]=theta;
464  theta += std::fabs(scan.angle_increment);
465  }
466 
467  ROS_DEBUG("Laser angles in laser-frame: min: %.3f max: %.3f inc: %.3f", scan.angle_min, scan.angle_max,
468  scan.angle_increment);
469  ROS_DEBUG("Laser angles in top-down centered laser-frame: min: %.3f max: %.3f inc: %.3f", laser_angles_.front(),
470  laser_angles_.back(), std::fabs(scan.angle_increment));
471 
472  GMapping::OrientedPoint gmap_pose(0, 0, 0);
473 
474  // setting maxRange and maxUrange here so we can set a reasonable default
476  if(!private_nh_.getParam("maxRange", maxRange_))
477  maxRange_ = scan.range_max - 0.01;
478  if(!private_nh_.getParam("maxUrange", maxUrange_))
480 
481  // The laser must be called "FLASER".
482  // We pass in the absolute value of the computed angle increment, on the
483  // assumption that GMapping requires a positive angle increment. If the
484  // actual increment is negative, we'll swap the order of ranges before
485  // feeding each scan to GMapping.
486  gsp_laser_ = new GMapping::RangeSensor("FLASER",
488  fabs(scan.angle_increment),
489  gmap_pose,
490  0.0,
491  maxRange_);
493 
494  GMapping::SensorMap smap;
495  smap.insert(make_pair(gsp_laser_->getName(), gsp_laser_));
496  gsp_->setSensorMap(smap);
497 
500 
501 
503  GMapping::OrientedPoint initialPose;
504  if(!getOdomPose(initialPose, scan.header.stamp))
505  {
506  ROS_WARN("Unable to determine inital pose of laser! Starting point will be set to zero.");
507  initialPose = GMapping::OrientedPoint(0.0, 0.0, 0.0);
508  }
509 
512  lsigma_, ogain_, lskip_);
513 
517  gsp_->setgenerateMap(false);
518  gsp_->GridSlamProcessor::init(particles_, xmin_, ymin_, xmax_, ymax_,
519  delta_, initialPose);
520  gsp_->setllsamplerange(llsamplerange_);
521  gsp_->setllsamplestep(llsamplestep_);
525  gsp_->setlasamplerange(lasamplerange_);
526  gsp_->setlasamplestep(lasamplestep_);
527  gsp_->setminimumScore(minimum_score_);
528 
529  // Call the sampling function once to set the seed.
531 
532  ROS_INFO("Initialization complete");
533 
534  return true;
535 }
536 
537 bool
538 SlamGMapping::addScan(const sensor_msgs::LaserScan& scan, GMapping::OrientedPoint& gmap_pose)
539 {
540  if(!getOdomPose(gmap_pose, scan.header.stamp))
541  return false;
542 
543  if(scan.ranges.size() != gsp_laser_beam_count_)
544  return false;
545 
546  // GMapping wants an array of doubles...
547  double* ranges_double = new double[scan.ranges.size()];
548  // If the angle increment is negative, we have to invert the order of the readings.
549  if (do_reverse_range_)
550  {
551  ROS_DEBUG("Inverting scan");
552  int num_ranges = scan.ranges.size();
553  for(int i=0; i < num_ranges; i++)
554  {
555  // Must filter out short readings, because the mapper won't
556  if(scan.ranges[num_ranges - i - 1] < scan.range_min)
557  ranges_double[i] = (double)scan.range_max;
558  else
559  ranges_double[i] = (double)scan.ranges[num_ranges - i - 1];
560  }
561  } else
562  {
563  for(unsigned int i=0; i < scan.ranges.size(); i++)
564  {
565  // Must filter out short readings, because the mapper won't
566  if(scan.ranges[i] < scan.range_min)
567  ranges_double[i] = (double)scan.range_max;
568  else
569  ranges_double[i] = (double)scan.ranges[i];
570  }
571  }
572 
573  GMapping::RangeReading reading(scan.ranges.size(),
574  ranges_double,
575  gsp_laser_,
576  scan.header.stamp.toSec());
577 
578  // ...but it deep copies them in RangeReading constructor, so we don't
579  // need to keep our array around.
580  delete[] ranges_double;
581 
582  reading.setPose(gmap_pose);
583 
584  /*
585  ROS_DEBUG("scanpose (%.3f): %.3f %.3f %.3f\n",
586  scan.header.stamp.toSec(),
587  gmap_pose.x,
588  gmap_pose.y,
589  gmap_pose.theta);
590  */
591  ROS_DEBUG("processing scan");
592 
593  return gsp_->processScan(reading);
594 }
595 
596 void
597 SlamGMapping::laserCallback(const sensor_msgs::LaserScan::ConstPtr& scan)
598 {
599  laser_count_++;
600  if ((laser_count_ % throttle_scans_) != 0)
601  return;
602 
603  static ros::Time last_map_update(0,0);
604 
605  // We can't initialize the mapper until we've got the first scan
606  if(!got_first_scan_)
607  {
608  if(!initMapper(*scan))
609  return;
610  got_first_scan_ = true;
611  }
612 
613  GMapping::OrientedPoint odom_pose;
614 
615  if(addScan(*scan, odom_pose))
616  {
617  ROS_DEBUG("scan processed");
618 
620  ROS_DEBUG("new best pose: %.3f %.3f %.3f", mpose.x, mpose.y, mpose.theta);
621  ROS_DEBUG("odom pose: %.3f %.3f %.3f", odom_pose.x, odom_pose.y, odom_pose.theta);
622  ROS_DEBUG("correction: %.3f %.3f %.3f", mpose.x - odom_pose.x, mpose.y - odom_pose.y, mpose.theta - odom_pose.theta);
623 
624  tf::Transform laser_to_map = tf::Transform(tf::createQuaternionFromRPY(0, 0, mpose.theta), tf::Vector3(mpose.x, mpose.y, 0.0)).inverse();
625  tf::Transform odom_to_laser = tf::Transform(tf::createQuaternionFromRPY(0, 0, odom_pose.theta), tf::Vector3(odom_pose.x, odom_pose.y, 0.0));
626 
627  map_to_odom_mutex_.lock();
628  map_to_odom_ = (odom_to_laser * laser_to_map).inverse();
629  map_to_odom_mutex_.unlock();
630 
631  if(!got_map_ || (scan->header.stamp - last_map_update) > map_update_interval_)
632  {
633  updateMap(*scan);
634  last_map_update = scan->header.stamp;
635  ROS_DEBUG("Updated the map");
636  }
637  } else
638  ROS_DEBUG("cannot process scan");
639 }
640 
641 double
643 {
644  double weight_total=0.0;
645  for(std::vector<GMapping::GridSlamProcessor::Particle>::const_iterator it = gsp_->getParticles().begin();
646  it != gsp_->getParticles().end();
647  ++it)
648  {
649  weight_total += it->weight;
650  }
651  double entropy = 0.0;
652  for(std::vector<GMapping::GridSlamProcessor::Particle>::const_iterator it = gsp_->getParticles().begin();
653  it != gsp_->getParticles().end();
654  ++it)
655  {
656  if(it->weight/weight_total > 0.0)
657  entropy += it->weight/weight_total * log(it->weight/weight_total);
658  }
659  return -entropy;
660 }
661 
662 void
663 SlamGMapping::updateMap(const sensor_msgs::LaserScan& scan)
664 {
665  ROS_DEBUG("Update map");
666  boost::mutex::scoped_lock map_lock (map_mutex_);
667  GMapping::ScanMatcher matcher;
668 
669  matcher.setLaserParameters(scan.ranges.size(), &(laser_angles_[0]),
670  gsp_laser_->getPose());
671 
672  matcher.setlaserMaxRange(maxRange_);
673  matcher.setusableRange(maxUrange_);
674  matcher.setgenerateMap(true);
675 
678  std_msgs::Float64 entropy;
679  entropy.data = computePoseEntropy();
680  if(entropy.data > 0.0)
681  entropy_publisher_.publish(entropy);
682 
683  if(!got_map_) {
684  map_.map.info.resolution = delta_;
685  map_.map.info.origin.position.x = 0.0;
686  map_.map.info.origin.position.y = 0.0;
687  map_.map.info.origin.position.z = 0.0;
688  map_.map.info.origin.orientation.x = 0.0;
689  map_.map.info.origin.orientation.y = 0.0;
690  map_.map.info.origin.orientation.z = 0.0;
691  map_.map.info.origin.orientation.w = 1.0;
692  }
693 
694  GMapping::Point center;
695  center.x=(xmin_ + xmax_) / 2.0;
696  center.y=(ymin_ + ymax_) / 2.0;
697 
699  delta_);
700 
701  ROS_DEBUG("Trajectory tree:");
703  n;
704  n = n->parent)
705  {
706  ROS_DEBUG(" %.3f %.3f %.3f",
707  n->pose.x,
708  n->pose.y,
709  n->pose.theta);
710  if(!n->reading)
711  {
712  ROS_DEBUG("Reading is NULL");
713  continue;
714  }
715  matcher.invalidateActiveArea();
716  matcher.computeActiveArea(smap, n->pose, &((*n->reading)[0]));
717  matcher.registerScan(smap, n->pose, &((*n->reading)[0]));
718  }
719 
720  // the map may have expanded, so resize ros message as well
721  if(map_.map.info.width != (unsigned int) smap.getMapSizeX() || map_.map.info.height != (unsigned int) smap.getMapSizeY()) {
722 
723  // NOTE: The results of ScanMatcherMap::getSize() are different from the parameters given to the constructor
724  // so we must obtain the bounding box in a different way
725  GMapping::Point wmin = smap.map2world(GMapping::IntPoint(0, 0));
727  xmin_ = wmin.x; ymin_ = wmin.y;
728  xmax_ = wmax.x; ymax_ = wmax.y;
729 
730  ROS_DEBUG("map size is now %dx%d pixels (%f,%f)-(%f, %f)", smap.getMapSizeX(), smap.getMapSizeY(),
731  xmin_, ymin_, xmax_, ymax_);
732 
733  map_.map.info.width = smap.getMapSizeX();
734  map_.map.info.height = smap.getMapSizeY();
735  map_.map.info.origin.position.x = xmin_;
736  map_.map.info.origin.position.y = ymin_;
737  map_.map.data.resize(map_.map.info.width * map_.map.info.height);
738 
739  ROS_DEBUG("map origin: (%f, %f)", map_.map.info.origin.position.x, map_.map.info.origin.position.y);
740  }
741 
742  for(int x=0; x < smap.getMapSizeX(); x++)
743  {
744  for(int y=0; y < smap.getMapSizeY(); y++)
745  {
747  GMapping::IntPoint p(x, y);
748  double occ=smap.cell(p);
749  assert(occ <= 1.0);
750  if(occ < 0)
751  map_.map.data[MAP_IDX(map_.map.info.width, x, y)] = -1;
752  else if(occ > occ_thresh_)
753  {
754  //map_.map.data[MAP_IDX(map_.map.info.width, x, y)] = (int)round(occ*100.0);
755  map_.map.data[MAP_IDX(map_.map.info.width, x, y)] = 100;
756  }
757  else
758  map_.map.data[MAP_IDX(map_.map.info.width, x, y)] = 0;
759  }
760  }
761  got_map_ = true;
762 
763  //make sure to set the header information on the map
764  map_.map.header.stamp = ros::Time::now();
765  map_.map.header.frame_id = tf_.resolve( map_frame_ );
766 
767  sst_.publish(map_.map);
768  sstm_.publish(map_.map.info);
769 }
770 
771 bool
772 SlamGMapping::mapCallback(nav_msgs::GetMap::Request &req,
773  nav_msgs::GetMap::Response &res)
774 {
775  boost::mutex::scoped_lock map_lock (map_mutex_);
776  if(got_map_ && map_.map.info.width && map_.map.info.height)
777  {
778  res = map_;
779  return true;
780  }
781  else
782  return false;
783 }
784 
786 {
787  map_to_odom_mutex_.lock();
788  ros::Time tf_expiration = ros::Time::now() + ros::Duration(tf_delay_);
790  map_to_odom_mutex_.unlock();
791 }
bool mapCallback(nav_msgs::GetMap::Request &req, nav_msgs::GetMap::Response &res)
void laserCallback(const sensor_msgs::LaserScan::ConstPtr &scan)
static tf::Quaternion createQuaternionFromRPY(double roll, double pitch, double yaw)
double registerScan(ScanMatcherMap &map, const OrientedPoint &p, const double *readings)
message_filters::Subscriber< sensor_msgs::LaserScan > * scan_filter_sub_
Definition: slam_gmapping.h:58
Cell & cell(int x, int y)
double linearUpdate_
const ParticleVector & getParticles() const
bool initMapper(const sensor_msgs::LaserScan &scan)
void updateMap(const sensor_msgs::LaserScan &scan)
void publish(const boost::shared_ptr< M > &message) const
tf::TransformListener tf_
Definition: slam_gmapping.h:57
double lasamplerange_
void open(std::string const &filename, uint32_t mode=bagmode::Read)
ros::ServiceServer ss_
Definition: slam_gmapping.h:56
double angularUpdate_
std::string odom_frame_
Definition: slam_gmapping.h:93
double computePoseEntropy()
void setMatchingParameters(double urange, double range, double sigma, int kernsize, double lopt, double aopt, int iterations, double likelihoodSigma=1, double likelihoodGain=1, unsigned int likelihoodSkip=0)
std::string laser_frame_
Definition: slam_gmapping.h:91
static double getYaw(const Quaternion &bt_q)
double sampleGaussian(double sigma, unsigned int S=0)
double occ_thresh_
XmlRpcServer s
double lasamplestep_
ros::Publisher sst_
Definition: slam_gmapping.h:54
bool processScan(const RangeReading &reading, int adaptParticles=0)
double llsamplestep_
bool setTransform(const StampedTransform &transform, const std::string &authority="default_authority")
void transformPoint(const std::string &target_frame, const geometry_msgs::PointStamped &stamped_in, geometry_msgs::PointStamped &stamped_out) const
ServiceServer advertiseService(const std::string &service, bool(T::*srv_func)(MReq &, MRes &), T *obj)
std::string resolve(const std::string &frame_name)
TFSIMD_FORCE_INLINE const tfScalar & y() const
unsigned long int seed_
#define ROS_WARN(...)
void publishLoop(double transform_publish_period)
TFSIMD_FORCE_INLINE void setValue(const tfScalar &x, const tfScalar &y, const tfScalar &z)
void close()
bool got_first_scan_
Definition: slam_gmapping.h:75
bool do_reverse_range_
Definition: slam_gmapping.h:71
bool getOdomPose(GMapping::OrientedPoint &gmap_pose, const ros::Time &t)
tf::Stamped< tf::Pose > centered_laser_pose_
Definition: slam_gmapping.h:68
unsigned int gsp_laser_beam_count_
Definition: slam_gmapping.h:72
double tf_delay_
void setLaserParameters(unsigned int beams, double *angles, const OrientedPoint &lpose)
tf::TransformBroadcaster * tfB_
Definition: slam_gmapping.h:60
void startReplay(const std::string &bag_fname, std::string scan_topic)
boost::mutex map_mutex_
Definition: slam_gmapping.h:83
tf::Transform map_to_odom_
Definition: slam_gmapping.h:81
void setUpdateDistances(double linear, double angular, double resampleThreshold)
void startLiveSlam()
void setSensorMap(const SensorMap &smap)
Duration & fromSec(double t)
nav_msgs::GetMap::Response map_
Definition: slam_gmapping.h:78
#define ROS_INFO(...)
double llsamplerange_
bool param(const std::string &param_name, T &param_val, const T &default_val) const
TFSIMD_FORCE_INLINE Quaternion inverse(const Quaternion &q)
boost::shared_ptr< T > instantiate() const
ROSCPP_DECL bool ok()
double maxUrange_
double temporalUpdate_
void sendTransform(const StampedTransform &transform)
boost::mutex map_to_odom_mutex_
Definition: slam_gmapping.h:82
boost::thread * transform_thread_
Definition: slam_gmapping.h:88
ros::Publisher entropy_publisher_
Definition: slam_gmapping.h:53
TFSIMD_FORCE_INLINE const tfScalar & x() const
Transform inverse() const
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
int getMapSizeX() const
#define ROS_WARN_STREAM(args)
void lookupTransform(const std::string &target_frame, const std::string &source_frame, const ros::Time &time, StampedTransform &transform) const
std::map< std::string, Sensor * > SensorMap
bool sleep()
ros::Duration map_update_interval_
Definition: slam_gmapping.h:80
ros::Time stamp_
double transform_publish_period_
#define MAP_IDX(sx, i, j)
ros::NodeHandle private_nh_
std::string frame_id_
void setPose(const OrientedPoint &pose)
void transformPose(const std::string &target_frame, const geometry_msgs::PoseStamped &stamped_in, geometry_msgs::PoseStamped &stamped_out) const
GMapping::GridSlamProcessor * gsp_
Definition: slam_gmapping.h:62
int getMapSizeY() const
bool getParam(const std::string &key, std::string &s) const
std::string getName() const
double resampleThreshold_
static Time now()
ros::Publisher sstm_
Definition: slam_gmapping.h:55
void publishTransform()
std::string base_frame_
Definition: slam_gmapping.h:90
OrientedPoint getPose() const
void setUpdatePeriod(double p)
std::string map_frame_
Definition: slam_gmapping.h:92
#define ROS_ASSERT(cond)
#define n
void setMotionModelParameters(double srr, double srt, double str, double stt)
void computeActiveArea(ScanMatcherMap &map, const OrientedPoint &p, const double *readings)
double maxRange_
static void transformStampedMsgToTF(const geometry_msgs::TransformStamped &msg, StampedTransform &bt)
int getBestParticleIndex() const
Point map2world(const IntPoint &p) const
std::vector< double > laser_angles_
Definition: slam_gmapping.h:66
ros::NodeHandle node_
Definition: slam_gmapping.h:52
double minimum_score_
tf::MessageFilter< sensor_msgs::LaserScan > * scan_filter_
Definition: slam_gmapping.h:59
GMapping::OdometrySensor * gsp_odom_
Definition: slam_gmapping.h:73
orientedpoint< double, double > OrientedPoint
GMapping::RangeSensor * gsp_laser_
Definition: slam_gmapping.h:63
bool addScan(const sensor_msgs::LaserScan &scan, GMapping::OrientedPoint &gmap_pose)
Connection registerCallback(const C &callback)
#define ROS_DEBUG(...)


gmapping
Author(s): Brian Gerkey
autogenerated on Mon Jun 10 2019 15:08:12