Multi-robot SLAM

README.md

@@ -46,6 +46,17 @@ The video below was collected at [Circuit Launch](https://www.circuitlaunch.com/
 
 ![map_image](/images/circuit_launch.gif?raw=true "Map Image")
 
+# Multi-Robot SLAM
+SLAM Toolbox supports multi-robot SLAM. The robots must all start in the same location, facing the same way. The first robot to connect determines the world origin, and the relative starting points of the other robots are determined using laser scan matching. This allows all robots to add to a shared pose graph, which makes possible:
+- Localization of multiple robots
+- Mapping using information from all sensor readings
+- Loop closures across robots
+
+Currently, synchornous and asynchornous mapping is supported.
+
+![MRSLAM_gif](/images/mrslam_readme.gif?raw=true "Multi-Robot SLAM")
+
+`odom_frames`, `base_frames`, and `laser_topics` define odom frame, base frame, and laser topic for all robots. These params must all have the same order of robots, and must have the same number of elements. This means that each robot can only have one LIDAR.
 
 # 03/23/2021 Note On Serialized Files
 

slam_toolbox/config/mapper_params_online_async.yaml

@@ -7,17 +7,17 @@ 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
 # or at the dock, but they are mutually exclusive, if pose is given
 # will use pose
 #map_file_name: test_steve
-# map_start_pose: [0.0, 0.0, 0.0]
+#map_start_pose: [0.0, 0.0, 0.0]
 #map_start_at_dock: true
 
 debug_logging: false

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/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/get_pose_helper.hpp

@@ -37,8 +37,13 @@ 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)
   {
+    // 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/loop_closure_assistant.hpp

@@ -55,6 +55,7 @@ class LoopClosureAssistant
   bool interactiveModeCallback(slam_toolbox_msgs::ToggleInteractive::Request  &req, slam_toolbox_msgs::ToggleInteractive::Response &resp);
   void moveNode(const int& id, const Eigen::Vector3d& pose);
   void addMovedNodes(const int& id, Eigen::Vector3d vec);
+  std_msgs::ColorRGBA createNewColor();
 
   std::unique_ptr<tf2_ros::TransformBroadcaster> tfB_;
   laser_utils::ScanHolder* scan_holder_;
@@ -72,6 +73,7 @@ class LoopClosureAssistant
   std::string map_frame_;
   PausedState& state_;
   ProcessType& processor_type_;
+  std::map<karto::Name, std_msgs::ColorRGBA> m_sensor_name_to_color_;
 };
 
 }  // end namespace

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

@@ -45,6 +45,7 @@
 #include <fstream>
 #include <boost/thread.hpp>
 #include <sys/resource.h>
+#include <assert.h>
 
 namespace slam_toolbox
 {
@@ -69,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,
@@ -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,18 +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_;
+  std::map<std::string, std::string> m_base_id_to_odom_id_, m_laser_id_to_base_id_;
 
   // 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::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_;
 
   // Internal state
   std::vector<std::unique_ptr<boost::thread> > threads_;
-  tf2::Transform map_to_odom_;
-  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/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

@@ -66,14 +66,16 @@ 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
-  Pose2 pose;
-  if(!pose_helper_->getOdomPose(pose, scan->header.stamp))
-  {
+  // no odom info on any pose helper
+  karto::Pose2 pose;
+  if(pose_helpers_.find(base_frame_id) == pose_helpers_.end())
     return;
+  else
+  {
+    pose_helpers_[base_frame_id]->getOdomPose(pose, scan->header.stamp, base_frame_id);
   }
 
   // ensure the laser can be used

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/loop_closure_assistant.cpp

@@ -171,6 +171,12 @@ void LoopClosureAssistant::publishGraph()
          inner_it != outer_it->second.end(); ++inner_it)
     {
       karto::LocalizedRangeScan * scan = inner_it->second->GetObject();
+      // Update color map and marker color
+      karto::Name cur_sensor = scan->GetSensorName();
+      std::map<karto::Name, std_msgs::ColorRGBA>::const_iterator map_it = m_sensor_name_to_color_.find(cur_sensor);
+      if(map_it == m_sensor_name_to_color_.end())
+        m_sensor_name_to_color_[cur_sensor] = createNewColor();
+      vertex_marker.color = m_sensor_name_to_color_[cur_sensor];
 
       vertex_marker.pose.position.x = scan->GetCorrectedPose().GetX();
       vertex_marker.pose.position.y = scan->GetCorrectedPose().GetY();
@@ -211,6 +217,20 @@ void LoopClosureAssistant::publishGraph()
       const karto::Edge<karto::LocalizedRangeScan> * edge = *it;
       karto::LocalizedRangeScan * source_scan = edge->GetSource()->GetObject();
       karto::LocalizedRangeScan * target_scan = edge->GetTarget()->GetObject();
+      // Set edge color based on if edge is for one robot, or two
+      karto::Name source_sensor = source_scan->GetSensorName();
+      if(source_sensor == target_scan->GetSensorName())
+      {
+        edge_marker.color = m_sensor_name_to_color_[source_sensor];
+        edge_marker.color.a = 0.5;
+      }
+      else
+      {
+        edge_marker.color.r = 1.0;
+        edge_marker.color.g = 0.0;
+        edge_marker.color.b = 0.0;
+        edge_marker.color.a = 0.5;
+      }
       const karto::Pose2 source_pose = source_scan->GetCorrectedPose();
       const karto::Pose2 target_pose = target_scan->GetCorrectedPose();
 
@@ -362,4 +382,16 @@ void LoopClosureAssistant::addMovedNodes(const int& id, Eigen::Vector3d vec)
   moved_nodes_[id] = vec;
 }
 
+/*****************************************************************************/
+std_msgs::ColorRGBA LoopClosureAssistant::createNewColor()
+{
+  std_msgs::ColorRGBA new_color;
+  new_color.r = (float)(rand() % 1000) / 1000;
+  new_color.g = (float)(rand() % 1000) / 1000;
+  new_color.b = (float)(rand() % 1000) / 1000;
+  new_color.a = 1;
+  return new_color;
+}
+/*****************************************************************************/
+
 } // end namespace

slam_toolbox/src/slam_toolbox_async.cpp

@@ -32,16 +32,27 @@ 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
+  // no odom info on any pose helper
   karto::Pose2 pose;
-  if(!pose_helper_->getOdomPose(pose, scan->header.stamp))
-  {
+  if(pose_helpers_.find(base_frame_id) == pose_helpers_.end())
     return;
+  else
+  {
+    pose_helpers_[base_frame_id]->getOdomPose(pose, scan->header.stamp, base_frame_id);
   }
 
+  // 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);