Laser frame no longer needs to be same as base frame (#2)

* Use laser frame to base frame map for lasers not at base frame

* Use references instead of copies

* Use laser frames for keys instead of base id

* No longer need to provide laser_frames as param in yaml

* Add mutex to protect m_laser_id_to_base_id_

* Add check to see if laser ID is in map

* Add check to see if laser assistant is in map

* Update async callback for multi robot laser frame

* Updated sync slam toolbox formatting

slam_toolbox/include/slam_toolbox/experimental/slam_toolbox_lifelong.hpp

@@ -42,7 +42,7 @@ class LifelongSlamToolbox : public SlamToolbox
 
 protected:
   virtual void laserCallback(
-    const sensor_msgs::LaserScan::ConstPtr& scan) override final;
+    const sensor_msgs::LaserScan::ConstPtr& scan, const std::string& base_frame_id) override final;
   virtual bool deserializePoseGraphCallback(
     slam_toolbox_msgs::DeserializePoseGraph::Request& req,
     slam_toolbox_msgs::DeserializePoseGraph::Response& resp) override final;

slam_toolbox/include/slam_toolbox/slam_toolbox_async.hpp

@@ -33,7 +33,7 @@ class AsynchronousSlamToolbox : public SlamToolbox
 
 protected:
   virtual void laserCallback(
-    const sensor_msgs::LaserScan::ConstPtr& scan) override final;
+    const sensor_msgs::LaserScan::ConstPtr& scan, const std::string& base_frame_id) override final;
   virtual bool deserializePoseGraphCallback(
     slam_toolbox_msgs::DeserializePoseGraph::Request& req,
     slam_toolbox_msgs::DeserializePoseGraph::Response& resp) override final;

slam_toolbox/include/slam_toolbox/slam_toolbox_common.hpp

@@ -70,7 +70,7 @@ class SlamToolbox
   void setROSInterfaces(ros::NodeHandle& node);
 
   // callbacks
-  virtual void laserCallback(const sensor_msgs::LaserScan::ConstPtr& scan) = 0;
+  virtual void laserCallback(const sensor_msgs::LaserScan::ConstPtr& scan, const std::string& base_frame_id) = 0;
   bool mapCallback(nav_msgs::GetMap::Request& req,
     nav_msgs::GetMap::Response& res);
   virtual bool serializePoseGraphCallback(slam_toolbox_msgs::SerializePoseGraph::Request& req,
@@ -123,7 +123,7 @@ class SlamToolbox
   std::unique_ptr<karto::Dataset> dataset_;
   std::map<std::string, laser_utils::LaserMetadata> lasers_;
   std::map<std::string, tf2::Transform> m_map_to_odoms_;
-  std::map<std::string, std::string> m_base_id_to_odom_id_;
+  std::map<std::string, std::string> m_base_id_to_odom_id_, m_laser_id_to_base_id_;
 
   // helpers
   std::map<std::string,std::unique_ptr<laser_utils::LaserAssistant>> laser_assistants_;
@@ -134,7 +134,7 @@ class SlamToolbox
 
   // Internal state
   std::vector<std::unique_ptr<boost::thread> > threads_;
-  boost::mutex map_to_odom_mutex_, smapper_mutex_, pose_mutex_;
+  boost::mutex map_to_odom_mutex_, smapper_mutex_, pose_mutex_, laser_id_map_mutex_;
   PausedState state_;
   nav_msgs::GetMap::Response map_;
   ProcessType processor_type_;

slam_toolbox/include/slam_toolbox/slam_toolbox_localization.hpp

@@ -35,7 +35,7 @@ class LocalizationSlamToolbox : public SlamToolbox
 
 protected:
   virtual void laserCallback(
-    const sensor_msgs::LaserScan::ConstPtr& scan) override final;
+    const sensor_msgs::LaserScan::ConstPtr& scan, const std::string& base_frame_id) override final;
   void localizePoseCallback(
     const geometry_msgs::PoseWithCovarianceStampedConstPtr& msg);
 

slam_toolbox/include/slam_toolbox/slam_toolbox_sync.hpp

@@ -33,7 +33,7 @@ class SynchronousSlamToolbox : public SlamToolbox
   void run();
 
 protected:
-  virtual void laserCallback(const sensor_msgs::LaserScan::ConstPtr& scan) override final;
+  virtual void laserCallback(const sensor_msgs::LaserScan::ConstPtr& scan, const std::string& base_frame_id) override final;
   bool clearQueueCallback(slam_toolbox_msgs::ClearQueue::Request& req, slam_toolbox_msgs::ClearQueue::Response& resp);
   virtual bool deserializePoseGraphCallback(slam_toolbox_msgs::DeserializePoseGraph::Request& req,
     slam_toolbox_msgs::DeserializePoseGraph::Response& resp) override final;

slam_toolbox/src/experimental/slam_toolbox_lifelong.cpp

