Multi-robot SLAM

slam_toolbox/config/mapper_params_online_sync.yaml

@@ -7,10 +7,10 @@ ceres_dogleg_type: TRADITIONAL_DOGLEG
 ceres_loss_function: None
 
 # ROS Parameters
-odom_frame: odom
 map_frame: map
-base_frame: base_footprint
-scan_topic: /scan
+odom_frames: ["robot1/odom", "robot2/odom"]
+base_frames: ["robot1/base_footprint", "robot2/base_footprint"]
+laser_topics: ["robot1/scan, robot2/scan"]
 mode: mapping #localization
 
 # if you'd like to immediately start continuing a map at a given pose

slam_toolbox/include/slam_toolbox/get_pose_helper.hpp

@@ -37,8 +37,15 @@ class GetPoseHelper
   {
   };
 
-  bool getOdomPose(karto::Pose2& karto_pose, const ros::Time& t)
+  bool getOdomPose(karto::Pose2& karto_pose,
+    const ros::Time& t,
+    std::string ref_frame = std::string(""))
   {
+    // Only succeed if ref_frame matches base_frame_; otherwise using wrong pose helper
+    if(ref_frame != base_frame_)
+    {
+      return false;
+    }
     geometry_msgs::TransformStamped base_ident, odom_pose;
     base_ident.header.stamp = t;
     base_ident.header.frame_id = base_frame_;

slam_toolbox/include/slam_toolbox/slam_toolbox_common.hpp

@@ -45,6 +45,7 @@
 #include <fstream>
 #include <boost/thread.hpp>
 #include <sys/resource.h>
+#include <assert.h>
 
 namespace slam_toolbox
 {
@@ -86,7 +87,7 @@ class SlamToolbox
   karto::LocalizedRangeScan* addScan(karto::LaserRangeFinder* laser, PosedScan& scanWPose);
   bool updateMap();
   tf2::Stamped<tf2::Transform> setTransformFromPoses(const karto::Pose2& pose,
-    const karto::Pose2& karto_pose, const ros::Time& t, const bool& update_reprocessing_transform);
+    const karto::Pose2& karto_pose, const std_msgs::Header& header, const bool& update_reprocessing_transform);
   karto::LocalizedRangeScan* getLocalizedRangeScan(karto::LaserRangeFinder* laser,
     const sensor_msgs::LaserScan::ConstPtr& scan,
     karto::Pose2& karto_pose);
@@ -103,13 +104,14 @@ class SlamToolbox
   std::unique_ptr<tf2_ros::Buffer> tf_;
   std::unique_ptr<tf2_ros::TransformListener> tfL_;
   std::unique_ptr<tf2_ros::TransformBroadcaster> tfB_;
-  std::unique_ptr<message_filters::Subscriber<sensor_msgs::LaserScan> > scan_filter_sub_;
-  std::unique_ptr<tf2_ros::MessageFilter<sensor_msgs::LaserScan> > scan_filter_;
+  std::vector<std::unique_ptr<message_filters::Subscriber<sensor_msgs::LaserScan> > > scan_filter_subs_;
+  std::vector<std::unique_ptr<tf2_ros::MessageFilter<sensor_msgs::LaserScan> > > scan_filters_;
   ros::Publisher sst_, sstm_;
   ros::ServiceServer ssMap_, ssPauseMeasurements_, ssSerialize_, ssDesserialize_;
 
   // Storage for ROS parameters
-  std::string odom_frame_, map_frame_, base_frame_, map_name_, scan_topic_;
+  std::string map_frame_, map_name_;
+  std::vector<std::string> odom_frames_, base_frames_, laser_topics_;
   ros::Duration transform_timeout_, tf_buffer_dur_, minimum_time_interval_;
   int throttle_scans_;
 
@@ -120,17 +122,19 @@ class SlamToolbox
   std::unique_ptr<mapper_utils::SMapper> smapper_;
   std::unique_ptr<karto::Dataset> dataset_;
   std::map<std::string, laser_utils::LaserMetadata> lasers_;
+  std::map<std::string, tf2::Transform> m_map_to_odoms_;
 
   // helpers
-  std::unique_ptr<laser_utils::LaserAssistant> laser_assistant_;
-  std::unique_ptr<pose_utils::GetPoseHelper> pose_helper_;
+  std::map<std::string,std::unique_ptr<laser_utils::LaserAssistant>> laser_assistants_;
+  std::vector<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_;
 
   // Internal state
   std::vector<std::unique_ptr<boost::thread> > threads_;
   tf2::Transform map_to_odom_;
+  std::string map_to_odom_child_frame_id_;
   boost::mutex map_to_odom_mutex_, smapper_mutex_, pose_mutex_;
   PausedState state_;
   nav_msgs::GetMap::Response map_;

slam_toolbox/lib/karto_sdk/src/Mapper.cpp

@@ -1513,7 +1513,7 @@ namespace karto
         kt_double response = m_pMapper->m_pSequentialScanMatcher->MatchScan<LocalizedRangeScanMap>(pScan,
                                                                   pSensorManager->GetScans(rCandidateSensorName),
                                                                   bestPose, covariance);
-        LinkScans(pScan, pSensorManager->GetScan(rCandidateSensorName, 0), bestPose, covariance);
+        LinkScans(pSensorManager->GetScan(rCandidateSensorName, 0), pScan, bestPose, covariance);
 
         // only add to means and covariances if response was high "enough"
         if (response > m_pMapper->m_pLinkMatchMinimumResponseFine->GetValue())

slam_toolbox/src/experimental/slam_toolbox_lifelong.cpp

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

slam_toolbox/src/laser_utils.cpp

@@ -92,9 +92,10 @@ LaserMetadata LaserAssistant::toLaserMetadata(sensor_msgs::LaserScan scan)
 
 karto::LaserRangeFinder* LaserAssistant::makeLaser(const double& mountingYaw)
 {
+  const std::string name = scan_.header.frame_id;
   karto::LaserRangeFinder* laser = 
     karto::LaserRangeFinder::CreateLaserRangeFinder(
-    karto::LaserRangeFinder_Custom, karto::Name("Custom Described Lidar"));
+    karto::LaserRangeFinder_Custom, karto::Name(name.c_str())); // Sets name to laser frame
   laser->SetOffsetPose(karto::Pose2(laser_pose_.transform.translation.x,
     laser_pose_.transform.translation.y, mountingYaw));
   laser->SetMinimumRange(scan_.range_min);

slam_toolbox/src/slam_toolbox_async.cpp

@@ -35,12 +35,17 @@ void AsynchronousSlamToolbox::laserCallback(
   const sensor_msgs::LaserScan::ConstPtr& scan)
 /*****************************************************************************/
 {
-  // no odom info
+  // no odom info on any pose helper
   karto::Pose2 pose;
-  if(!pose_helper_->getOdomPose(pose, scan->header.stamp))
+  bool found_odom = false;
+  for(size_t idx = 0; idx < pose_helpers_.size(); idx++)
   {
-    return;
+    found_odom = pose_helpers_[idx]->getOdomPose(pose, scan->header.stamp, scan->header.frame_id);
+    if(found_odom)
+      break;
   }
+  if(!found_odom)
+    return;
 
   // ensure the laser can be used
   karto::LaserRangeFinder* laser = getLaser(scan);

slam_toolbox/src/slam_toolbox_common.cpp

@@ -40,10 +40,18 @@ SlamToolbox::SlamToolbox(ros::NodeHandle& nh)
   setROSInterfaces(nh_);
   setSolver(nh_);
 
-  laser_assistant_ = std::make_unique<laser_utils::LaserAssistant>(
-    nh_, tf_.get(), base_frame_);
-  pose_helper_ = std::make_unique<pose_utils::GetPoseHelper>(
-    tf_.get(), base_frame_, odom_frame_);
+  if(base_frames_.size() != laser_topics_.size())
+    ROS_FATAL("[RoboSAR:slam_toolbox_common:SlamToolbox] base_frames_.size() != laser_topics_.size()");
+  if(base_frames_.size() != odom_frames_.size())
+    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());
+
+  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_);
   closure_assistant_ =
