Multirobot SLAM ros2

CMakeLists.txt

@@ -74,6 +74,7 @@ set(libraries
     toolbox_common
     SlamToolboxPlugin
     ceres_solver_plugin
+    multirobot_slam_toolbox
     async_slam_toolbox
     sync_slam_toolbox
     localization_slam_toolbox
@@ -103,6 +104,7 @@ include_directories(include lib/karto_sdk/include
 add_definitions(${EIGEN3_DEFINITIONS})
 
 rosidl_generate_interfaces(${PROJECT_NAME}
+  msg/LocalizedLaserScan.msg
   srvs/Pause.srv
   srvs/Reset.srv
   srvs/ClearQueue.srv
@@ -114,7 +116,7 @@ rosidl_generate_interfaces(${PROJECT_NAME}
   srvs/AddSubmap.srv
   srvs/DeserializePoseGraph.srv
   srvs/SerializePoseGraph.srv
-  DEPENDENCIES builtin_interfaces geometry_msgs std_msgs nav_msgs visualization_msgs
+  DEPENDENCIES builtin_interfaces geometry_msgs std_msgs nav_msgs visualization_msgs sensor_msgs
 )
 
 #### rviz Plugin
@@ -157,6 +159,11 @@ rosidl_get_typesupport_target(cpp_typesupport_target ${PROJECT_NAME} "rosidl_typ
 target_link_libraries(toolbox_common "${cpp_typesupport_target}")
 
 #### Mapping executibles
+add_library(multirobot_slam_toolbox src/slam_toolbox_multirobot.cpp)
+target_link_libraries(multirobot_slam_toolbox toolbox_common kartoSlamToolbox ${Boost_LIBRARIES})
+add_executable(multirobot_slam_toolbox_node src/slam_toolbox_multirobot_node.cpp)
+target_link_libraries(multirobot_slam_toolbox_node multirobot_slam_toolbox)
+
 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)
@@ -203,7 +210,8 @@ rclcpp_components_register_nodes(lifelong_slam_toolbox "slam_toolbox::LifelongSl
 rclcpp_components_register_nodes(map_and_localization_slam_toolbox "slam_toolbox::MapAndLocalizationSlamToolbox")
 
 #### Install
-install(TARGETS async_slam_toolbox_node
+install(TARGETS multirobot_slam_toolbox_node
+                async_slam_toolbox_node
                 sync_slam_toolbox_node
                 localization_slam_toolbox_node
                 map_and_localization_slam_toolbox_node

README.md

@@ -111,6 +111,37 @@ To minimize the amount of changes required for moving to this mode over AMCL, we
 
 In summary, this approach I dub `elastic pose-graph localization` is where we take existing map pose-graphs and localized with-in them with a rolling window of recent scans. This way we can localize in an existing map using the scan matcher, but not update the underlaying map long-term should something go wrong. It can be considered a replacement to AMCL and results is not needing any .pgm maps ever again. The lifelong mapping/continuous slam mode above will do better if you'd like to modify the underlying graph while moving. This method of localization might not be suitable for all applications, it does require quite a bit of tuning for your particular robot and needs high quality odometry. If in doubt, you're always welcome to use other 2D map localizers in the ecosystem like AMCL. For most beginners or users looking for a good out of the box experience, I'd recommend AMCL. 
 
+# Multi-Robot SLAM
+![multirobot_slam](/images/multi-robot_mapping.gif?raw=true "Multi-Robot SLAM")
+
+*multirobot_slam_toolbox_node* extends slam_toolbox for multi-robot mapping and localization. Slam_toolbox instances are run on each robot under unique namespaces and pose graph data are shared between the nodes through */localized_scan* topic. Each slam_toolbox instance publishes transform of host robot and peers in the network. Peer transform frame names are prefixed with the namespace of the peer.
+
+Initially the robots should start close to each other, in the same orientation, as scan matching is used to find the connection between different pose graphs. This however is not required in simulation environments such as gazebo, as all robots' odometry is referenced to a gazebo's global frame. 
+
+![multirobot_slam_architecture](/images/multi-robot_architecture.png?raw=true "Multi-Robot SLAM Node Architecture")
+
+### Running Multi-Robot SLAM with turtlebot3 simulation
+
+Install and launch turtlebot3 multirobot simulation as described in the [navigation2 documentation](https://navigation.ros.org/getting_started/index.html)
+
+```shell
+ros2 launch nav2_bringup multi_tb3_simulation_launch.py headless:=False
+```
+
+In two seperate terminals, launch two *multirobot_slam_toolbox_node* instances for robot1 and robot2.
+
+```shell
+ros2 launch slam_toolbox online_async_multirobot_launch.py namespace:=robot1
+ros2 launch slam_toolbox online_async_multirobot_launch.py namespace:=robot2
+```
+
+In another two seperate terminals, run *teleop_twist_keyboard* to control the robots. The merged map will appear on both rviz windows when moving the robots around in the environment.
+
+```shell
+ros2 run teleop_twist_keyboard teleop_twist_keyboard __ns:=/robot1
+ros2 run teleop_twist_keyboard teleop_twist_keyboard __ns:=/robot2
+```
+
 ## Tools 
 
 ### Plugin based Optimizers

config/mapper_params_online_multi_async.yaml

@@ -0,0 +1,73 @@
+$(var namespace)/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_frame: odom
+    map_frame: map
+    base_frame: base_footprint
+    scan_topic: 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_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
+    max_laser_range: 20.0 #for rastering images
+    minimum_time_interval: 0.5
+    transform_timeout: 0.2
+    tf_buffer_duration: 30.0
+    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/laser_utils.hpp

@@ -82,10 +82,14 @@ class LaserAssistant
     const std::string & base_frame);
   ~LaserAssistant();
   LaserMetadata toLaserMetadata(sensor_msgs::msg::LaserScan scan);
+  LaserMetadata toLaserMetadata(
+    sensor_msgs::msg::LaserScan scan,
+    geometry_msgs::msg::TransformStamped laser_pose);
 
 private:
   karto::LaserRangeFinder * makeLaser(const double & mountingYaw);
   bool isInverted(double & mountingYaw);
+  geometry_msgs::msg::TransformStamped readLaserPose();
 
   rclcpp::Logger logger_;
   rclcpp::node_interfaces::NodeParametersInterface::SharedPtr parameters_interface_;

include/slam_toolbox/slam_toolbox_multirobot.hpp

@@ -0,0 +1,65 @@
+/*
+ * multirobot_slam_toolbox
+ * Copyright Work Modifications (c) 2023, Achala Athukorala
+ *
+ * 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.
+ *
+ */
+
+#ifndef SLAM_TOOLBOX__SLAM_TOOLBOX_MULTIROBOT_HPP_
+#define SLAM_TOOLBOX__SLAM_TOOLBOX_MULTIROBOT_HPP_
+
+#include <memory>
+#include <string>
+#include "slam_toolbox/slam_toolbox_common.hpp"
+#include "slam_toolbox/toolbox_msgs.hpp"
+
+namespace slam_toolbox
+{
+
+class MultiRobotSlamToolbox : public SlamToolbox
+{
+public:
+  explicit MultiRobotSlamToolbox(rclcpp::NodeOptions);
+  ~MultiRobotSlamToolbox() {}
+
+protected:
+  LocalizedRangeScan * addExternalScan(
+    LaserRangeFinder * laser,
+    const sensor_msgs::msg::LaserScan::ConstSharedPtr & scan,
+    Pose2 & odom_pose);
+  void publishLocalizedScan(
+    const sensor_msgs::msg::LaserScan::ConstSharedPtr & scan,
+    const Pose2 & offset,
+    const Pose2 & pose, const Matrix3 & cov,
+    const rclcpp::Time & t);
+
+  // callbacks
+  void laserCallback(sensor_msgs::msg::LaserScan::ConstSharedPtr scan) 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;
+  void localizedScanCallback(
+    slam_toolbox::msg::LocalizedLaserScan::ConstSharedPtr localized_scan);
+  LaserRangeFinder * getLaser(
+    const slam_toolbox::msg::LocalizedLaserScan::ConstSharedPtr localized_scan);
+  using SlamToolbox::getLaser;
+
+  std::shared_ptr<rclcpp::Publisher<slam_toolbox::msg::LocalizedLaserScan>> localized_scan_pub_;
+  rclcpp::Subscription<slam_toolbox::msg::LocalizedLaserScan>::SharedPtr localized_scan_sub_;
+  std::string localized_scan_topic_;
+  std::string current_ns_;
+};
+
+}  // namespace slam_toolbox
+
+#endif   // SLAM_TOOLBOX__SLAM_TOOLBOX_MULTIROBOT_HPP_

include/slam_toolbox/toolbox_msgs.hpp

@@ -40,5 +40,6 @@
 #include "slam_toolbox/srv/deserialize_pose_graph.hpp"
 #include "slam_toolbox/srv/merge_maps.hpp"
 #include "slam_toolbox/srv/add_submap.hpp"
+#include "slam_toolbox/msg/localized_laser_scan.hpp"
 
 #endif  // SLAM_TOOLBOX__TOOLBOX_MSGS_HPP_

launch/online_async_multirobot_launch.py

@@ -0,0 +1,51 @@
+import os
+
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument
+from launch.substitutions import LaunchConfiguration
+from launch_ros.actions import Node
+from launch_ros.descriptions import ParameterFile
+from ament_index_python.packages import get_package_share_directory
+
+
+def generate_launch_description():
+    pkg_dir = get_package_share_directory("slam_toolbox")
+
+    use_sim_time = LaunchConfiguration('use_sim_time')
+    namespace = LaunchConfiguration('namespace')
+
+    # Topic remappings
+    remappings = [  ('/map', 'map'),
+                    ('/tf', 'tf'), 
+                    ('/tf_static', 'tf_static'),
+                    ('/map_metadata', 'map_metadata')]
+
+    declare_use_sim_time_argument = DeclareLaunchArgument(
+        'use_sim_time',
+        default_value='true',
+        description='Use simulation/Gazebo clock')
+
+    declare_robot_name_argument = DeclareLaunchArgument(
+        'namespace',
+        default_value='robot1',
+        description='Robot Name / Namespace')
+
+    start_async_slam_toolbox_node = Node(
+        package='slam_toolbox',
+        executable='multirobot_slam_toolbox_node',
+        name='slam_toolbox',
+        namespace=namespace,
+        output='screen',
+        remappings=remappings,
+        parameters=[
+          ParameterFile(os.path.join(pkg_dir, 'config', 'mapper_params_online_multi_async.yaml'), allow_substs=True),
+          {'use_sim_time': use_sim_time}]
+        )
+
+    ld = LaunchDescription()
+    ld.add_action(declare_robot_name_argument)
+
+    ld.add_action(declare_use_sim_time_argument)
+    ld.add_action(start_async_slam_toolbox_node)
+
+    return ld

msg/LocalizedLaserScan.msg

@@ -0,0 +1,7 @@
+# Single scan from a planar laser range-finder and the pose of the robot
+
+sensor_msgs/LaserScan scan
+# Scanner pose in robot base_frame
+geometry_msgs/TransformStamped scanner_offset
+# Robot base_frame pose in a global (map) frame
+geometry_msgs/PoseWithCovarianceStamped pose

package.xml

@@ -69,10 +69,7 @@
   <exec_depend>nav2_map_server</exec_depend>
   <exec_depend>builtin_interfaces</exec_depend>
   <exec_depend>rosidl_default_generators</exec_depend>
-  <exec_depend>lifecycle_msgs</exec_depend>
-  <exec_depend>bondcpp</exec_depend>
-  <exec_depend>bond</exec_depend>
-  <exec_depend>rclcpp_lifecycle</exec_depend>
+  <exec_depend>rosidl_default_runtime</exec_depend>
 
   <exec_depend>libqt5-core</exec_depend>
   <exec_depend>libqt5-gui</exec_depend>

src/laser_utils.cpp

@@ -85,6 +85,16 @@ LaserMetadata LaserAssistant::toLaserMetadata(sensor_msgs::msg::LaserScan scan)
 {
   scan_ = scan;
   frame_ = scan_.header.frame_id;
+  return toLaserMetadata(scan, readLaserPose());
+}
+
+LaserMetadata LaserAssistant::toLaserMetadata(
+  sensor_msgs::msg::LaserScan scan,
+  geometry_msgs::msg::TransformStamped laser_pose)
+{
+  scan_ = scan;
+  frame_ = scan_.header.frame_id;
+  laser_pose_ = laser_pose;
 
   double mountingYaw;
   bool inverted = isInverted(mountingYaw);
@@ -95,9 +105,10 @@ LaserMetadata LaserAssistant::toLaserMetadata(sensor_msgs::msg::LaserScan scan)
 
 karto::LaserRangeFinder * LaserAssistant::makeLaser(const double & mountingYaw)
 {
+  std::string laser_namespace = scan_.header.frame_id.substr(0, scan_.header.frame_id.find('/'));
   karto::LaserRangeFinder * laser =
     karto::LaserRangeFinder::CreateLaserRangeFinder(
-    karto::LaserRangeFinder_Custom, karto::Name("Custom Described Lidar"));
+    karto::LaserRangeFinder_Custom, karto::Name("Custom Described Lidar : " + laser_namespace));
   laser->SetOffsetPose(karto::Pose2(laser_pose_.transform.translation.x,
     laser_pose_.transform.translation.y, mountingYaw));
 
@@ -174,12 +185,6 @@ karto::LaserRangeFinder * LaserAssistant::makeLaser(const double & mountingYaw)
 
 bool LaserAssistant::isInverted(double & mountingYaw)
 {
-  geometry_msgs::msg::TransformStamped laser_ident;
-  laser_ident.header.stamp = scan_.header.stamp;
-  laser_ident.header.frame_id = frame_;
-  laser_ident.transform.rotation.w = 1.0;
-
-  laser_pose_ = tf_->transform(laser_ident, base_frame_);
   mountingYaw = tf2::getYaw(laser_pose_.transform.rotation);
 
   RCLCPP_DEBUG(
@@ -188,12 +193,13 @@ bool LaserAssistant::isInverted(double & mountingYaw)
     laser_pose_.transform.translation.y,
     laser_pose_.transform.translation.z, mountingYaw);
 
-  geometry_msgs::msg::Vector3Stamped laser_orient;
-  laser_orient.vector.z = laser_orient.vector.y = 0.;
-  laser_orient.vector.z = 1 + laser_pose_.transform.translation.z;
-  laser_orient.header.stamp = scan_.header.stamp;
-  laser_orient.header.frame_id = base_frame_;
-  laser_orient = tf_->transform(laser_orient, frame_);
+  tf2::Vector3 laser_orient;
+  tf2::Transform laser_pose;
+  tf2::convert(laser_pose_.transform, laser_pose);
+  laser_orient.setY(0.);
+  laser_orient.setZ(0.);
+  laser_orient.setZ(1 + laser_pose_.transform.translation.z);  // TOOD can remove addition of laser_pose z component
+  laser_orient = laser_pose * laser_orient;
 
   if (laser_orient.vector.z <= 0) {
     RCLCPP_DEBUG(
@@ -204,6 +210,16 @@ bool LaserAssistant::isInverted(double & mountingYaw)
   return false;
 }
 
+geometry_msgs::msg::TransformStamped LaserAssistant::readLaserPose()
+{
+  geometry_msgs::msg::TransformStamped laser_ident;
+  laser_ident.header.stamp = scan_.header.stamp;
+  laser_ident.header.frame_id = frame_;
+  laser_ident.transform.rotation.w = 1.0;
+
+  return tf_->transform(laser_ident, base_frame_);
+}
+
 ScanHolder::ScanHolder(std::map<std::string, laser_utils::LaserMetadata> & lasers)
 : lasers_(lasers)
 {