@@ -66,7 +66,7 @@ LifelongSlamToolbox::LifelongSlamToolbox(ros::NodeHandle& nh)
 
 /*****************************************************************************/
 void LifelongSlamToolbox::laserCallback(
-  const sensor_msgs::LaserScan::ConstPtr& scan)
+  const sensor_msgs::LaserScan::ConstPtr& scan, const std::string& base_frame_id)
 /*****************************************************************************/
 {
   // no odom info on any pose helper

slam_toolbox/src/slam_toolbox_async.cpp

@@ -32,21 +32,31 @@ AsynchronousSlamToolbox::AsynchronousSlamToolbox(ros::NodeHandle& nh)
 
 /*****************************************************************************/
 void AsynchronousSlamToolbox::laserCallback(
-  const sensor_msgs::LaserScan::ConstPtr& scan)
+  const sensor_msgs::LaserScan::ConstPtr& scan, const std::string& base_frame_id)
 /*****************************************************************************/
 {
   // no odom info on any pose helper
   karto::Pose2 pose;
   bool found_odom = false;
   for(size_t idx = 0; idx < pose_helpers_.size(); idx++)
   {
-    found_odom = pose_helpers_[idx]->getOdomPose(pose, scan->header.stamp, scan->header.frame_id);
+    // try compute odometry
+    found_odom = pose_helpers_[idx]->getOdomPose(pose, scan->header.stamp, base_frame_id);
     if(found_odom)
       break;
   }
   if(!found_odom)
     return;
 
+  // Add new sensor to laser ID map, and to laser assistants
+  {
+    boost::mutex::scoped_lock l(laser_id_map_mutex_);
+    if(m_laser_id_to_base_id_.find(scan->header.frame_id) == m_laser_id_to_base_id_.end())
+    {
+      m_laser_id_to_base_id_[scan->header.frame_id] = base_frame_id;
+      laser_assistants_[scan->header.frame_id] = std::make_unique<laser_utils::LaserAssistant>(nh_, tf_.get(), base_frame_id);
+    }
+  }
   // ensure the laser can be used
   karto::LaserRangeFinder* laser = getLaser(scan);
 

slam_toolbox/src/slam_toolbox_common.cpp

@@ -46,16 +46,15 @@ SlamToolbox::SlamToolbox(ros::NodeHandle& nh)
     ROS_FATAL("[RoboSAR:slam_toolbox_common:SlamToolbox] base_frames_.size() != odom_frames_.size()");
   assert(base_frames_.size() == laser_topics_.size());
   assert(base_frames_.size() == odom_frames_.size());
-  // Create map to compute odom frame given base frame
+  // Setup map: base frame to odom frame
   for(size_t idx = 0; idx < base_frames_.size(); idx++)
   {
     m_base_id_to_odom_id_[base_frames_[idx]] = odom_frames_[idx];
   }
-  // Set up pose helpers and laser assistants for each robot
+  // 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]));
-    laser_assistants_[base_frames_[idx]] = std::make_unique<laser_utils::LaserAssistant>(nh_, tf_.get(), base_frames_[idx]); // Assumes base frame = laser frame
   }
   scan_holder_ = std::make_unique<laser_utils::ScanHolder>(lasers_);
   map_saver_ = std::make_unique<map_saver::MapSaver>(nh_, map_name_);
@@ -90,7 +89,6 @@ SlamToolbox::~SlamToolbox()
   for(size_t idx = 0; idx < pose_helpers_.size(); idx++)
   {
     pose_helpers_[idx].reset();
-    // laser_assistants_.reset();
   }
   for(std::map<std::string,std::unique_ptr<laser_utils::LaserAssistant>>::iterator it = laser_assistants_.begin(); it != laser_assistants_.end(); it++)
   {
@@ -189,7 +187,7 @@ void SlamToolbox::setROSInterfaces(ros::NodeHandle& node)
     ROS_INFO("Subscribing to scan: %s", laser_topics_[idx].c_str());
     scan_filter_subs_.push_back(std::make_unique<message_filters::Subscriber<sensor_msgs::LaserScan> >(node, laser_topics_[idx], 5));
     scan_filters_.push_back(std::make_unique<tf2_ros::MessageFilter<sensor_msgs::LaserScan> >(*scan_filter_subs_.back(), *tf_, odom_frames_[idx], 5, node));
-    scan_filters_.back()->registerCallback(boost::bind(&SlamToolbox::laserCallback, this, _1));
+    scan_filters_.back()->registerCallback(boost::bind(&SlamToolbox::laserCallback, this, _1, base_frames_[idx]));
   }
 }
 