@@ -74,8 +82,15 @@ SlamToolbox::~SlamToolbox()
   dataset_.reset();
   closure_assistant_.reset();
   map_saver_.reset();
-  pose_helper_.reset();
-  laser_assistant_.reset();
+  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++)
+  {
+    it->second.reset();
+  }
   scan_holder_.reset();
 }
 
@@ -109,12 +124,24 @@ void SlamToolbox::setParams(ros::NodeHandle& private_nh)
 /*****************************************************************************/
 {
   map_to_odom_.setIdentity();
-  private_nh.param("odom_frame", odom_frame_, std::string("odom"));
   private_nh.param("map_frame", map_frame_, std::string("map"));
-  private_nh.param("base_frame", base_frame_, std::string("base_footprint"));
   private_nh.param("resolution", resolution_, 0.05);
   private_nh.param("map_name", map_name_, std::string("/map"));
-  private_nh.param("scan_topic", scan_topic_, std::string("/scan"));
+  std::vector<std::string> default_laser = {"/scan"};
+  if (!private_nh.getParam("laser_topics", laser_topics_))
+  {
+    laser_topics_ = default_laser;
+  }
+  std::vector<std::string> default_base_frame = {"base_footprint"};
+  if (!private_nh.getParam("base_frames", base_frames_))
+  {
+    base_frames_ = default_base_frame;
+  }
+  std::vector<std::string> default_odom_frame = {"odom"};
+  if (!private_nh.getParam("odom_frames", odom_frames_))
+  {
+    odom_frames_ = default_odom_frame;
+  }
   private_nh.param("throttle_scans", throttle_scans_, 1);
   private_nh.param("enable_interactive_mode", enable_interactive_mode_, false);
 
@@ -153,9 +180,13 @@ void SlamToolbox::setROSInterfaces(ros::NodeHandle& node)
   ssPauseMeasurements_ = node.advertiseService("pause_new_measurements", &SlamToolbox::pauseNewMeasurementsCallback, this);
   ssSerialize_ = node.advertiseService("serialize_map", &SlamToolbox::serializePoseGraphCallback, this);
   ssDesserialize_ = node.advertiseService("deserialize_map", &SlamToolbox::deserializePoseGraphCallback, this);
-  scan_filter_sub_ = std::make_unique<message_filters::Subscriber<sensor_msgs::LaserScan> >(node, scan_topic_, 5);
-  scan_filter_ = std::make_unique<tf2_ros::MessageFilter<sensor_msgs::LaserScan> >(*scan_filter_sub_, *tf_, odom_frame_, 5, node);
-  scan_filter_->registerCallback(boost::bind(&SlamToolbox::laserCallback, this, _1));
+  for(size_t idx = 0; idx < laser_topics_.size(); idx++)
+  {
+    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));
+  }
 }
 
 /*****************************************************************************/
