Multirobot SLAM ros2

[acachathuranga]

---

Basic Info

An extension of slam_toolbox for ROS2 multi-robot distributed SLAM. This work was done as part of a research at Singapore University of Technology and Design.

Info	Please fill out this column
Ticket(s) this addresses	(76)
Primary OS tested on	(Ubuntu 22.04)
Tested on gazebo and indoor space at Singapore University of Technology and Design	Gazebo and Custom made Physical Robot (Monsterborg platform with RPLidar A3)

---

Description of contribution in a few bullet points

• Extends slam_toolbox to perform multi-robot mapping and localization in ROS2 (Can also be used for multi-laser scenarios).
• multirobot_slam_toolbox_node implements the new functionality separately (Behaviors and functionalities of all other slam_toolbox nodes remain unchanged).
• Reduces network bandwidth usage by only sharing pose-graph scan data between each robot (Uses new message type LocalizedLaserScan). No sharing of any other topics or data between the robots is required.
• Constructs a merged pose-graph and performs loop closure distributively on each robot.
• Not the original use-case, but this can also be used in multi-laser robots, by running a slam_toolbox instance for each laser.

Description of documentation updates required from your changes

• Slam_toolbox instances needs to be run on each robot under unique namespaces.
• New topic '/localized_scan' to share pose-graph data between robots. This topic must not be in a namespace.
• 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.
• Robots must start close to each other, in the same orientation. Scan matching is used to get the relative pose between the two pose graphs.
• Each laser is named by the host namespace.

---

Future work that may be required in bullet points

• Take the initial start pose of robots through input arguments / parameters (without requiring robots to start closer to each other)
• Experiment on lossy WiFi networks. How would the nodes handle scan message losses between each others.

[Daniel-Meza]

@acachathuranga this looks great.

Could you go into more detail about how you set up the two robot scenario and run the multirobot_slam_toolbox_node?
Looking at your forked multirobot_ros2 repo I don't see launch files to use as reference. If you have the ones you used for the demo above that would also be very helpful!

FYI, I am trying to replicate your demo using two Turtlebot3 robot platforms in Gazebo. I have set up the namespaces according to your diagram but when launching everything I can't seem to get the correct /robot1/map and /robot2/map topics to publish and the /localized_scan topic shows no published messages.

Appreciate any pointers.

[acachathuranga]

@Daniel-Meza I have added the launch and parameter files for multi-robot mapping. The parameters file is identical to mapper_params_online_async.yaml.

I have also updated the ReadMe on how to run multi-robot slam with turtlebot3 multirobot simulation (provided with navigation2 nav2_bringup package).

In short, you can follow the installation instructions in navigation2 Getting started tutorial. Once you've installed navigation2, nav2_bringup and turtlebot3 packages, source the environment variables as described and run the following commands in separate terminals.

ros2 launch nav2_bringup multi_tb3_simulation_launch.py headless:=False
ros2 launch slam_toolbox online_async_multirobot_launch.py namespace:=robot1
ros2 launch slam_toolbox online_async_multirobot_launch.py namespace:=robot2

Then you can use teleop_twist_keyboard to drive the robots around using following commands. When driving the robots around, the merged map will be displayed on both rviz windows.

ros2 run teleop_twist_keyboard teleop_twist_keyboard __ns:=/robot1
ros2 run teleop_twist_keyboard teleop_twist_keyboard __ns:=/robot2

[Daniel-Meza]

@acachathuranga Thank you, I was able to replicate the demo and also get it working with a 3rd robot.

Going a bit further, do you have some insights on the following:

Maximum number of robots. Since every robot/sensor runs its own slam_toolbox node, do you foresee any limitation on the number of robots that can be used simultaneously? Assuming of course there is enough computing power and bandwidth to handle all of the data.

Running on real robots. In Gazebo simulation, I believe the "map" and "odom" frame is always placed at a global (0, 0, 0) regardless of where the robots are spawned. This means that every slam_toolbox node has their "map" located based on the same initial point. This is not the case when booting up real robots since the map frame is placed on the current physical location of the robot itself. So, would it be necessary to provide an additional transform to ensure that the "map" frame of real robots align on top of each other? Or is this transformation handled automatically by the multirobot nodes?

[acachathuranga]

@Daniel-Meza nice to hear you got it working!

Maximum number of robots: Given there is enough computing power and bandwidth, I do not see any limitation on the number of robots (Unless there are underlying SLAM limitations within slam_toolbox on number of graph nodes etc). But nothing specific to 'multi-robot slam'. However, when the robot count increases in real world scenarios (at least during our testing), you're likely to run into bandwidth problems before anything else :)

Running on real robots: This is explained in the PR notes as well as the updated ReadMe. To summarize; gazebo simulation odometry is indeed in a global frame. So everything will automatically work. When you run on actual robots, you need to start the robots very close to each other, and also in the same orientation. Then the scan matcher will handle the initial offsets. In our testing, what we did was, 1) turn on one robot at a marked location, 2) move the robot away from the location 3) place another robot at the marked location and repeat. Taking initial poses from input args /parameters will be added in near future.

[i1Cps]

Good Morning, Are you able to incoperate the recently added /reset feature with the multi-robot solution?

[acachathuranga]

@SteveMacenski any thoughts or comments on this? I noticed many people find this multi-robot implementation useful and there have also been some recent duplicate efforts (PR #727) on this multi-robot slam topic.

This work has been tested extensively with real robot fleets (2 robot, 4 robot, 8 robot configurations) and has provided very good results. The use of '/localized_scan' topic instead of sharing all laser scans among all robots provides real-life benefits in terms of network bandwidth. We also did extensive tests with different types of network (Infrastructure based, mesh .etc) and found out sharing all laser messages is indeed not practical for real situations. We already have done further extensions on this work, in terms of bandwidth optimizations for large robot fleets. I will push these updates once our work is published.