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  bool debug = false;
130  if (private_nh.getParam("debug_logging", debug) && debug)
131  {
134  {
136  }
137  }
138 
139  smapper_->configure(private_nh);
140  private_nh.setParam("paused_new_measurements", false);
141 }
142 
143 /*****************************************************************************/
145 /*****************************************************************************/
146 {
147  tf_ = std::make_unique<tf2_ros::Buffer>(ros::Duration(tf_buffer_dur_));
148  tfL_ = std::make_unique<tf2_ros::TransformListener>(*tf_);
149  tfB_ = std::make_unique<tf2_ros::TransformBroadcaster>();
150  sst_ = node.advertise<nav_msgs::OccupancyGrid>(map_name_, 1, true);
151  sstm_ = node.advertise<nav_msgs::MapMetaData>(map_name_ + "_metadata", 1, true);
152  ssMap_ = node.advertiseService("dynamic_map", &SlamToolbox::mapCallback, this);
156  scan_filter_sub_ = std::make_unique<message_filters::Subscriber<sensor_msgs::LaserScan> >(node, scan_topic_, 5);
157  scan_filter_ = std::make_unique<tf2_ros::MessageFilter<sensor_msgs::LaserScan> >(*scan_filter_sub_, *tf_, odom_frame_, 5, node);
158  scan_filter_->registerCallback(boost::bind(&SlamToolbox::laserCallback, this, _1));
159 }
160 
161 /*****************************************************************************/
162 void SlamToolbox::publishTransformLoop(const double& transform_publish_period)
163 /*****************************************************************************/
164 {
165  if(transform_publish_period == 0)
166  {
167  return;
168  }
169 
170  ros::Rate r(1.0 / transform_publish_period);
171  while(ros::ok())
172  {
173  {
174  boost::mutex::scoped_lock lock(map_to_odom_mutex_);
175  geometry_msgs::TransformStamped msg;
176  tf2::convert(map_to_odom_, msg.transform);
177  msg.child_frame_id = odom_frame_;
178  msg.header.frame_id = map_frame_;
179  msg.header.stamp = ros::Time::now() + transform_timeout_;
180  tfB_->sendTransform(msg);
181  }
182  r.sleep();
183  }
184 }
185 
186 /*****************************************************************************/
188 /*****************************************************************************/
189 {
190  nav_msgs::OccupancyGrid& og = map_.map;
191  og.info.resolution = resolution_;
192  og.info.origin.position.x = 0.0;
193  og.info.origin.position.y = 0.0;
194  og.info.origin.position.z = 0.0;
195  og.info.origin.orientation.x = 0.0;
196  og.info.origin.orientation.y = 0.0;
197  og.info.origin.orientation.z = 0.0;
198  og.info.origin.orientation.w = 1.0;
199  og.header.frame_id = map_frame_;
200 
201  double map_update_interval;
202  if(!nh_.getParam("map_update_interval", map_update_interval))
203  map_update_interval = 10.0;
204  ros::Rate r(1.0 / map_update_interval);
205 
206  while(ros::ok())
207  {
208  updateMap();
210  {
211  closure_assistant_->publishGraph();
212  }
213  r.sleep();
214  }
215 }
216 
217 /*****************************************************************************/
219 /*****************************************************************************/
220 {
221  std::string filename;
222  geometry_msgs::Pose2D pose;
223  bool dock = false;
224  if (shouldStartWithPoseGraph(filename, pose, dock))
225  {
226  slam_toolbox_msgs::DeserializePoseGraph::Request req;
227  slam_toolbox_msgs::DeserializePoseGraph::Response resp;
228  req.initial_pose = pose;
229  req.filename = filename;
230  if (dock)
231  {
232  req.match_type =
233  slam_toolbox_msgs::DeserializePoseGraph::Request::START_AT_FIRST_NODE;
234  }
235  else
236  {
237  req.match_type =
238  slam_toolbox_msgs::DeserializePoseGraph::Request::START_AT_GIVEN_POSE;
239  }
240  deserializePoseGraphCallback(req, resp);
241  }
242 }
243 
244 /*****************************************************************************/
245 bool SlamToolbox::shouldStartWithPoseGraph(std::string& filename,
246  geometry_msgs::Pose2D& pose, bool& start_at_dock)
247 /*****************************************************************************/
248  {
249  // if given a map to load at run time, do it.
250  if (nh_.getParam("map_file_name", filename))
251  {
252  std::vector<double> read_pose;
253  if (nh_.getParam("map_start_pose", read_pose))
254  {
255  start_at_dock = false;
256  if (read_pose.size() != 3)
257  {
258  ROS_ERROR("LocalizationSlamToolbox: Incorrect number of "
259  "arguments for map starting pose. Must be in format: "
260  "[x, y, theta]. Starting at the origin");
261  pose.x = 0.;
262  pose.y = 0.;
263  pose.theta = 0.;
264  }
265  else
266  {
267  pose.x = read_pose[0];
268  pose.y = read_pose[1];
269  pose.theta = read_pose[2];
270  }
271  }
272  else
273  {
274  nh_.getParam("map_start_at_dock", start_at_dock);
275  }
276 
277  return true;
278  }
279 
280  return false;
281 }
282 
283 /*****************************************************************************/
285  sensor_msgs::LaserScan::ConstPtr& scan)
286 /*****************************************************************************/
287 {
288  const std::string& frame = scan->header.frame_id;
289  if(lasers_.find(frame) == lasers_.end())
290  {
291  try
292  {
293  lasers_[frame] = laser_assistant_->toLaserMetadata(*scan);
294  dataset_->Add(lasers_[frame].getLaser(), true);
295  }
296  catch (tf2::TransformException& e)
297  {
298  ROS_ERROR("Failed to compute laser pose, aborting initialization (%s)",
299  e.what());
300  return nullptr;
301  }
302  }
303 
304  return lasers_[frame].getLaser();
305 }
306 
307 /*****************************************************************************/
309 /*****************************************************************************/
310 {
311  if (sst_.getNumSubscribers() == 0)
312  {
313  return true;
314  }
315  boost::mutex::scoped_lock lock(smapper_mutex_);
316  karto::OccupancyGrid* occ_grid = smapper_->getOccupancyGrid(resolution_);
317  if(!occ_grid)
318  {
319  return false;
320  }
321 
322  vis_utils::toNavMap(occ_grid, map_.map);
323 
324  // publish map as current
325  map_.map.header.stamp = ros::Time::now();
326  sst_.publish(map_.map);
327  sstm_.publish(map_.map.info);
328 
329  delete occ_grid;
330  occ_grid = nullptr;
331  return true;
332 }
333 
334 /*****************************************************************************/
336  const karto::Pose2& corrected_pose,
337  const karto::Pose2& karto_pose,
338  const ros::Time& t,
339  const bool& update_reprocessing_transform)
340 /*****************************************************************************/
341 {
342  // Compute the map->odom transform
343  tf2::Stamped<tf2::Transform> odom_to_map;
344  tf2::Quaternion q(0.,0.,0.,1.0);
345  q.setRPY(0., 0., corrected_pose.GetHeading());
346  tf2::Stamped<tf2::Transform> base_to_map(
347  tf2::Transform(q, tf2::Vector3(corrected_pose.GetX(),
348  corrected_pose.GetY(), 0.0)).inverse(), t, base_frame_);
349  try
350  {
351  geometry_msgs::TransformStamped base_to_map_msg, odom_to_map_msg;
352  tf2::convert(base_to_map, base_to_map_msg);
353  odom_to_map_msg = tf_->transform(base_to_map_msg, odom_frame_);
354  tf2::convert(odom_to_map_msg, odom_to_map);
355  }
356  catch(tf2::TransformException& e)
357  {
358  ROS_ERROR("Transform from base_link to odom failed: %s", e.what());
359  return odom_to_map;
360  }
361 
362  // if we're continuing a previous session, we need to
363  // estimate the homogenous transformation between the old and new
364  // odometry frames and transform the new session
365  // into the older session's frame
366  if (update_reprocessing_transform)
367  {
368  tf2::Transform odom_to_base_serialized = base_to_map.inverse();
369  tf2::Quaternion q1(0.,0.,0.,1.0);
370  q1.setRPY(0., 0., tf2::getYaw(odom_to_base_serialized.getRotation()));
371  odom_to_base_serialized.setRotation(q1);
372  tf2::Transform odom_to_base_current = smapper_->toTfPose(karto_pose);
374  odom_to_base_serialized * odom_to_base_current.inverse();
375  }
376 
377  // set map to odom for our transformation thread to publish
378  boost::mutex::scoped_lock lock(map_to_odom_mutex_);
379  map_to_odom_ = tf2::Transform(tf2::Quaternion( odom_to_map.getRotation() ),
380  tf2::Vector3( odom_to_map.getOrigin() ) ).inverse();
381 
382  return odom_to_map;
383 }
384 
385 /*****************************************************************************/
388  const sensor_msgs::LaserScan::ConstPtr& scan,
389  karto::Pose2& karto_pose)
390 /*****************************************************************************/
391 {
392  // Create a vector of doubles for karto
393  std::vector<kt_double> readings = laser_utils::scanToReadings(
394  *scan, lasers_[scan->header.frame_id].isInverted());
395 
396  // transform by the reprocessing transform
397  tf2::Transform pose_original = smapper_->toTfPose(karto_pose);
398  tf2::Transform tf_pose_transformed = reprocessing_transform_ * pose_original;
399  karto::Pose2 transformed_pose = smapper_->toKartoPose(tf_pose_transformed);
400 
401  // create localized range scan
403  laser->GetName(), readings);
404  range_scan->SetOdometricPose(transformed_pose);
405  range_scan->SetCorrectedPose(transformed_pose);
406  return range_scan;
407 }
408 
409 /*****************************************************************************/
411  const sensor_msgs::LaserScan::ConstPtr& scan,
412  const karto::Pose2& pose)
413 /*****************************************************************************/
414 {
415  static karto::Pose2 last_pose;
416  static ros::Time last_scan_time = ros::Time(0.);
417  static const double dist_thresh_sq = smapper_->getMapper()->getParamMinimumTravelDistance()*
418  smapper_->getMapper()->getParamMinimumTravelDistance();
419 
420  // we give it a pass on the first measurement to get the ball rolling
421  if (first_measurement_)
422  {
423  last_scan_time = scan->header.stamp;
424  last_pose = pose;
425  first_measurement_ = false;
426  return true;
427  }
428 
429  // we are in a paused mode, reject incomming information
431  {
432  return false;
433  }
434 
435  // throttled out
436  if ((scan->header.seq % throttle_scans_) != 0)
437  {
438  return false;
439  }
440 
441  // not enough time
442  if (scan->header.stamp - last_scan_time < minimum_time_interval_)
443  {
444  return false;
445  }
446 
447  // check moved enough, within 10% for correction error
448  const double sq_dist_to_last_accepted_pose = last_pose.SquaredDistance(pose);
449 
450  if(sq_dist_to_last_accepted_pose < 0.8 * dist_thresh_sq || scan->header.seq < 5)
451  {
452  return false;
453  }
454 
455  last_pose = pose;
456  last_scan_time = scan->header.stamp;
457 
458  return true;
459 }
460 
461 /*****************************************************************************/
464  PosedScan& scan_w_pose)
465 /*****************************************************************************/
466 {
467  return addScan(laser, scan_w_pose.scan, scan_w_pose.pose);
468 }
469 
470 /*****************************************************************************/
473  const sensor_msgs::LaserScan::ConstPtr& scan,
474  karto::Pose2& karto_pose)
475 /*****************************************************************************/
476 {
477  // get our localized range scan
479  laser, scan, karto_pose);
480 
481  // Add the localized range scan to the smapper
482  boost::mutex::scoped_lock lock(smapper_mutex_);
483  bool processed = false, update_reprocessing_transform = false;
484 
485  if (processor_type_ == PROCESS)
486  {
487  processed = smapper_->getMapper()->Process(range_scan);
488  }
490  {
491  processed = smapper_->getMapper()->ProcessAtDock(range_scan);
493  update_reprocessing_transform = true;
494  }
496  {
497  boost::mutex::scoped_lock l(pose_mutex_);
498  if (!process_near_pose_)
499  {
500  ROS_ERROR("Process near region called without a "
501  "valid region request. Ignoring scan.");
502  return nullptr;
503  }
504  range_scan->SetOdometricPose(*process_near_pose_);
505  range_scan->SetCorrectedPose(range_scan->GetOdometricPose());
506  process_near_pose_.reset(nullptr);
507  processed = smapper_->getMapper()->ProcessAgainstNodesNearBy(range_scan);
508  update_reprocessing_transform = true;
510  }
511  else
512  {
513  ROS_FATAL("SlamToolbox: No valid processor type set! Exiting.");
514  exit(-1);
515  }
516 
517  // if successfully processed, create odom to map transformation
518  // and add our scan to storage
519  if(processed)
520  {
522  {
523  scan_holder_->addScan(*scan);
524  }
525 
526  setTransformFromPoses(range_scan->GetCorrectedPose(), karto_pose,
527  scan->header.stamp, update_reprocessing_transform);
528  dataset_->Add(range_scan);
529  }
530  else
531  {
532  delete range_scan;
533  range_scan = nullptr;
534  }
535 
536  return range_scan;
537 }
538 
539 /*****************************************************************************/
541  nav_msgs::GetMap::Request &req,
542  nav_msgs::GetMap::Response &res)
543 /*****************************************************************************/
544 {
545  if(map_.map.info.width && map_.map.info.height)
546  {
547  boost::mutex::scoped_lock lock(smapper_mutex_);
548  res = map_;
549  return true;
550  }
551  else
552  {
553  return false;
554  }
555 }
556 
557 /*****************************************************************************/
559  slam_toolbox_msgs::Pause::Request& req,
560  slam_toolbox_msgs::Pause::Response& resp)
561 /*****************************************************************************/
562 {
563  bool curr_state = isPaused(NEW_MEASUREMENTS);
564  state_.set(NEW_MEASUREMENTS, !curr_state);
565 
566  nh_.setParam("paused_new_measurements", !curr_state);
567  ROS_INFO("SlamToolbox: Toggled to %s",
568  !curr_state ? "pause taking new measurements." :
569  "actively taking new measurements.");
570  resp.status = true;
571  return true;
572 }
573 
574 /*****************************************************************************/
576 /*****************************************************************************/
577 {
578  return state_.get(app);
579 }
580 
581 /*****************************************************************************/
583  slam_toolbox_msgs::SerializePoseGraph::Request &req,
584  slam_toolbox_msgs::SerializePoseGraph::Response &resp)
585 /*****************************************************************************/
586 {
587  std::string filename = req.filename;
588 
589  // if we're inside the snap, we need to write to commonly accessible space
590  if (snap_utils::isInSnap())
591  {
592  filename = snap_utils::getSnapPath() + std::string("/") + filename;
593  }
594 
595  boost::mutex::scoped_lock lock(smapper_mutex_);
596  serialization::write(filename, *smapper_->getMapper(), *dataset_);
597  return true;
598 }
599 
600 /*****************************************************************************/
602  std::unique_ptr<karto::Mapper>& mapper,
603  std::unique_ptr<karto::Dataset>& dataset)
604 /*****************************************************************************/
605 {
606  boost::mutex::scoped_lock lock(smapper_mutex_);
607 
608  solver_->Reset();
609 
610  // add the nodes and constraints to the optimizer
611  VerticeMap mapper_vertices = mapper->GetGraph()->GetVertices();
612  VerticeMap::iterator vertex_map_it = mapper_vertices.begin();
613  for(vertex_map_it; vertex_map_it != mapper_vertices.end(); ++vertex_map_it)
614  {
615  ScanMap::iterator vertex_it = vertex_map_it->second.begin();
616  for(vertex_it; vertex_it != vertex_map_it->second.end(); ++vertex_it)
617  {
618  if (vertex_it->second != nullptr)
619  {
620  solver_->AddNode(vertex_it->second);
621  }
622  }
623  }
624 
625  EdgeVector mapper_edges = mapper->GetGraph()->GetEdges();
626  EdgeVector::iterator edges_it = mapper_edges.begin();
627  for( edges_it; edges_it != mapper_edges.end(); ++edges_it)
628  {
629  if (*edges_it != nullptr)
630  {
631  solver_->AddConstraint(*edges_it);
632  }
633  }
634 
635  mapper->SetScanSolver(solver_.get());
636 
637  // move the memory to our working dataset
638  smapper_->setMapper(mapper.release());
639  smapper_->configure(nh_);
640  dataset_.reset(dataset.release());
641 
642  if (!smapper_->getMapper())
643  {
644  ROS_FATAL("loadSerializedPoseGraph: Could not properly load "
645  "a valid mapping object. Did you modify something by hand?");
646  exit(-1);
647  }
648 
649  if (dataset_->GetLasers().size() < 1)
650  {
651  ROS_FATAL("loadSerializedPoseGraph: Cannot deserialize "
652  "dataset with no laser objects.");
653  exit(-1);
654  }
655 
656  // create a current laser sensor
657  karto::LaserRangeFinder* laser =
658  dynamic_cast<karto::LaserRangeFinder*>(
659  dataset_->GetLasers()[0]);
660  karto::Sensor* pSensor = dynamic_cast<karto::Sensor*>(laser);
661  if (pSensor)
662  {
664 
665  while (ros::ok())
666  {
667  ROS_INFO("Waiting for incoming scan to get metadata...");
669  ros::topic::waitForMessage<sensor_msgs::LaserScan>(
670  scan_topic_, ros::Duration(1.0));
671  if (scan)
672  {
673  ROS_INFO("Got scan!");
674  try
675  {
676  lasers_[scan->header.frame_id] =
677  laser_assistant_->toLaserMetadata(*scan);
678  break;
679  }
680  catch (tf2::TransformException& e)
681  {
682  ROS_ERROR("Failed to compute laser pose, aborting continue mapping (%s)",
683  e.what());
684  exit(-1);
685  }
686  }
687  }
688  }
689  else
690  {
691  ROS_ERROR("Invalid sensor pointer in dataset. Unable to register sensor.");
692  }
693 
694  solver_->Compute();
695 
696  return;
697 }
698 
699 /*****************************************************************************/
701  slam_toolbox_msgs::DeserializePoseGraph::Request &req,
702  slam_toolbox_msgs::DeserializePoseGraph::Response &resp)
703 /*****************************************************************************/
704 {
705  if (req.match_type == slam_toolbox_msgs::DeserializePoseGraph::Request::UNSET)
706  {
707  ROS_ERROR("Deserialization called without valid processor type set. "
708  "Undefined behavior!");
709  return false;
710  }
711 
712  std::string filename = req.filename;
713 
714  if (filename.empty())
715  {
716  ROS_WARN("No map file given!");
717  return true;
718  }
719 
720  // if we're inside the snap, we need to write to commonly accessible space
721  if (snap_utils::isInSnap())
722  {
723  filename = snap_utils::getSnapPath() + std::string("/") + filename;
724  }
725 
726  std::unique_ptr<karto::Dataset> dataset = std::make_unique<karto::Dataset>();
727  std::unique_ptr<karto::Mapper> mapper = std::make_unique<karto::Mapper>();
728 
729  if (!serialization::read(filename, *mapper, *dataset))
730  {
731  ROS_ERROR("DeserializePoseGraph: Failed to read "
732  "file: %s.", filename.c_str());
733  return true;
734  }
735  ROS_DEBUG("DeserializePoseGraph: Successfully read file.");
736 
737  loadSerializedPoseGraph(mapper, dataset);
738  updateMap();
739 
740  first_measurement_ = true;
741  boost::mutex::scoped_lock l(pose_mutex_);
742  switch (req.match_type)
743  {
744  case procType::START_AT_FIRST_NODE:
746  break;
747  case procType::START_AT_GIVEN_POSE:
749  process_near_pose_ = std::make_unique<karto::Pose2>(req.initial_pose.x,
750  req.initial_pose.y, req.initial_pose.theta);
751  break;
752  case procType::LOCALIZE_AT_POSE:
754  process_near_pose_ = std::make_unique<karto::Pose2>(req.initial_pose.x,
755  req.initial_pose.y, req.initial_pose.theta);
756  break;
757  default:
758  ROS_FATAL("Deserialization called without valid processor type set.");
759  }
760 
761  return true;
762 }
763 
764 } // end namespace
std::vector< std::unique_ptr< boost::thread > > threads_
bool pauseNewMeasurementsCallback(slam_toolbox_msgs::Pause::Request &req, slam_toolbox_msgs::Pause::Response &resp)
std::string getSnapPath()
Definition: snap_utils.hpp:37
std::unique_ptr< laser_utils::LaserAssistant > laser_assistant_
void setRPY(const tf2Scalar &roll, const tf2Scalar &pitch, const tf2Scalar &yaw)
filename
kt_double SquaredDistance(const Pose2 &rOther) const
Definition: Karto.h:2169
#define ROS_FATAL(...)
nav_msgs::GetMap::Response map_
ROSCONSOLE_DECL void notifyLoggerLevelsChanged()
const Pose2 & GetOdometricPose() const
Definition: Karto.h:5538
std::unique_ptr< map_saver::MapSaver > map_saver_
bool isInSnap()
Definition: snap_utils.hpp:26
tf2::Stamped< tf2::Transform > setTransformFromPoses(const karto::Pose2 &pose, const karto::Pose2 &karto_pose, const ros::Time &t, const bool &update_reprocessing_transform)
bool isPaused(const PausedApplication &app)
karto::LaserRangeFinder * getLaser(const sensor_msgs::LaserScan::ConstPtr &scan)
virtual void laserCallback(const sensor_msgs::LaserScan::ConstPtr &scan)=0
bool shouldProcessScan(const sensor_msgs::LaserScan::ConstPtr &scan, const karto::Pose2 &pose)
TF2SIMD_FORCE_INLINE void setRotation(const Quaternion &q)
const Pose2 & GetCorrectedPose() const
Definition: Karto.h:5560
std::unique_ptr< karto::Pose2 > process_near_pose_
boost::shared_ptr< T > createInstance(const std::string &lookup_name)
void toNavMap(const karto::OccupancyGrid *occ_grid, nav_msgs::OccupancyGrid &map)
std::unique_ptr< tf2_ros::TransformBroadcaster > tfB_
std::vector< karto::Edge< karto::LocalizedRangeScan > * > EdgeVector
karto::LocalizedRangeScan * getLocalizedRangeScan(karto::LaserRangeFinder *laser, const sensor_msgs::LaserScan::ConstPtr &scan, karto::Pose2 &karto_pose)
ServiceServer advertiseService(const std::string &service, bool(T::*srv_func)(MReq &, MRes &), T *obj)
bool read(const std::string &filename, karto::Mapper &mapper, karto::Dataset &dataset)
#define ROS_WARN(...)
virtual bool serializePoseGraphCallback(slam_toolbox_msgs::SerializePoseGraph::Request &req, slam_toolbox_msgs::SerializePoseGraph::Response &resp)
void setIdentity()
Transform inverse() const
SlamToolbox(ros::NodeHandle &nh)
boost::shared_ptr< karto::ScanSolver > solver_
std::unique_ptr< mapper_utils::SMapper > smapper_
virtual bool deserializePoseGraphCallback(slam_toolbox_msgs::DeserializePoseGraph::Request &req, slam_toolbox_msgs::DeserializePoseGraph::Response &resp)
std::unique_ptr< loop_closure_assistant::LoopClosureAssistant > closure_assistant_
bool param(const std::string &param_name, T &param_val, const T &default_val) const
void publish(const boost::shared_ptr< M > &message) const
void setROSInterfaces(ros::NodeHandle &node)
#define ROS_INFO(...)
bool getParam(const std::string &key, std::string &s) const
TFSIMD_FORCE_INLINE Quaternion inverse(const Quaternion &q)
std::map< karto::Name, std::map< int, karto::Vertex< karto::LocalizedRangeScan > * > > VerticeMap
kt_double GetX() const
Definition: Karto.h:2097
ROSCPP_DECL bool ok()
std::unique_ptr< karto::Dataset > dataset_
std::vector< double > scanToReadings(const sensor_msgs::LaserScan &scan, const bool &inverted)
Definition: laser_utils.hpp:33
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
std::map< std::string, laser_utils::LaserMetadata > lasers_
static SensorManager * GetInstance()
Definition: Karto.cpp:57
virtual karto::LocalizedRangeScan * addScan(karto::LaserRangeFinder *laser, const sensor_msgs::LaserScan::ConstPtr &scan, karto::Pose2 &karto_pose)
bool sleep()
double getYaw(const A &a)
void SetOdometricPose(const Pose2 &rPose)
Definition: Karto.h:5547
std::unique_ptr< tf2_ros::Buffer > tf_
bool mapCallback(nav_msgs::GetMap::Request &req, nav_msgs::GetMap::Response &res)
void publishTransformLoop(const double &transform_publish_period)
void loadPoseGraphByParams(ros::NodeHandle &nh)
void setParam(const std::string &key, const XmlRpc::XmlRpcValue &v) const
const Name & GetName() const
Definition: Karto.h:656
void SetCorrectedPose(const Pose2 &rPose)
Definition: Karto.h:5569
void setParams(ros::NodeHandle &nh)
ros::ServiceServer ssDesserialize_
void loadSerializedPoseGraph(std::unique_ptr< karto::Mapper > &, std::unique_ptr< karto::Dataset > &)
Quaternion getRotation() const
static Time now()
void convert(const A &a, B &b)
ros::ServiceServer ssPauseMeasurements_
const std::string header
void RegisterSensor(Sensor *pSensor, kt_bool override=false)
Definition: Karto.h:3730
bool shouldStartWithPoseGraph(std::string &filename, geometry_msgs::Pose2D &pose, bool &start_at_dock)
ROSCONSOLE_DECL bool set_logger_level(const std::string &name, levels::Level level)
std::unique_ptr< laser_utils::ScanHolder > scan_holder_
kt_double GetY() const
Definition: Karto.h:2115
std::unique_ptr< pose_utils::GetPoseHelper > pose_helper_
uint32_t getNumSubscribers() const
r
std::unique_ptr< message_filters::Subscriber< sensor_msgs::LaserScan > > scan_filter_sub_
#define ROS_ERROR(...)
#define ROSCONSOLE_DEFAULT_NAME
std::unique_ptr< tf2_ros::TransformListener > tfL_
void setSolver(ros::NodeHandle &private_nh_)
#define ROS_DEBUG(...)
void write(const std::string &filename, karto::Mapper &mapper, karto::Dataset &dataset)
pluginlib::ClassLoader< karto::ScanSolver > solver_loader_
kt_double GetHeading() const
Definition: Karto.h:2151
std::unique_ptr< tf2_ros::MessageFilter< sensor_msgs::LaserScan > > scan_filter_


slam_toolbox
Author(s): Steve Macenski
autogenerated on Wed Mar 3 2021 03:49:20