@@ -172,12 +203,20 @@ void SlamToolbox::publishTransformLoop(const double& transform_publish_period)
   {
     {
       boost::mutex::scoped_lock lock(map_to_odom_mutex_);
-      geometry_msgs::TransformStamped msg;
-      tf2::convert(map_to_odom_, msg.transform);
-      msg.child_frame_id = odom_frame_;
-      msg.header.frame_id = map_frame_;
-      msg.header.stamp = ros::Time::now() + transform_timeout_;
-      tfB_->sendTransform(msg);
+      // Update with / add latest transform
+      if(map_to_odom_child_frame_id_.length() > 0)
+        m_map_to_odoms_[map_to_odom_child_frame_id_] = map_to_odom_;
+      // Publish all past and current transforms so none of them go stale
+      std::map<std::string, tf2::Transform>::iterator iter;
+      for(iter = m_map_to_odoms_.begin(); iter != m_map_to_odoms_.end(); iter++)
+      {
+        geometry_msgs::TransformStamped msg;
+        tf2::convert(iter->second, msg.transform);
+        msg.child_frame_id = iter->first;
+        msg.header.frame_id = map_frame_;
+        msg.header.stamp = ros::Time::now() + transform_timeout_;
+        tfB_->sendTransform(msg);
+      }
     }
     r.sleep();
   }
@@ -291,7 +330,7 @@ karto::LaserRangeFinder* SlamToolbox::getLaser(const
   {
     try
     {
-      lasers_[frame] = laser_assistant_->toLaserMetadata(*scan);
+      lasers_[frame] = laser_assistants_[frame]->toLaserMetadata(*scan);
       dataset_->Add(lasers_[frame].getLaser(), true);
     }
     catch (tf2::TransformException& e)
@@ -336,22 +375,26 @@ bool SlamToolbox::updateMap()
 tf2::Stamped<tf2::Transform> SlamToolbox::setTransformFromPoses(
   const karto::Pose2& corrected_pose,
   const karto::Pose2& karto_pose,
-  const ros::Time& t,
+  const std_msgs::Header& header,
   const bool& update_reprocessing_transform)
 /*****************************************************************************/
 {
+  // Turn base frame into odom frame
+  std::string robot_name = header.frame_id.substr(0, header.frame_id.find("/"));
+  std::string odom_frame = robot_name + "/" + "odom"; // TODO: Make not hard coded
   // 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, base_frame_);
+    corrected_pose.GetY(), 0.0)).inverse(), t, header.frame_id); // Assumes base frame = laser frame
   try
   {
     geometry_msgs::TransformStamped base_to_map_msg, odom_to_map_msg;
     tf2::convert(base_to_map, base_to_map_msg);
-    odom_to_map_msg = tf_->transform(base_to_map_msg, odom_frame_);
+    odom_to_map_msg = tf_->transform(base_to_map_msg, odom_frame);
     tf2::convert(odom_to_map_msg, odom_to_map);
   }
   catch(tf2::TransformException& e)
