Changed pose_helpers_ from vector to map

Author: jsongCMU

slam_toolbox/include/slam_toolbox/slam_toolbox_common.hpp

@@ -127,7 +127,7 @@ class SlamToolbox
 
   // helpers
   std::map<std::string,std::unique_ptr<laser_utils::LaserAssistant>> laser_assistants_;
-  std::vector<std::unique_ptr<pose_utils::GetPoseHelper>> pose_helpers_;
+  std::map<std::string,std::unique_ptr<pose_utils::GetPoseHelper>> pose_helpers_;
   std::unique_ptr<map_saver::MapSaver> map_saver_;
   std::unique_ptr<loop_closure_assistant::LoopClosureAssistant> closure_assistant_;
   std::unique_ptr<laser_utils::ScanHolder> scan_holder_;

slam_toolbox/src/experimental/slam_toolbox_lifelong.cpp

@@ -71,15 +71,12 @@ void LifelongSlamToolbox::laserCallback(
 {
   // no odom info on any pose helper
   karto::Pose2 pose;
-  bool found_odom = false;
-  for(size_t idx = 0; idx < pose_helpers_.size(); idx++)
+  if(pose_helpers_.find(base_frame_id) == pose_helpers_.end())
+    return;
+  else
   {
-    found_odom = pose_helpers_[idx]->getOdomPose(pose, scan->header.stamp, scan->header.frame_id);
-    if(found_odom)
-      break;
+    pose_helpers_[base_frame_id]->getOdomPose(pose, scan->header.stamp, base_frame_id);
   }
-  if(!found_odom)
-    return;
 
   // ensure the laser can be used
   LaserRangeFinder* laser = getLaser(scan);

slam_toolbox/src/slam_toolbox_async.cpp

@@ -37,16 +37,12 @@ void AsynchronousSlamToolbox::laserCallback(
 {
   // no odom info on any pose helper
   karto::Pose2 pose;
-  bool found_odom = false;
-  for(size_t idx = 0; idx < pose_helpers_.size(); idx++)
+  if(pose_helpers_.find(base_frame_id) == pose_helpers_.end())
+    return;
+  else
   {
-    // try compute odometry
-    found_odom = pose_helpers_[idx]->getOdomPose(pose, scan->header.stamp, base_frame_id);
-    if(found_odom)
-      break;
+    pose_helpers_[base_frame_id]->getOdomPose(pose, scan->header.stamp, base_frame_id);
   }
-  if(!found_odom)
-    return;
 
   // Add new sensor to laser ID map, and to laser assistants
   {

slam_toolbox/src/slam_toolbox_common.cpp

@@ -54,7 +54,7 @@ SlamToolbox::SlamToolbox(ros::NodeHandle& nh)
   // Set up pose helpers for each robot
   for(size_t idx = 0; idx < base_frames_.size(); idx++)
   {
-    pose_helpers_.push_back(std::make_unique<pose_utils::GetPoseHelper>(tf_.get(), base_frames_[idx], odom_frames_[idx]));
+    pose_helpers_[base_frames_[idx]] = std::make_unique<pose_utils::GetPoseHelper>(tf_.get(), base_frames_[idx], odom_frames_[idx]);
   }
   scan_holder_ = std::make_unique<laser_utils::ScanHolder>(lasers_);
   map_saver_ = std::make_unique<map_saver::MapSaver>(nh_, map_name_);
@@ -86,9 +86,9 @@ SlamToolbox::~SlamToolbox()
   dataset_.reset();
   closure_assistant_.reset();
   map_saver_.reset();
-  for(size_t idx = 0; idx < pose_helpers_.size(); idx++)
+  for(std::map<std::string,std::unique_ptr<pose_utils::GetPoseHelper>>::iterator it = pose_helpers_.begin(); it != pose_helpers_.end(); it++)
   {
-    pose_helpers_[idx].reset();
+    it->second.reset();
   }
   for(std::map<std::string,std::unique_ptr<laser_utils::LaserAssistant>>::iterator it = laser_assistants_.begin(); it != laser_assistants_.end(); it++)
   {

slam_toolbox/src/slam_toolbox_localization.cpp

@@ -106,15 +106,12 @@ void LocalizationSlamToolbox::laserCallback(
 {
 // no odom info on any pose helper
   karto::Pose2 pose;
-  bool found_odom = false;
-  for(size_t idx = 0; idx < pose_helpers_.size(); idx++)
+  if(pose_helpers_.find(base_frame_id) == pose_helpers_.end())
+    return;
+  else
   {
-    found_odom = pose_helpers_[idx]->getOdomPose(pose, scan->header.stamp, scan->header.frame_id);
-    if(found_odom)
-      break;
+    pose_helpers_[base_frame_id]->getOdomPose(pose, scan->header.stamp, base_frame_id);
   }
-  if(!found_odom)
-    return;
 
   // ensure the laser can be used
   LaserRangeFinder* laser = getLaser(scan);

slam_toolbox/src/slam_toolbox_sync.cpp

@@ -70,16 +70,12 @@ void SynchronousSlamToolbox::laserCallback(
 {
   // no odom info on any pose helper
   karto::Pose2 pose;
-  bool found_odom = false;
-  for(size_t idx = 0; idx < pose_helpers_.size(); idx++)
+  if(pose_helpers_.find(base_frame_id) == pose_helpers_.end())
+    return;
+  else
   {
-    // try compute odometry
-    found_odom = pose_helpers_[idx]->getOdomPose(pose, scan->header.stamp, base_frame_id);
-    if(found_odom)
-      break;
+    pose_helpers_[base_frame_id]->getOdomPose(pose, scan->header.stamp, base_frame_id);
   }
-  if(!found_odom)
-    return;
 
   // Add new sensor to laser ID map, and to laser assistants
   {