slam_toolbox_common.cpp
Go to the documentation of this file.
1 /*
2  * slam_toolbox
3  * Copyright Work Modifications (c) 2018, Simbe Robotics, Inc.
4  * Copyright Work Modifications (c) 2019, Samsung Research America
5  *
6  * THE WORK (AS DEFINED BELOW) IS PROVIDED UNDER THE TERMS OF THIS CREATIVE
7  * COMMONS PUBLIC LICENSE ("CCPL" OR "LICENSE"). THE WORK IS PROTECTED BY
8  * COPYRIGHT AND/OR OTHER APPLICABLE LAW. ANY USE OF THE WORK OTHER THAN AS
9  * AUTHORIZED UNDER THIS LICENSE OR COPYRIGHT LAW IS PROHIBITED.
10  *
11  * BY EXERCISING ANY RIGHTS TO THE WORK PROVIDED HERE, YOU ACCEPT AND AGREE TO
12  * BE BOUND BY THE TERMS OF THIS LICENSE. THE LICENSOR GRANTS YOU THE RIGHTS
13  * CONTAINED HERE IN CONSIDERATION OF YOUR ACCEPTANCE OF SUCH TERMS AND
14  * CONDITIONS.
15  *
16  */
17 
18 /* Orginal Author for slam_karto the original work was based on: Brian Gerkey */
19 /* Author: Steven Macenski */
20 
23 
24 namespace slam_toolbox
25 {
26 
27 /*****************************************************************************/
29 : solver_loader_("slam_toolbox", "karto::ScanSolver"),
30  processor_type_(PROCESS),
31  first_measurement_(true),
32  nh_(nh),
33  process_near_pose_(nullptr)
34 /*****************************************************************************/
35 {
36  smapper_ = std::make_unique<mapper_utils::SMapper>();
37  dataset_ = std::make_unique<karto::Dataset>();
38 
39  setParams(nh_);
41  setSolver(nh_);
42 
43  laser_assistant_ = std::make_unique<laser_utils::LaserAssistant>(
44  nh_, tf_.get(), base_frame_);
45  pose_helper_ = std::make_unique<pose_utils::GetPoseHelper>(
46  tf_.get(), base_frame_, odom_frame_);
47  scan_holder_ = std::make_unique<laser_utils::ScanHolder>(lasers_);
48  map_saver_ = std::make_unique<map_saver::MapSaver>(nh_, map_name_);
50  std::make_unique<loop_closure_assistant::LoopClosureAssistant>(
51  nh_, smapper_->getMapper(), scan_holder_.get(), state_, processor_type_);
52 
54 
55  double transform_publish_period;
56  nh_.param("transform_publish_period", transform_publish_period, 0.05);
57  threads_.push_back(std::make_unique<boost::thread>(
59  this, transform_publish_period)));
60  threads_.push_back(std::make_unique<boost::thread>(
61  boost::bind(&SlamToolbox::publishVisualizations, this)));
62 }
63 
64 /*****************************************************************************/
66 /*****************************************************************************/
67 {
68  for (int i=0; i != threads_.size(); i++)
69  {
70  threads_[i]->join();
71  }
72 
73  smapper_.reset();
74  dataset_.reset();
75  closure_assistant_.reset();
76  map_saver_.reset();
77  pose_helper_.reset();
78  laser_assistant_.reset();
79  scan_holder_.reset();
80 }
81 
82 /*****************************************************************************/
84 /*****************************************************************************/
85 {
86  // Set solver to be used in loop closure
87  std::string solver_plugin;
88  if(!private_nh_.getParam("solver_plugin", solver_plugin))
89  {
90  ROS_WARN("unable to find requested solver plugin, defaulting to SPA");
91  solver_plugin = "solver_plugins::CeresSolver";
92  }
93  try
94  {
95  solver_ = solver_loader_.createInstance(solver_plugin);
96  ROS_INFO("Using plugin %s", solver_plugin.c_str());
97  }
98  catch (const pluginlib::PluginlibException& ex)
99  {
100  ROS_FATAL("Failed to create %s, is it registered and built? Exception: %s.",
101  solver_plugin.c_str(), ex.what());
102  exit(1);
103  }
104  smapper_->getMapper()->SetScanSolver(solver_.get());
105 }
106 
107 /*****************************************************************************/
109 /*****************************************************************************/
110 {
112  private_nh.param("odom_frame", odom_frame_, std::string("odom"));
113  private_nh.param("map_frame", map_frame_, std::string("map"));
114  private_nh.param("base_frame", base_frame_, std::string("base_footprint"));
115  private_nh.param("resolution", resolution_, 0.05);
116  private_nh.param("map_name", map_name_, std::string("/map"));
117  private_nh.param("scan_topic", scan_topic_, std::string("/scan"));
118  private_nh.param("throttle_scans", throttle_scans_, 1);
119  private_nh.param("enable_interactive_mode", enable_interactive_mode_, false);
120 
121  double tmp_val;
122  private_nh.param("transform_timeout", tmp_val, 0.2);
124  private_nh.param("tf_buffer_duration", tmp_val, 30.);
125  tf_buffer_dur_ = ros::Duration(tmp_val);
126  private_nh.param("minimum_time_interval", tmp_val, 0.5);
128 
129  private_nh.param("position_covariance_scale", position_covariance_scale_, 1.0);
130  private_nh.param("yaw_covariance_scale", yaw_covariance_scale_, 1.0);
131 
132  bool debug = false;
133  if (private_nh.getParam("debug_logging", debug) && debug)
134  {
137  {
139  }
140  }
141 
142  smapper_->configure(private_nh);
143  private_nh.setParam("paused_new_measurements", false);
144 }
145 
146 /*****************************************************************************/
148 /*****************************************************************************/
149 {
150  tf_ = std::make_unique<tf2_ros::Buffer>(ros::Duration(tf_buffer_dur_));
151  tfL_ = std::make_unique<tf2_ros::TransformListener>(*tf_);
152  tfB_ = std::make_unique<tf2_ros::TransformBroadcaster>();
153  sst_ = node.advertise<nav_msgs::OccupancyGrid>(map_name_, 1, true);
154  sstm_ = node.advertise<nav_msgs::MapMetaData>(map_name_ + "_metadata", 1, true);
155  ssMap_ = node.advertiseService("dynamic_map", &SlamToolbox::mapCallback, this);
159  scan_filter_sub_ = std::make_unique<message_filters::Subscriber<sensor_msgs::LaserScan> >(node, scan_topic_, 5);
160  scan_filter_ = std::make_unique<tf2_ros::MessageFilter<sensor_msgs::LaserScan> >(*scan_filter_sub_, *tf_, odom_frame_, 5, node);
161  scan_filter_->registerCallback(boost::bind(&SlamToolbox::laserCallback, this, _1));
162  pose_pub_ = node.advertise<geometry_msgs::PoseWithCovarianceStamped>("pose", 10, true);
163 }
164 
165 /*****************************************************************************/
166 void SlamToolbox::publishTransformLoop(const double& transform_publish_period)
167 /*****************************************************************************/
168 {
169  if(transform_publish_period == 0)
170  {
171  return;
172  }
173 
174  ros::Rate r(1.0 / transform_publish_period);
175  while(ros::ok())
176  {
177  {
178  boost::mutex::scoped_lock lock(map_to_odom_mutex_);
179  geometry_msgs::TransformStamped msg;
180  tf2::convert(map_to_odom_, msg.transform);
181  msg.child_frame_id = odom_frame_;
182  msg.header.frame_id = map_frame_;
183  msg.header.stamp = ros::Time::now() + transform_timeout_;
184  tfB_->sendTransform(msg);
185  }
186  r.sleep();
187  }
188 }
189 
190 /*****************************************************************************/
192 /*****************************************************************************/
193 {
194  nav_msgs::OccupancyGrid& og = map_.map;
195  og.info.resolution = resolution_;
196  og.info.origin.position.x = 0.0;
197  og.info.origin.position.y = 0.0;
198  og.info.origin.position.z = 0.0;
199  og.info.origin.orientation.x = 0.0;
200  og.info.origin.orientation.y = 0.0;
201  og.info.origin.orientation.z = 0.0;
202  og.info.origin.orientation.w = 1.0;
203  og.header.frame_id = map_frame_;
204 
205  double map_update_interval;
206  if(!nh_.getParam("map_update_interval", map_update_interval))
207  map_update_interval = 10.0;
208  ros::Rate r(1.0 / map_update_interval);
209 
210  while(ros::ok())
211  {
212  updateMap();
214  {
215  boost::mutex::scoped_lock lock(smapper_mutex_);
216  closure_assistant_->publishGraph();
217  }
218  r.sleep();
219  }
220 }
221 
222 /*****************************************************************************/
224 /*****************************************************************************/
225 {
226  std::string filename;
227  geometry_msgs::Pose2D pose;
228  bool dock = false;
229  if (shouldStartWithPoseGraph(filename, pose, dock))
230  {
231  slam_toolbox_msgs::DeserializePoseGraph::Request req;
232  slam_toolbox_msgs::DeserializePoseGraph::Response resp;
233  req.initial_pose = pose;
234  req.filename = filename;
235  if (dock)
236  {
237  req.match_type =
238  slam_toolbox_msgs::DeserializePoseGraph::Request::START_AT_FIRST_NODE;
239  }
240  else
241  {
242  req.match_type =
243  slam_toolbox_msgs::DeserializePoseGraph::Request::START_AT_GIVEN_POSE;
244  }
245  deserializePoseGraphCallback(req, resp);
246  }
247 }
248 
249 /*****************************************************************************/
251  geometry_msgs::Pose2D& pose, bool& start_at_dock)
252 /*****************************************************************************/
253  {
254  // if given a map to load at run time, do it.
255  if (nh_.getParam("map_file_name", filename))
256  {
257  std::vector<double> read_pose;
258  if (nh_.getParam("map_start_pose", read_pose))
259  {
260  start_at_dock = false;
261  if (read_pose.size() != 3)
262  {
263  ROS_ERROR("LocalizationSlamToolbox: Incorrect number of "
264  "arguments for map starting pose. Must be in format: "
265  "[x, y, theta]. Starting at the origin");
266  pose.x = 0.;
267  pose.y = 0.;
268  pose.theta = 0.;
269  }
270  else
271  {
272  pose.x = read_pose[0];
273  pose.y = read_pose[1];
274  pose.theta = read_pose[2];
275  }
276  }
277  else
278  {
279  nh_.getParam("map_start_at_dock", start_at_dock);
280  }
281 
282  return true;
283  }
284 
285  return false;
286 }
287 
288 /*****************************************************************************/
290  sensor_msgs::LaserScan::ConstPtr& scan)
291 /*****************************************************************************/
292 {
293  const std::string& frame = scan->header.frame_id;
294  if(lasers_.find(frame) == lasers_.end())
295  {
296  try
297  {
298  lasers_[frame] = laser_assistant_->toLaserMetadata(*scan);
299  dataset_->Add(lasers_[frame].getLaser(), true);
300  }
301  catch (tf2::TransformException& e)
302  {
303  ROS_ERROR("Failed to compute laser pose, aborting initialization (%s)",
304  e.what());
305  return nullptr;
306  }
307  }
308 
309  return lasers_[frame].getLaser();
310 }
311 
312 /*****************************************************************************/
314 /*****************************************************************************/
315 {
316  if (sst_.getNumSubscribers() == 0)
317  {
318  return true;
319  }
320  boost::mutex::scoped_lock lock(smapper_mutex_);
321  karto::OccupancyGrid* occ_grid = smapper_->getOccupancyGrid(resolution_);
322  if(!occ_grid)
323  {
324  return false;
325  }
326 
327  vis_utils::toNavMap(occ_grid, map_.map);
328 
329  // publish map as current
330  map_.map.header.stamp = ros::Time::now();
331  sst_.publish(map_.map);
332  sstm_.publish(map_.map.info);
333 
334  delete occ_grid;
335  occ_grid = nullptr;
336  return true;
337 }
338 
339 /*****************************************************************************/
341  const karto::Pose2& corrected_pose,
342  const karto::Pose2& karto_pose,
343  const ros::Time& t,
344  const bool& update_reprocessing_transform)
345 /*****************************************************************************/
346 {
347  // Compute the map->odom transform
348  tf2::Stamped<tf2::Transform> odom_to_map;
349  tf2::Quaternion q(0.,0.,0.,1.0);
350  q.setRPY(0., 0., corrected_pose.GetHeading());
351  tf2::Stamped<tf2::Transform> base_to_map(
352  tf2::Transform(q, tf2::Vector3(corrected_pose.GetX(),
353  corrected_pose.GetY(), 0.0)).inverse(), t, base_frame_);
354  try
355  {
356  geometry_msgs::TransformStamped base_to_map_msg, odom_to_map_msg;
357  tf2::convert(base_to_map, base_to_map_msg);
358  odom_to_map_msg = tf_->transform(base_to_map_msg, odom_frame_);
359  tf2::convert(odom_to_map_msg, odom_to_map);
360  }
361  catch(tf2::TransformException& e)
362  {
363  ROS_ERROR("Transform from base_link to odom failed: %s", e.what());
364  return odom_to_map;
365  }
366 
367  // if we're continuing a previous session, we need to
368  // estimate the homogenous transformation between the old and new
369  // odometry frames and transform the new session
370  // into the older session's frame
371  if (update_reprocessing_transform)
372  {
373  tf2::Transform odom_to_base_serialized = base_to_map.inverse();
374  tf2::Quaternion q1(0.,0.,0.,1.0);
375  q1.setRPY(0., 0., tf2::getYaw(odom_to_base_serialized.getRotation()));
376  odom_to_base_serialized.setRotation(q1);
377  tf2::Transform odom_to_base_current = smapper_->toTfPose(karto_pose);
379  odom_to_base_serialized * odom_to_base_current.inverse();
380  }
381 
382  // set map to odom for our transformation thread to publish
383  boost::mutex::scoped_lock lock(map_to_odom_mutex_);
384  map_to_odom_ = tf2::Transform(tf2::Quaternion( odom_to_map.getRotation() ),
385  tf2::Vector3( odom_to_map.getOrigin() ) ).inverse();
386 
387  return odom_to_map;
388 }
389 
390 /*****************************************************************************/
393  const sensor_msgs::LaserScan::ConstPtr& scan,
394  karto::Pose2& karto_pose)
395 /*****************************************************************************/
396 {
397  // Create a vector of doubles for karto
398  std::vector<kt_double> readings = laser_utils::scanToReadings(
399  *scan, lasers_[scan->header.frame_id].isInverted());
400 
401  // transform by the reprocessing transform
402  tf2::Transform pose_original = smapper_->toTfPose(karto_pose);
403  tf2::Transform tf_pose_transformed = reprocessing_transform_ * pose_original;
404  karto::Pose2 transformed_pose = smapper_->toKartoPose(tf_pose_transformed);
405 
406  // create localized range scan
408  laser->GetName(), readings);
409  range_scan->SetOdometricPose(transformed_pose);
410  range_scan->SetCorrectedPose(transformed_pose);
411  return range_scan;
412 }
413 
414 /*****************************************************************************/
416  const sensor_msgs::LaserScan::ConstPtr& scan,
417  const karto::Pose2& pose)
418 /*****************************************************************************/
419 {
420  static karto::Pose2 last_pose;
421  static ros::Time last_scan_time = ros::Time(0.);
422  static double min_dist2 =
423  smapper_->getMapper()->getParamMinimumTravelDistance() *
424  smapper_->getMapper()->getParamMinimumTravelDistance();
425 
426  // we give it a pass on the first measurement to get the ball rolling
427  if (first_measurement_)
428  {
429  last_scan_time = scan->header.stamp;
430  last_pose = pose;
431  first_measurement_ = false;
432  return true;
433  }
434 
435  // we are in a paused mode, reject incomming information
437  {
438  return false;
439  }
440 
441  // throttled out
442  if ((scan->header.seq % throttle_scans_) != 0)
443  {
444  return false;
445  }
446 
447  // not enough time
448  if (scan->header.stamp - last_scan_time < minimum_time_interval_)
449  {
450  return false;
451  }
452 
453  // check moved enough, within 10% for correction error
454  const double dist2 = last_pose.SquaredDistance(pose);
455  if(dist2 < 0.8 * min_dist2 || scan->header.seq < 5)
456  {
457  return false;
458  }
459 
460  last_pose = pose;
461  last_scan_time = scan->header.stamp;
462 
463  return true;
464 }
465 
466 /*****************************************************************************/
469  PosedScan& scan_w_pose)
470 /*****************************************************************************/
471 {
472  return addScan(laser, scan_w_pose.scan, scan_w_pose.pose);
473 }
474 
475 /*****************************************************************************/
478  const sensor_msgs::LaserScan::ConstPtr& scan,
479  karto::Pose2& karto_pose)
480 /*****************************************************************************/
481 {
482  // get our localized range scan
484  laser, scan, karto_pose);
485 
486  // Add the localized range scan to the smapper
487  boost::mutex::scoped_lock lock(smapper_mutex_);
488  bool processed = false, update_reprocessing_transform = false;
489 
490  karto::Matrix3 covariance;
491  covariance.SetToIdentity();
492 
493  if (processor_type_ == PROCESS)
494  {
495  processed = smapper_->getMapper()->Process(range_scan, &covariance);
496  }
498  {
499  processed = smapper_->getMapper()->ProcessAtDock(range_scan, &covariance);
501  update_reprocessing_transform = true;
502  }
504  {
505  boost::mutex::scoped_lock l(pose_mutex_);
506  if (!process_near_pose_)
507  {
508  ROS_ERROR("Process near region called without a "
509  "valid region request. Ignoring scan.");
510  return nullptr;
511  }
512  range_scan->SetOdometricPose(*process_near_pose_);
513  range_scan->SetCorrectedPose(range_scan->GetOdometricPose());
514  process_near_pose_.reset(nullptr);
515  processed = smapper_->getMapper()->ProcessAgainstNodesNearBy(range_scan, false, &covariance);
516  update_reprocessing_transform = true;
518  }
519  else
520  {
521  ROS_FATAL("SlamToolbox: No valid processor type set! Exiting.");
522  exit(-1);
523  }
524 
525  // if successfully processed, create odom to map transformation
526  // and add our scan to storage
527  if(processed)
528  {
530  {
531  scan_holder_->addScan(*scan);
532  }
533 
534  setTransformFromPoses(range_scan->GetCorrectedPose(), karto_pose,
535  scan->header.stamp, update_reprocessing_transform);
536  dataset_->Add(range_scan);
537  publishPose(range_scan->GetCorrectedPose(), covariance, scan->header.stamp);
538  }
539  else
540  {
541  delete range_scan;
542  range_scan = nullptr;
543  }
544 
545  return range_scan;
546 }
547 
548 /*****************************************************************************/
550  const karto::Pose2 & pose,
551  const karto::Matrix3 & cov,
552  const ros::Time & t)
553 /*****************************************************************************/
554 {
555  geometry_msgs::PoseWithCovarianceStamped pose_msg;
556  pose_msg.header.stamp = t;
557  pose_msg.header.frame_id = map_frame_;
558 
559  tf2::Quaternion q(0., 0., 0., 1.0);
560  q.setRPY(0., 0., pose.GetHeading());
561  tf2::Transform transform(q, tf2::Vector3(pose.GetX(), pose.GetY(), 0.0));
562  tf2::toMsg(transform, pose_msg.pose.pose);
563 
564  pose_msg.pose.covariance[0] = cov(0, 0) * position_covariance_scale_; // x
565  pose_msg.pose.covariance[1] = cov(0, 1) * position_covariance_scale_; // xy
566  pose_msg.pose.covariance[6] = cov(1, 0) * position_covariance_scale_; // xy
567  pose_msg.pose.covariance[7] = cov(1, 1) * position_covariance_scale_; // y
568  pose_msg.pose.covariance[35] = cov(2, 2) * yaw_covariance_scale_; // yaw
569 
570  pose_pub_.publish(pose_msg);
571 }
572 
573 /*****************************************************************************/
575  nav_msgs::GetMap::Request &req,
576  nav_msgs::GetMap::Response &res)
577 /*****************************************************************************/
578 {
579  if(map_.map.info.width && map_.map.info.height)
580  {
581  boost::mutex::scoped_lock lock(smapper_mutex_);
582  res = map_;
583  return true;
584  }
585  else
586  {
587  return false;
588  }
589 }
590 
591 /*****************************************************************************/
593  slam_toolbox_msgs::Pause::Request& req,
594  slam_toolbox_msgs::Pause::Response& resp)
595 /*****************************************************************************/
596 {
597  bool curr_state = isPaused(NEW_MEASUREMENTS);
598  state_.set(NEW_MEASUREMENTS, !curr_state);
599 
600  nh_.setParam("paused_new_measurements", !curr_state);
601  ROS_INFO("SlamToolbox: Toggled to %s",
602  !curr_state ? "pause taking new measurements." :
603  "actively taking new measurements.");
604  resp.status = true;
605  return true;
606 }
607 
608 /*****************************************************************************/
610 /*****************************************************************************/
611 {
612  return state_.get(app);
613 }
614 
615 /*****************************************************************************/
617  slam_toolbox_msgs::SerializePoseGraph::Request &req,
618  slam_toolbox_msgs::SerializePoseGraph::Response &resp)
619 /*****************************************************************************/
620 {
621  std::string filename = req.filename;
622 
623  // if we're inside the snap, we need to write to commonly accessible space
624  if (snap_utils::isInSnap())
625  {
626  filename = snap_utils::getSnapPath() + std::string("/") + filename;
627  }
628 
629  boost::mutex::scoped_lock lock(smapper_mutex_);
631  return true;
632 }
633 
634 /*****************************************************************************/
636  std::unique_ptr<karto::Mapper>& mapper,
637  std::unique_ptr<karto::Dataset>& dataset)
638 /*****************************************************************************/
639 {
640  boost::mutex::scoped_lock lock(smapper_mutex_);
641 
642  solver_->Reset();
643 
644  // add the nodes and constraints to the optimizer
645  VerticeMap mapper_vertices = mapper->GetGraph()->GetVertices();
646  VerticeMap::iterator vertex_map_it = mapper_vertices.begin();
647  for(vertex_map_it; vertex_map_it != mapper_vertices.end(); ++vertex_map_it)
648  {
649  ScanMap::iterator vertex_it = vertex_map_it->second.begin();
650  for(vertex_it; vertex_it != vertex_map_it->second.end(); ++vertex_it)
651  {
652  if (vertex_it->second != nullptr)
653  {
654  solver_->AddNode(vertex_it->second);
655  }
656  }
657  }
658 
659  EdgeVector mapper_edges = mapper->GetGraph()->GetEdges();
660  EdgeVector::iterator edges_it = mapper_edges.begin();
661  for( edges_it; edges_it != mapper_edges.end(); ++edges_it)
662  {
663  if (*edges_it != nullptr)
664  {
665  solver_->AddConstraint(*edges_it);
666  }
667  }
668 
669  mapper->SetScanSolver(solver_.get());
670 
671  // move the memory to our working dataset
672  smapper_->setMapper(mapper.release());
673  smapper_->configure(nh_);
674  dataset_.reset(dataset.release());
675 
676  closure_assistant_->setMapper(smapper_->getMapper());
677 
678  if (!smapper_->getMapper())
679  {
680  ROS_FATAL("loadSerializedPoseGraph: Could not properly load "
681  "a valid mapping object. Did you modify something by hand?");
682  exit(-1);
683  }
684 
685  if (dataset_->GetLasers().size() < 1)
686  {
687  ROS_FATAL("loadSerializedPoseGraph: Cannot deserialize "
688  "dataset with no laser objects.");
689  exit(-1);
690  }
691 
692  // create a current laser sensor
693  karto::LaserRangeFinder* laser =
694  dynamic_cast<karto::LaserRangeFinder*>(
695  dataset_->GetLasers()[0]);
696  karto::Sensor* pSensor = dynamic_cast<karto::Sensor*>(laser);
697  if (pSensor)
698  {
700 
701  while (ros::ok())
702  {
703  ROS_INFO("Waiting for incoming scan to get metadata...");
705  ros::topic::waitForMessage<sensor_msgs::LaserScan>(
706  scan_topic_, ros::Duration(1.0));
707  if (scan)
708  {
709  ROS_INFO("Got scan!");
710  try
711  {
712  lasers_[scan->header.frame_id] =
713  laser_assistant_->toLaserMetadata(*scan);
714  break;
715  }
716  catch (tf2::TransformException& e)
717  {
718  ROS_ERROR("Failed to compute laser pose, aborting continue mapping (%s)",
719  e.what());
720  exit(-1);
721  }
722  }
723  }
724  }
725  else
726  {
727  ROS_ERROR("Invalid sensor pointer in dataset. Unable to register sensor.");
728  }
729 
730  solver_->Compute();
731 
732  return;
733 }
734 
735 /*****************************************************************************/
737  slam_toolbox_msgs::DeserializePoseGraph::Request &req,
738  slam_toolbox_msgs::DeserializePoseGraph::Response &resp)
739 /*****************************************************************************/
740 {
741  if (req.match_type == slam_toolbox_msgs::DeserializePoseGraph::Request::UNSET)
742  {
743  ROS_ERROR("Deserialization called without valid processor type set. "
744  "Undefined behavior!");
745  return false;
746  }
747 
748  std::string filename = req.filename;
749 
750  if (filename.empty())
751  {
752  ROS_WARN("No map file given!");
753  return true;
754  }
755 
756  // if we're inside the snap, we need to write to commonly accessible space
757  if (snap_utils::isInSnap())
758  {
759  filename = snap_utils::getSnapPath() + std::string("/") + filename;
760  }
761 
762  std::unique_ptr<karto::Dataset> dataset = std::make_unique<karto::Dataset>();
763  std::unique_ptr<karto::Mapper> mapper = std::make_unique<karto::Mapper>();
764 
765  if (!serialization::read(filename, *mapper, *dataset))
766  {
767  ROS_ERROR("DeserializePoseGraph: Failed to read "
768  "file: %s.", filename.c_str());
769  return true;
770  }
771  ROS_DEBUG("DeserializePoseGraph: Successfully read file.");
772 
773  loadSerializedPoseGraph(mapper, dataset);
774  updateMap();
775 
776  first_measurement_ = true;
777  boost::mutex::scoped_lock l(pose_mutex_);
778  switch (req.match_type)
779  {
780  case procType::START_AT_FIRST_NODE:
782  break;
783  case procType::START_AT_GIVEN_POSE:
785  process_near_pose_ = std::make_unique<karto::Pose2>(req.initial_pose.x,
786  req.initial_pose.y, req.initial_pose.theta);
787  break;
788  case procType::LOCALIZE_AT_POSE:
790  process_near_pose_ = std::make_unique<karto::Pose2>(req.initial_pose.x,
791  req.initial_pose.y, req.initial_pose.theta);
792  break;
793  default:
794  ROS_FATAL("Deserialization called without valid processor type set.");
795  }
796 
797  return true;
798 }
799 
800 } // end namespace
slam_toolbox::SlamToolbox::process_near_pose_
std::unique_ptr< karto::Pose2 > process_near_pose_
Definition: slam_toolbox_common.hpp:141
karto::Pose2::SquaredDistance
kt_double SquaredDistance(const Pose2 &rOther) const
Definition: Karto.h:2171
slam_toolbox::SlamToolbox::scan_holder_
std::unique_ptr< laser_utils::ScanHolder > scan_holder_
Definition: slam_toolbox_common.hpp:132
slam_toolbox::SlamToolbox::map_
nav_msgs::GetMap::Response map_
Definition: slam_toolbox_common.hpp:139
tf2::convert
void convert(const A &a, B &b)
ros::NodeHandle::setParam
void setParam(const std::string &key, bool b) const
slam_toolbox::SlamToolbox::ssSerialize_
ros::ServiceServer ssSerialize_
Definition: slam_toolbox_common.hpp:110
karto::SensorManager::RegisterSensor
void RegisterSensor(Sensor *pSensor, kt_bool override=false)
Definition: Karto.h:3732
toolbox_types::PROCESS
@ PROCESS
Definition: toolbox_types.hpp:86
toolbox_types::PausedApplication
PausedApplication
Definition: toolbox_types.hpp:76
slam_toolbox::SlamToolbox::ssMap_
ros::ServiceServer ssMap_
Definition: slam_toolbox_common.hpp:110
slam_toolbox::SlamToolbox::threads_
std::vector< std::unique_ptr< boost::thread > > threads_
Definition: slam_toolbox_common.hpp:135
slam_toolbox::SlamToolbox::scan_filter_
std::unique_ptr< tf2_ros::MessageFilter< sensor_msgs::LaserScan > > scan_filter_
Definition: slam_toolbox_common.hpp:108
slam_toolbox::SlamToolbox::solver_loader_
pluginlib::ClassLoader< karto::ScanSolver > solver_loader_
Definition: slam_toolbox_common.hpp:145
slam_toolbox::SlamToolbox::nh_
ros::NodeHandle nh_
Definition: slam_toolbox_common.hpp:103
slam_toolbox::SlamToolbox::getLaser
karto::LaserRangeFinder * getLaser(const sensor_msgs::LaserScan::ConstPtr &scan)
Definition: slam_toolbox_common.cpp:289
karto::Object::GetName
const Name & GetName() const
Definition: Karto.h:658
slam_toolbox::SlamToolbox::publishPose
void publishPose(const karto::Pose2 &pose, const karto::Matrix3 &cov, const ros::Time &t)
Definition: slam_toolbox_common.cpp:549
karto::LocalizedRangeScan::GetOdometricPose
const Pose2 & GetOdometricPose() const
Definition: Karto.h:5540
karto::LaserRangeFinder
Definition: Karto.h:3922
boost::shared_ptr
slam_toolbox::SlamToolbox::yaw_covariance_scale_
double yaw_covariance_scale_
Definition: slam_toolbox_common.hpp:119
tf2::Transform::inverse
Transform inverse() const
slam_toolbox::SlamToolbox::getLocalizedRangeScan
karto::LocalizedRangeScan * getLocalizedRangeScan(karto::LaserRangeFinder *laser, const sensor_msgs::LaserScan::ConstPtr &scan, karto::Pose2 &karto_pose)
Definition: slam_toolbox_common.cpp:391
tf2::getYaw
double getYaw(const A &a)
karto::OccupancyGrid
Definition: Karto.h:6008
slam_toolbox::SlamToolbox::setSolver
void setSolver(ros::NodeHandle &private_nh_)
Definition: slam_toolbox_common.cpp:83
ros::NodeHandle::getParam
bool getParam(const std::string &key, bool &b) const
slam_toolbox::SlamToolbox::updateMap
bool updateMap()
Definition: slam_toolbox_common.cpp:313
laser_utils::scanToReadings
std::vector< double > scanToReadings(const sensor_msgs::LaserScan &scan, const bool &inverted)
Definition: laser_utils.hpp:33
slam_toolbox::SlamToolbox::state_
PausedState state_
Definition: slam_toolbox_common.hpp:138
karto::Pose2::GetY
kt_double GetY() const
Definition: Karto.h:2117
slam_toolbox_common.hpp
slam_toolbox::SlamToolbox::laser_assistant_
std::unique_ptr< laser_utils::LaserAssistant > laser_assistant_
Definition: slam_toolbox_common.hpp:128
tf2::Quaternion::setRPY
void setRPY(const tf2Scalar &roll, const tf2Scalar &pitch, const tf2Scalar &yaw)
tf2::Transform::setRotation
TF2SIMD_FORCE_INLINE void setRotation(const Quaternion &q)
toolbox_types::PROCESS_NEAR_REGION
@ PROCESS_NEAR_REGION
Definition: toolbox_types.hpp:88
slam_toolbox::SlamToolbox::transform_timeout_
ros::Duration transform_timeout_
Definition: slam_toolbox_common.hpp:114
slam_toolbox::SlamToolbox::shouldProcessScan
bool shouldProcessScan(const sensor_msgs::LaserScan::ConstPtr &scan, const karto::Pose2 &pose)
Definition: slam_toolbox_common.cpp:415
karto::LocalizedRangeScan::SetOdometricPose
void SetOdometricPose(const Pose2 &rPose)
Definition: Karto.h:5549
slam_toolbox::SlamToolbox::smapper_
std::unique_ptr< mapper_utils::SMapper > smapper_
Definition: slam_toolbox_common.hpp:123
ros::NodeHandle::advertiseService
ServiceServer advertiseService(AdvertiseServiceOptions &ops)
serialization::read
bool read(const std::string &filename, karto::Mapper &mapper, karto::Dataset &dataset)
Definition: serialization.hpp:53
vis_utils::toNavMap
void toNavMap(const karto::OccupancyGrid *occ_grid, nav_msgs::OccupancyGrid &map)
Definition: visualization_utils.hpp:122
slam_toolbox::SlamToolbox::deserializePoseGraphCallback
virtual bool deserializePoseGraphCallback(slam_toolbox_msgs::DeserializePoseGraph::Request &req, slam_toolbox_msgs::DeserializePoseGraph::Response &resp)
Definition: slam_toolbox_common.cpp:736
tf2::Stamped
karto::LocalizedRangeScan
Definition: Karto.h:5505
slam_toolbox::SlamToolbox::closure_assistant_
std::unique_ptr< loop_closure_assistant::LoopClosureAssistant > closure_assistant_
Definition: slam_toolbox_common.hpp:131
slam_toolbox::SlamToolbox::map_saver_
std::unique_ptr< map_saver::MapSaver > map_saver_
Definition: slam_toolbox_common.hpp:130
slam_toolbox::SlamToolbox::setTransformFromPoses
tf2::Stamped< tf2::Transform > setTransformFromPoses(const karto::Pose2 &pose, const karto::Pose2 &karto_pose, const ros::Time &t, const bool &update_reprocessing_transform)
Definition: slam_toolbox_common.cpp:340
ros::console::set_logger_level
ROSCONSOLE_DECL bool set_logger_level(const std::string &name, levels::Level level)
slam_toolbox::SlamToolbox::isPaused
bool isPaused(const PausedApplication &app)
Definition: slam_toolbox_common.cpp:609
slam_toolbox::SlamToolbox::SlamToolbox
SlamToolbox(ros::NodeHandle &nh)
Definition: slam_toolbox_common.cpp:28
slam_toolbox::SlamToolbox::tfB_
std::unique_ptr< tf2_ros::TransformBroadcaster > tfB_
Definition: slam_toolbox_common.hpp:106
ros::Publisher::publish
void publish(const boost::shared_ptr< M > &message) const
slam_toolbox::SlamToolbox::reprocessing_transform_
tf2::Transform reprocessing_transform_
Definition: slam_toolbox_common.hpp:142
karto::Matrix3::SetToIdentity
void SetToIdentity()
Definition: Karto.h:2467
ros::NodeHandle::advertise
Publisher advertise(AdvertiseOptions &ops)
ros::ok
ROSCPP_DECL bool ok()
serialization.hpp
karto::Matrix3
Definition: Karto.h:2444
slam_toolbox::SlamToolbox::throttle_scans_
int throttle_scans_
Definition: slam_toolbox_common.hpp:115
pluginlib::PluginlibException
slam_toolbox::SlamToolbox::publishVisualizations
void publishVisualizations()
Definition: slam_toolbox_common.cpp:191
toolbox_types::PROCESS_FIRST_NODE
@ PROCESS_FIRST_NODE
Definition: toolbox_types.hpp:87
slam_toolbox::SlamToolbox::enable_interactive_mode_
bool enable_interactive_mode_
Definition: slam_toolbox_common.hpp:120
ros::console::levels::Debug
Debug
toolbox_types::NEW_MEASUREMENTS
@ NEW_MEASUREMENTS
Definition: toolbox_types.hpp:80
slam_toolbox::SlamToolbox::sst_
ros::Publisher sst_
Definition: slam_toolbox_common.hpp:109
slam_toolbox::SlamToolbox::~SlamToolbox
~SlamToolbox()
Definition: slam_toolbox_common.cpp:65
ROS_DEBUG
#define ROS_DEBUG(...)
karto::SensorManager::GetInstance
static SensorManager * GetInstance()
Definition: Karto.cpp:57
slam_toolbox::SlamToolbox::serializePoseGraphCallback
virtual bool serializePoseGraphCallback(slam_toolbox_msgs::SerializePoseGraph::Request &req, slam_toolbox_msgs::SerializePoseGraph::Response &resp)
Definition: slam_toolbox_common.cpp:616
slam_toolbox::SlamToolbox::tf_buffer_dur_
ros::Duration tf_buffer_dur_
Definition: slam_toolbox_common.hpp:114
slam_toolbox::SlamToolbox::resolution_
double resolution_
Definition: slam_toolbox_common.hpp:117
slam_toolbox::SlamToolbox::tf_
std::unique_ptr< tf2_ros::Buffer > tf_
Definition: slam_toolbox_common.hpp:104
slam_toolbox::SlamToolbox::smapper_mutex_
boost::mutex smapper_mutex_
Definition: slam_toolbox_common.hpp:137
slam_toolbox::SlamToolbox::setROSInterfaces
void setROSInterfaces(ros::NodeHandle &node)
Definition: slam_toolbox_common.cpp:147
slam_toolbox::SlamToolbox::solver_
boost::shared_ptr< karto::ScanSolver > solver_
Definition: slam_toolbox_common.hpp:146
tf2::Transform
tf2::Transform::getRotation
Quaternion getRotation() const
slam_toolbox::SlamToolbox::map_to_odom_
tf2::Transform map_to_odom_
Definition: slam_toolbox_common.hpp:136
slam_toolbox::SlamToolbox::mapCallback
bool mapCallback(nav_msgs::GetMap::Request &req, nav_msgs::GetMap::Response &res)
Definition: slam_toolbox_common.cpp:574
slam_toolbox::SlamToolbox::lasers_
std::map< std::string, laser_utils::LaserMetadata > lasers_
Definition: slam_toolbox_common.hpp:125
slam_toolbox::SlamToolbox::map_frame_
std::string map_frame_
Definition: slam_toolbox_common.hpp:113
karto::LocalizedRangeScan::SetCorrectedPose
void SetCorrectedPose(const Pose2 &rPose)
Definition: Karto.h:5571
slam_toolbox::SlamToolbox::position_covariance_scale_
double position_covariance_scale_
Definition: slam_toolbox_common.hpp:118
ROS_WARN
#define ROS_WARN(...)
karto::LocalizedRangeScan::GetCorrectedPose
const Pose2 & GetCorrectedPose() const
Definition: Karto.h:5562
slam_toolbox::SlamToolbox::setParams
void setParams(ros::NodeHandle &nh)
Definition: slam_toolbox_common.cpp:108
slam_toolbox::SlamToolbox::ssDesserialize_
ros::ServiceServer ssDesserialize_
Definition: slam_toolbox_common.hpp:110
slam_toolbox::SlamToolbox::loadSerializedPoseGraph
void loadSerializedPoseGraph(std::unique_ptr< karto::Mapper > &, std::unique_ptr< karto::Dataset > &)
Definition: slam_toolbox_common.cpp:635
app
app
ros::Rate::sleep
bool sleep()
ROS_FATAL
#define ROS_FATAL(...)
slam_toolbox::SlamToolbox::scan_topic_
std::string scan_topic_
Definition: slam_toolbox_common.hpp:113
toolbox_types::VerticeMap
std::map< karto::Name, std::map< int, karto::Vertex< karto::LocalizedRangeScan > * > > VerticeMap
Definition: toolbox_types.hpp:118
slam_toolbox::SlamToolbox::loadPoseGraphByParams
void loadPoseGraphByParams(ros::NodeHandle &nh)
Definition: slam_toolbox_common.cpp:223
slam_toolbox::SlamToolbox::shouldStartWithPoseGraph
bool shouldStartWithPoseGraph(std::string &filename, geometry_msgs::Pose2D &pose, bool &start_at_dock)
Definition: slam_toolbox_common.cpp:250
slam_toolbox::SlamToolbox::first_measurement_
bool first_measurement_
Definition: slam_toolbox_common.hpp:120
slam_toolbox::SlamToolbox::processor_type_
ProcessType processor_type_
Definition: slam_toolbox_common.hpp:140
slam_toolbox::SlamToolbox::base_frame_
std::string base_frame_
Definition: slam_toolbox_common.hpp:113
ros::console::notifyLoggerLevelsChanged
ROSCONSOLE_DECL void notifyLoggerLevelsChanged()
pluginlib::ClassLoader::createInstance
boost::shared_ptr< T > createInstance(const std::string &lookup_name)
ros::Time
slam_toolbox::SlamToolbox::addScan
virtual karto::LocalizedRangeScan * addScan(karto::LaserRangeFinder *laser, const sensor_msgs::LaserScan::ConstPtr &scan, karto::Pose2 &karto_pose)
Definition: slam_toolbox_common.cpp:476
slam_toolbox::SlamToolbox::dataset_
std::unique_ptr< karto::Dataset > dataset_
Definition: slam_toolbox_common.hpp:124
slam_toolbox::SlamToolbox::scan_filter_sub_
std::unique_ptr< message_filters::Subscriber< sensor_msgs::LaserScan > > scan_filter_sub_
Definition: slam_toolbox_common.hpp:107
slam_toolbox::SlamToolbox::minimum_time_interval_
ros::Duration minimum_time_interval_
Definition: slam_toolbox_common.hpp:114
karto::Pose2::GetX
kt_double GetX() const
Definition: Karto.h:2099
ROS_ERROR
#define ROS_ERROR(...)
slam_toolbox::SlamToolbox::tfL_
std::unique_ptr< tf2_ros::TransformListener > tfL_
Definition: slam_toolbox_common.hpp:105
slam_toolbox::SlamToolbox::pose_helper_
std::unique_ptr< pose_utils::GetPoseHelper > pose_helper_
Definition: slam_toolbox_common.hpp:129
ros::NodeHandle::param
T param(const std::string &param_name, const T &default_val) const
tf2::Quaternion
slam_toolbox::SlamToolbox::map_name_
std::string map_name_
Definition: slam_toolbox_common.hpp:113
tf2::toMsg
B toMsg(const A &a)
snap_utils::getSnapPath
std::string getSnapPath()
Definition: snap_utils.hpp:37
slam_toolbox::SlamToolbox::sstm_
ros::Publisher sstm_
Definition: slam_toolbox_common.hpp:109
ros::Rate
slam_toolbox
Definition: slam_toolbox_lifelong.hpp:24
process_constraints.filename
filename
Definition: process_constraints.py:114
inverse
TFSIMD_FORCE_INLINE Quaternion inverse(const Quaternion &q)
karto::Pose2
Definition: Karto.h:2046
ros::Publisher::getNumSubscribers
uint32_t getNumSubscribers() const
slam_toolbox::SlamToolbox::pauseNewMeasurementsCallback
bool pauseNewMeasurementsCallback(slam_toolbox_msgs::Pause::Request &req, slam_toolbox_msgs::Pause::Response &resp)
Definition: slam_toolbox_common.cpp:592
ROSCONSOLE_DEFAULT_NAME
#define ROSCONSOLE_DEFAULT_NAME
toolbox_types::VISUALIZING_GRAPH
@ VISUALIZING_GRAPH
Definition: toolbox_types.hpp:79
header
const std::string header
tf2::TransformException
ROS_INFO
#define ROS_INFO(...)
slam_toolbox::SlamToolbox::map_to_odom_mutex_
boost::mutex map_to_odom_mutex_
Definition: slam_toolbox_common.hpp:137
toolbox_types::PROCESS_LOCALIZATION
@ PROCESS_LOCALIZATION
Definition: toolbox_types.hpp:89
slam_toolbox::SlamToolbox::publishTransformLoop
void publishTransformLoop(const double &transform_publish_period)
Definition: slam_toolbox_common.cpp:166
toolbox_types::EdgeVector
std::vector< karto::Edge< karto::LocalizedRangeScan > * > EdgeVector
Definition: toolbox_types.hpp:119
karto::Sensor
Definition: Karto.h:3600
ros::Duration
slam_toolbox::SlamToolbox::odom_frame_
std::string odom_frame_
Definition: slam_toolbox_common.hpp:113
t
geometry_msgs::TransformStamped t
serialization::write
void write(const std::string &filename, karto::Mapper &mapper, karto::Dataset &dataset)
Definition: serialization.hpp:38
karto::Pose2::GetHeading
kt_double GetHeading() const
Definition: Karto.h:2153
slam_toolbox::SlamToolbox::pose_mutex_
boost::mutex pose_mutex_
Definition: slam_toolbox_common.hpp:137
ros::NodeHandle
slam_toolbox::SlamToolbox::pose_pub_
ros::Publisher pose_pub_
Definition: slam_toolbox_common.hpp:109
slam_toolbox::SlamToolbox::ssPauseMeasurements_
ros::ServiceServer ssPauseMeasurements_
Definition: slam_toolbox_common.hpp:110
slam_toolbox::SlamToolbox::laserCallback
virtual void laserCallback(const sensor_msgs::LaserScan::ConstPtr &scan)=0
ros::Time::now
static Time now()
tf2::Transform::setIdentity
void setIdentity()
snap_utils::isInSnap
bool isInSnap()
Definition: snap_utils.hpp:26


slam_toolbox
Author(s): Steve Macenski
autogenerated on Thu Jan 11 2024 03:37:56