@@ -379,6 +422,7 @@ tf2::Stamped<tf2::Transform> SlamToolbox::setTransformFromPoses(
   boost::mutex::scoped_lock lock(map_to_odom_mutex_);
   map_to_odom_ = tf2::Transform(tf2::Quaternion( odom_to_map.getRotation() ),
     tf2::Vector3( odom_to_map.getOrigin() ) ).inverse();
+  map_to_odom_child_frame_id_ = odom_frame;
 
   return odom_to_map;
 }
@@ -413,17 +457,26 @@ bool SlamToolbox::shouldProcessScan(
   const karto::Pose2& pose)
 /*****************************************************************************/
 {
-  static karto::Pose2 last_pose;
-  static ros::Time last_scan_time = ros::Time(0.);
+  static std::vector<std::string> scan_frame_ids;
+  static std::map<std::string, karto::Pose2> last_poses;
+  static std::map<std::string, ros::Time> last_scan_times;
   static double min_dist2 =
     smapper_->getMapper()->getParamMinimumTravelDistance() *
     smapper_->getMapper()->getParamMinimumTravelDistance();
-
+  // Check if frame_id of current scan is new
+  bool new_scan_frame_id = false;
+  std::string cur_frame_id = scan->header.frame_id;
+  if (std::find(scan_frame_ids.begin(), scan_frame_ids.end(), cur_frame_id) == scan_frame_ids.end()){
+    // New scan
+    new_scan_frame_id = true;
+    scan_frame_ids.push_back(cur_frame_id);
+    last_scan_times[cur_frame_id] = ros::Time(0.);
+  }
   // we give it a pass on the first measurement to get the ball rolling
-  if (first_measurement_)
+  if (first_measurement_ || new_scan_frame_id)
   {
-    last_scan_time = scan->header.stamp;
-    last_pose = pose;
+    last_scan_times[cur_frame_id] = scan->header.stamp;
+    last_poses[cur_frame_id] = pose;
     first_measurement_ = false;
     return true;
   }
@@ -441,20 +494,20 @@ bool SlamToolbox::shouldProcessScan(
   }
 
   // not enough time
-  if (scan->header.stamp - last_scan_time < minimum_time_interval_)
+  if (scan->header.stamp - last_scan_times[cur_frame_id] < minimum_time_interval_)
   {
     return false;
   }
 
   // check moved enough, within 10% for correction error
-  const double dist2 = last_pose.SquaredDistance(pose);
+  const double dist2 = last_poses[cur_frame_id].SquaredDistance(pose);
   if(dist2 < 0.8 * min_dist2 || scan->header.seq < 5)
   {
     return false;
   }
 
-  last_pose = pose;
-  last_scan_time = scan->header.stamp; 
+  last_poses[cur_frame_id] = pose;
+  last_scan_times[cur_frame_id] = scan->header.stamp; 
 
   return true;
 }
@@ -525,7 +578,7 @@ karto::LocalizedRangeScan* SlamToolbox::addScan(
     }
 
     setTransformFromPoses(range_scan->GetCorrectedPose(), karto_pose,
-      scan->header.stamp, update_reprocessing_transform);
+      scan->header, update_reprocessing_transform);
     dataset_->Add(range_scan);
   }
   else
@@ -670,14 +723,14 @@ void SlamToolbox::loadSerializedPoseGraph(
       ROS_INFO("Waiting for incoming scan to get metadata...");
       boost::shared_ptr<sensor_msgs::LaserScan const> scan =
         ros::topic::waitForMessage<sensor_msgs::LaserScan>(
-        scan_topic_, ros::Duration(1.0));
+        laser_topics_.front(), ros::Duration(1.0));
       if (scan)
       {
         ROS_INFO("Got scan!");
         try
         {
           lasers_[scan->header.frame_id] =
-            laser_assistant_->toLaserMetadata(*scan);
+            laser_assistants_[scan->header.frame_id]->toLaserMetadata(*scan);
           break;
         }
         catch (tf2::TransformException& e)