@@ -331,12 +329,19 @@ karto::LaserRangeFinder* SlamToolbox::getLaser(const
   sensor_msgs::LaserScan::ConstPtr& scan)
 /*****************************************************************************/
 {
+  // Use laser scan ID to get base frame ID
   const std::string& frame = scan->header.frame_id;
   if(lasers_.find(frame) == lasers_.end())
   {
     try
     {
-      lasers_[frame] = laser_assistants_[frame]->toLaserMetadata(*scan);
+      std::map<std::string,std::unique_ptr<laser_utils::LaserAssistant>>::const_iterator it = laser_assistants_.find(frame);
+      if(it == laser_assistants_.end())
+      {
+        ROS_ERROR("Failed to get requested laser assistant, aborting initialization (%s)", frame.c_str());
+        return nullptr;
+      }
+      lasers_[frame] = it->second->toLaserMetadata(*scan);
       dataset_->Add(lasers_[frame].getLaser(), true);
     }
     catch (tf2::TransformException& e)
@@ -385,16 +390,27 @@ tf2::Stamped<tf2::Transform> SlamToolbox::setTransformFromPoses(
   const bool& update_reprocessing_transform)
 /*****************************************************************************/
 {
-  // Use base frame to look up odom frame
-  std::string odom_frame = m_base_id_to_odom_id_[header.frame_id];
+  tf2::Stamped<tf2::Transform> odom_to_map;
+  // Use laser frame to look up base and odom frame
+  std::string base_frame;
+  {
+    boost::mutex::scoped_lock l(laser_id_map_mutex_);
+    std::map<std::string, std::string>::const_iterator it = m_laser_id_to_base_id_.find(header.frame_id);
+    if(it == m_laser_id_to_base_id_.end())
+    {
+      ROS_ERROR("Requested laser frame ID not in map: %s", header.frame_id.c_str());
+      return odom_to_map;
+    }
+    base_frame = it->second;
+  }
+  const std::string& odom_frame = m_base_id_to_odom_id_[base_frame];
   // Compute the map->odom transform
   const ros::Time& t = header.stamp;
-  tf2::Stamped<tf2::Transform> odom_to_map;
   tf2::Quaternion q(0.,0.,0.,1.0);
   q.setRPY(0., 0., corrected_pose.GetHeading());
   tf2::Stamped<tf2::Transform> base_to_map(
     tf2::Transform(q, tf2::Vector3(corrected_pose.GetX(),
-    corrected_pose.GetY(), 0.0)).inverse(), t, header.frame_id); // Assumes base frame = laser frame
+    corrected_pose.GetY(), 0.0)).inverse(), t, base_frame);
   try
   {
     geometry_msgs::TransformStamped base_to_map_msg, odom_to_map_msg;
@@ -735,8 +751,14 @@ void SlamToolbox::loadSerializedPoseGraph(
         ROS_INFO("Got scan!");
         try
         {
-          lasers_[scan->header.frame_id] =
-            laser_assistants_[scan->header.frame_id]->toLaserMetadata(*scan);
+          const std::string& frame = scan->header.frame_id;
+          std::map<std::string,std::unique_ptr<laser_utils::LaserAssistant>>::const_iterator it = laser_assistants_.find(frame);
+          if(it == laser_assistants_.end())
+          {
+            ROS_ERROR("Failed to get requested laser assistant, aborting continue mapping (%s)", frame.c_str());
+            exit(-1);
+          }
+          lasers_[frame] = it->second->toLaserMetadata(*scan);
           break;
         }
         catch (tf2::TransformException& e)

slam_toolbox/src/slam_toolbox_localization.cpp

@@ -101,7 +101,7 @@ bool LocalizationSlamToolbox::deserializePoseGraphCallback(
 
 /*****************************************************************************/
 void LocalizationSlamToolbox::laserCallback(
-  const sensor_msgs::LaserScan::ConstPtr& scan)
+  const sensor_msgs::LaserScan::ConstPtr& scan, const std::string& base_frame_id)
 /*****************************************************************************/
 {
 // no odom info on any pose helper

slam_toolbox/src/slam_toolbox_sync.cpp

@@ -65,21 +65,31 @@ void SynchronousSlamToolbox::run()
 
 /*****************************************************************************/
 void SynchronousSlamToolbox::laserCallback(
-  const sensor_msgs::LaserScan::ConstPtr& scan)
+  const sensor_msgs::LaserScan::ConstPtr& scan, const std::string& base_frame_id)
 /*****************************************************************************/
 {
   // no odom info on any pose helper
   karto::Pose2 pose;
   bool found_odom = false;
   for(size_t idx = 0; idx < pose_helpers_.size(); idx++)
   {
-    found_odom = pose_helpers_[idx]->getOdomPose(pose, scan->header.stamp, scan->header.frame_id);
+    // try compute odometry
+    found_odom = pose_helpers_[idx]->getOdomPose(pose, scan->header.stamp, base_frame_id);
     if(found_odom)
       break;
   }
   if(!found_odom)
     return;
 
+  // Add new sensor to laser ID map, and to laser assistants
+  {
+    boost::mutex::scoped_lock l(laser_id_map_mutex_);
+    if(m_laser_id_to_base_id_.find(scan->header.frame_id) == m_laser_id_to_base_id_.end())
+    {
+      m_laser_id_to_base_id_[scan->header.frame_id] = base_frame_id;
+      laser_assistants_[scan->header.frame_id] = std::make_unique<laser_utils::LaserAssistant>(nh_, tf_.get(), base_frame_id);
+    }
+  }
   // ensure the laser can be used
   karto::LaserRangeFinder* laser = getLaser(scan);