Multi-Robot SLAM in ROS 2

CMakeLists.txt

@@ -64,10 +64,13 @@ set(dependencies
 
 set(libraries
     toolbox_common
+    toolbox_common_multirobot
     SlamToolboxPlugin
     ceres_solver_plugin
     async_slam_toolbox
+    async_slam_toolbox_multirobot
     sync_slam_toolbox
+    sync_slam_toolbox_multirobot
     localization_slam_toolbox
     lifelong_slam_toolbox
     map_and_localization_slam_toolbox
@@ -149,17 +152,35 @@ target_link_libraries(toolbox_common kartoSlamToolbox ${Boost_LIBRARIES})
 rosidl_get_typesupport_target(cpp_typesupport_target ${PROJECT_NAME} "rosidl_typesupport_cpp")
 target_link_libraries(toolbox_common "${cpp_typesupport_target}")
 
+add_library(toolbox_common_multirobot src/multirobot/slam_toolbox_common_multirobot.cpp src/map_saver.cpp src/multirobot/loop_closure_assistant_multirobot.cpp src/laser_utils.cpp src/slam_mapper.cpp)
+ament_target_dependencies(toolbox_common_multirobot
+  ${dependencies}
+)
+target_link_libraries(toolbox_common_multirobot kartoSlamToolbox ${Boost_LIBRARIES})
+rosidl_get_typesupport_target(cpp_typesupport_target ${PROJECT_NAME} "rosidl_typesupport_cpp")
+target_link_libraries(toolbox_common_multirobot "${cpp_typesupport_target}")
+
 #### Mapping executibles
 add_library(async_slam_toolbox src/slam_toolbox_async.cpp)
 target_link_libraries(async_slam_toolbox toolbox_common kartoSlamToolbox ${Boost_LIBRARIES})
 add_executable(async_slam_toolbox_node src/slam_toolbox_async_node.cpp)
 target_link_libraries(async_slam_toolbox_node async_slam_toolbox)
 
+add_library(async_slam_toolbox_multirobot src/multirobot/slam_toolbox_async_multirobot.cpp)
+target_link_libraries(async_slam_toolbox_multirobot toolbox_common_multirobot kartoSlamToolbox ${Boost_LIBRARIES})
+add_executable(async_slam_toolbox_multirobot_node src/multirobot/slam_toolbox_async_multirobot_node.cpp)
+target_link_libraries(async_slam_toolbox_multirobot_node async_slam_toolbox_multirobot)
+
 add_library(sync_slam_toolbox src/slam_toolbox_sync.cpp)
 target_link_libraries(sync_slam_toolbox toolbox_common kartoSlamToolbox ${Boost_LIBRARIES})
 add_executable(sync_slam_toolbox_node src/slam_toolbox_sync_node.cpp)
 target_link_libraries(sync_slam_toolbox_node sync_slam_toolbox)
 
+add_library(sync_slam_toolbox_multirobot src/multirobot/slam_toolbox_sync_multirobot.cpp)
+target_link_libraries(sync_slam_toolbox_multirobot toolbox_common_multirobot kartoSlamToolbox ${Boost_LIBRARIES})
+add_executable(sync_slam_toolbox_multirobot_node src/multirobot/slam_toolbox_sync_multirobot_node.cpp)
+target_link_libraries(sync_slam_toolbox_multirobot_node sync_slam_toolbox_multirobot)
+
 add_library(localization_slam_toolbox src/slam_toolbox_localization.cpp)
 target_link_libraries(localization_slam_toolbox toolbox_common kartoSlamToolbox ${Boost_LIBRARIES})
 add_executable(localization_slam_toolbox_node src/slam_toolbox_localization_node.cpp)
@@ -190,14 +211,18 @@ target_link_libraries(merge_maps_kinematic kartoSlamToolbox toolbox_common)
 #endif()
 
 rclcpp_components_register_nodes(async_slam_toolbox "slam_toolbox::AsynchronousSlamToolbox")
+rclcpp_components_register_nodes(async_slam_toolbox_multirobot "slam_toolbox::AsynchronousSlamToolboxMultirobot")
 rclcpp_components_register_nodes(sync_slam_toolbox "slam_toolbox::SynchronousSlamToolbox")
+rclcpp_components_register_nodes(sync_slam_toolbox_multirobot "slam_toolbox::SynchronousSlamToolboxMultirobot")
 rclcpp_components_register_nodes(localization_slam_toolbox "slam_toolbox::LocalizationSlamToolbox")
 rclcpp_components_register_nodes(lifelong_slam_toolbox "slam_toolbox::LifelongSlamToolbox")
 rclcpp_components_register_nodes(map_and_localization_slam_toolbox "slam_toolbox::MapAndLocalizationSlamToolbox")
 
 #### Install
 install(TARGETS async_slam_toolbox_node
+                async_slam_toolbox_multirobot_node
                 sync_slam_toolbox_node
+                sync_slam_toolbox_multirobot_node
                 localization_slam_toolbox_node
                 map_and_localization_slam_toolbox_node
                 merge_maps_kinematic

README.md

@@ -162,6 +162,28 @@ It can map _very_ large spaces with reasonable CPU and memory consumption. My de
 
 You can get away without a loss function if your odometry is good (i.e. likelihood for outliers is extremely low). If you have an abnormal application or expect wheel slippage, I might recommend a `HuberLoss` function, which is a really good catch-all loss function if you're looking for a place to start. All these options and more are available from the ROS parameter server.
 
+# Multi-Robot SLAM
+
+SLAM Toolbox supports multi-robot SLAM.
+
+For operation, all robots must be part of the same transformation tree and provide accurate transformations between their base link frames and laser scan frames. They must start close to each other; the first robot to connect establishes the map frame origin and the relative starting poses of the other robots are determined using laser scan matching from the SLAM Toolbox solver. This setup enables all robots to contribute to a shared pose graph, making possible:
+- Localization of multiple robots
+- Mapping using information from all available sensors
+- Loop closure across paths from multiple robots
+- Robust and comprehensive map creation through collaborative SLAM
+
+![two_robot_slam_toolbox_gif](/images/multirobot/two_robot_slam_toolbox.gif?raw=true)
+
+Currently, both synchronous and asynchronous mapping is supported. Parameters `odom_frames`, `base_frames`, and `laser_topics` define the respective information for each robot. These parameters must match in order and number of elements, ensuring each robot is configured with a single LiDAR. On the other hand, parameter `map_frame` remains singular and defines the global map frame for all robots.
+
+Pose graph colors are randomly assigned for each robot (nodes and edges). Edges between graphs of different robots are solid blue.
+
+![pose_graph_colors](/images/multirobot/pose_graph_colors.png)
+
+Source code and launch files are provided separately from the default SLAM Toolbox files. These can be found in dedicated multirobot directories.
+
+A sample configuration file can be found [here](config/mapper_params_online_multirobot.yaml).
+
 # API
 
 The following are the services/topics that are exposed for use. See the rviz plugin for an implementation of their use. 

config/mapper_params_online_multirobot.yaml

@@ -0,0 +1,75 @@
+slam_toolbox:
+  ros__parameters:
+
+    # Plugin params
+    solver_plugin: solver_plugins::CeresSolver
+    ceres_linear_solver: SPARSE_NORMAL_CHOLESKY
+    ceres_preconditioner: SCHUR_JACOBI
+    ceres_trust_strategy: LEVENBERG_MARQUARDT
+    ceres_dogleg_type: TRADITIONAL_DOGLEG
+    ceres_loss_function: None
+
+    # ROS Parameters
+    odom_frames: ["robot0/odom", "robot1/odom"]
+    map_frame: map
+    base_frames: ["robot0/base_footprint", "robot1/base_footprint"]
+    scan_topics: ["robot0/scan", "robot1/scan"]
+    use_map_saver: true
+    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_at_dock: true
+
+    debug_logging: false
+    throttle_scans: 1
+    transform_publish_period: 0.02 #if 0 never publishes odometry
+    map_update_interval: 5.0
+    resolution: 0.05
+    min_laser_range: 0.0 #for rastering images
+    max_laser_range: 20.0 #for rastering images
+    minimum_time_interval: 0.5
+    transform_timeout: 0.2
+    tf_buffer_duration: 30.
+    stack_size_to_use: 40000000 #// program needs a larger stack size to serialize large maps
+    enable_interactive_mode: true
+
+    # General Parameters
+    use_scan_matching: true
+    use_scan_barycenter: true
+    minimum_travel_distance: 0.5
+    minimum_travel_heading: 0.5
+    scan_buffer_size: 10
+    scan_buffer_maximum_scan_distance: 10.0
+    link_match_minimum_response_fine: 0.1  
+    link_scan_maximum_distance: 1.5
+    loop_search_maximum_distance: 3.0
+    do_loop_closing: true 
+    loop_match_minimum_chain_size: 10
+    loop_match_maximum_variance_coarse: 3.0
+    loop_match_minimum_response_coarse: 0.35
+    loop_match_minimum_response_fine: 0.45
+
+    # Correlation Parameters - Correlation Parameters
+    correlation_search_space_dimension: 0.5
+    correlation_search_space_resolution: 0.01
+    correlation_search_space_smear_deviation: 0.1 
+
+    # Correlation Parameters - Loop Closure Parameters
+    loop_search_space_dimension: 8.0
+    loop_search_space_resolution: 0.05
+    loop_search_space_smear_deviation: 0.03
+
+    # Scan Matcher Parameters
+    distance_variance_penalty: 0.5
+    angle_variance_penalty: 1.0
+
+    fine_search_angle_offset: 0.00349
+    coarse_search_angle_offset: 0.349
+    coarse_angle_resolution: 0.0349
+    minimum_angle_penalty: 0.9
+    minimum_distance_penalty: 0.5
+    use_response_expansion: true

include/slam_toolbox/multirobot/get_pose_helper_multirobot.hpp

@@ -0,0 +1,74 @@
+/*
+ * snap_utils
+ * Copyright (c) 2019, Samsung Research America
+ * Copyright Work Modifications (c) 2024, Daniel I. Meza
+ *
+ * THE WORK (AS DEFINED BELOW) IS PROVIDED UNDER THE TERMS OF THIS CREATIVE
+ * COMMONS PUBLIC LICENSE ("CCPL" OR "LICENSE"). THE WORK IS PROTECTED BY
+ * COPYRIGHT AND/OR OTHER APPLICABLE LAW. ANY USE OF THE WORK OTHER THAN AS
+ * AUTHORIZED UNDER THIS LICENSE OR COPYRIGHT LAW IS PROHIBITED.
+ *
+ * BY EXERCISING ANY RIGHTS TO THE WORK PROVIDED HERE, YOU ACCEPT AND AGREE TO
+ * BE BOUND BY THE TERMS OF THIS LICENSE. THE LICENSOR GRANTS YOU THE RIGHTS
+ * CONTAINED HERE IN CONSIDERATION OF YOUR ACCEPTANCE OF SUCH TERMS AND
+ * CONDITIONS.
+ *
+ */
+
+/* Author: Steven Macenski */
+
+#ifndef SLAM_TOOLBOX__GET_POSE_HELPER_MULTIPLE_HPP_
+#define SLAM_TOOLBOX__GET_POSE_HELPER_MULTIPLE_HPP_
+
+#include <string>
+#include "geometry_msgs/msg/pose_with_covariance_stamped.hpp"
+#include "slam_toolbox/toolbox_types.hpp"
+#include "karto_sdk/Mapper.h"
+
+namespace pose_utils
+{
+
+// helper to get the robots position
+class GetPoseHelper
+{
+public:
+  GetPoseHelper(
+    tf2_ros::Buffer * tf,
+    const std::string & base_frame,
+    const std::string & odom_frame)
+  : tf_(tf), base_frame_(base_frame), odom_frame_(odom_frame)
+  {
+  }
+
+  bool getOdomPose(karto::Pose2 & karto_pose, const rclcpp::Time & t, std::string ref_frame)
+  {
+    // Ensure the right pose helper is being called
+    if (ref_frame != base_frame_) {
+      return false;
+    }
+    geometry_msgs::msg::TransformStamped base_ident, odom_pose;
+    base_ident.header.stamp = t;
+    base_ident.header.frame_id = base_frame_;
+    base_ident.transform.rotation.w = 1.0;
+
+    try {
+      odom_pose = tf_->transform(base_ident, odom_frame_);
+    } catch (tf2::TransformException & e) {
+      return false;
+    }
+
+    const double yaw = tf2::getYaw(odom_pose.transform.rotation);
+    karto_pose = karto::Pose2(odom_pose.transform.translation.x,
+        odom_pose.transform.translation.y, yaw);
+
+    return true;
+  }
+
+private:
+  tf2_ros::Buffer * tf_;
+  std::string base_frame_, odom_frame_;
+};
+
+}  // namespace pose_utils
+
+#endif  // SLAM_TOOLBOX__GET_POSE_HELPER_MULTIROBOT_HPP_

include/slam_toolbox/multirobot/loop_closure_assistant_multirobot.hpp

@@ -0,0 +1,101 @@
+/*
+ * loop_closure_assistant
+ * Copyright (c) 2019, Samsung Research America
+ * Copyright Work Modifications (c) 2024, Daniel I. Meza
+ *
+ * THE WORK (AS DEFINED BELOW) IS PROVIDED UNDER THE TERMS OF THIS CREATIVE
+ * COMMONS PUBLIC LICENSE ("CCPL" OR "LICENSE"). THE WORK IS PROTECTED BY
+ * COPYRIGHT AND/OR OTHER APPLICABLE LAW. ANY USE OF THE WORK OTHER THAN AS
+ * AUTHORIZED UNDER THIS LICENSE OR COPYRIGHT LAW IS PROHIBITED.
+ *
+ * BY EXERCISING ANY RIGHTS TO THE WORK PROVIDED HERE, YOU ACCEPT AND AGREE TO
+ * BE BOUND BY THE TERMS OF THIS LICENSE. THE LICENSOR GRANTS YOU THE RIGHTS
+ * CONTAINED HERE IN CONSIDERATION OF YOUR ACCEPTANCE OF SUCH TERMS AND
+ * CONDITIONS.
+ *
+ */
+
+/* Author: Steven Macenski */
+
+#ifndef SLAM_TOOLBOX__LOOP_CLOSURE_ASSISTANT_MULTIROBOT_HPP_
+#define SLAM_TOOLBOX__LOOP_CLOSURE_ASSISTANT_MULTIROBOT_HPP_
+
+#include <thread>
+#include <string>
+#include <functional>
+#include <memory>
+#include <map>
+
+#include "tf2_ros/transform_broadcaster.h"
+#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp"
+#include "tf2/utils.h"
+#include "rclcpp/rclcpp.hpp"
+#include "interactive_markers/interactive_marker_server.hpp"
+#include "interactive_markers/menu_handler.hpp"
+
+#include "slam_toolbox/toolbox_types.hpp"
+#include "slam_toolbox/laser_utils.hpp"
+#include "slam_toolbox/visualization_utils.hpp"
+
+namespace loop_closure_assistant
+{
+
+using namespace ::toolbox_types;  // NOLINT
+
+class LoopClosureAssistant
+{
+public:
+  LoopClosureAssistant(
+    rclcpp::Node::SharedPtr node, karto::Mapper * mapper,
+    laser_utils::ScanHolder * scan_holder, PausedState & state,
+    ProcessType & processor_type);
+
+  void clearMovedNodes();
+  void processInteractiveFeedback(
+    const visualization_msgs::msg::InteractiveMarkerFeedback::ConstSharedPtr feedback);
+  void publishGraph();
+  void setMapper(karto::Mapper * mapper);
+
+private:
+  bool manualLoopClosureCallback(
+    const std::shared_ptr<rmw_request_id_t> request_header,
+    const std::shared_ptr<slam_toolbox::srv::LoopClosure::Request> req, 
+    std::shared_ptr<slam_toolbox::srv::LoopClosure::Response> resp);
+  bool clearChangesCallback(
+    const std::shared_ptr<rmw_request_id_t> request_header,
+    const std::shared_ptr<slam_toolbox::srv::Clear::Request> req, 
+    std::shared_ptr<slam_toolbox::srv::Clear::Response> resp);
+  bool interactiveModeCallback(
+    const std::shared_ptr<rmw_request_id_t> request_header,
+    const std::shared_ptr<slam_toolbox::srv::ToggleInteractive::Request>  req,
+    std::shared_ptr<slam_toolbox::srv::ToggleInteractive::Response> resp);
+
+  void moveNode(const int& id, const Eigen::Vector3d& pose);
+  void addMovedNodes(const int& id, Eigen::Vector3d vec);
+
+  std_msgs::msg::ColorRGBA generateNewColor();
+
+  std::unique_ptr<tf2_ros::TransformBroadcaster> tfB_;
+  laser_utils::ScanHolder * scan_holder_;
+  rclcpp::Publisher<visualization_msgs::msg::MarkerArray>::SharedPtr marker_publisher_;
+  rclcpp::Publisher<sensor_msgs::msg::LaserScan>::SharedPtr scan_publisher_;
+  rclcpp::Service<slam_toolbox::srv::Clear>::SharedPtr ssClear_manual_;
+  rclcpp::Service<slam_toolbox::srv::LoopClosure>::SharedPtr ssLoopClosure_;
+  rclcpp::Service<slam_toolbox::srv::ToggleInteractive>::SharedPtr ssInteractive_;
+  boost::mutex moved_nodes_mutex_;
+  std::map<int, Eigen::Vector3d> moved_nodes_;
+  karto::Mapper * mapper_;
+  karto::ScanSolver * solver_;
+  std::unique_ptr<interactive_markers::InteractiveMarkerServer> interactive_server_;
+  boost::mutex interactive_mutex_;
+  bool interactive_mode_, enable_interactive_mode_;
+  rclcpp::Node::SharedPtr node_;
+  std::string map_frame_;
+  PausedState & state_;
+  ProcessType & processor_type_;
+  std::map<karto::Name, std_msgs::msg::ColorRGBA> m_sensor_name_to_color_;
+};
+
+}   // namespace loop_closure_assistant
+
+#endif  // SLAM_TOOLBOX__LOOP_CLOSURE_ASSISTANT_MULTIROBOT_HPP_

include/slam_toolbox/multirobot/slam_toolbox_async_multirobot.hpp

@@ -0,0 +1,47 @@
+/*
+ * slam_toolbox
+ * Copyright Work Modifications (c) 2018, Simbe Robotics, Inc.
+ * Copyright Work Modifications (c) 2019, Steve Macenski
+ * Copyright Work Modifications (c) 2024, Daniel I. Meza
+ *
+ * THE WORK (AS DEFINED BELOW) IS PROVIDED UNDER THE TERMS OF THIS CREATIVE
+ * COMMONS PUBLIC LICENSE ("CCPL" OR "LICENSE"). THE WORK IS PROTECTED BY
+ * COPYRIGHT AND/OR OTHER APPLICABLE LAW. ANY USE OF THE WORK OTHER THAN AS
+ * AUTHORIZED UNDER THIS LICENSE OR COPYRIGHT LAW IS PROHIBITED.
+ *
+ * BY EXERCISING ANY RIGHTS TO THE WORK PROVIDED HERE, YOU ACCEPT AND AGREE TO
+ * BE BOUND BY THE TERMS OF THIS LICENSE. THE LICENSOR GRANTS YOU THE RIGHTS
+ * CONTAINED HERE IN CONSIDERATION OF YOUR ACCEPTANCE OF SUCH TERMS AND
+ * CONDITIONS.
+ *
+ */
+
+/* Author: Steven Macenski */
+
+#ifndef SLAM_TOOLBOX__SLAM_TOOLBOX_ASYNC_MULTIROBOT_HPP_
+#define SLAM_TOOLBOX__SLAM_TOOLBOX_ASYNC_MULTIROBOT_HPP_
+
+#include <memory>
+#include "slam_toolbox/multirobot/slam_toolbox_common_multirobot.hpp"
+
+namespace slam_toolbox
+{
+
+class AsynchronousSlamToolboxMultirobot : public SlamToolboxMultirobot
+{
+public:
+  explicit AsynchronousSlamToolboxMultirobot(rclcpp::NodeOptions options);
+  ~AsynchronousSlamToolboxMultirobot() {}
+
+protected:
+  void laserCallback(
+    sensor_msgs::msg::LaserScan::ConstSharedPtr scan, const std::string & base_frame_id) override;
+  bool deserializePoseGraphCallback(
+    const std::shared_ptr<rmw_request_id_t> request_header,
+    const std::shared_ptr<slam_toolbox::srv::DeserializePoseGraph::Request> req,
+    std::shared_ptr<slam_toolbox::srv::DeserializePoseGraph::Response> resp) override;
+};
+
+}  // namespace slam_toolbox
+
+#endif  // SLAM_TOOLBOX__SLAM_TOOLBOX_ASYNC_MULTIROBOT_HPP_