Multiple Robots: Alternative 1
Introduction
In this section I am going to present some rather advanced technics for creating multi-robot environment. To start with, let's see what the task looks like in a perfect world. Note that this is NOT a recommended approach: the "traditional" approach I use throughout the sections is better (unlike an approach from the next section). This is just a tutorial.
Ok, we have two robots, robot1 and robot2. Each publishes something, for our example, let's say we have dif. drive, lidar, camera and imu. So if we just (how?) add two robots to the system, they will interfere, and there will be no way of distinduishing them. And if we use something like the following command to make our robot move:
$ ros2 topic pub cmd_vel geometry_msgs/msg/Twist '{linear: {x: 0.5, y: 0.0, z: 0.0}, angular: {x: 0.0, y: 0.0, z: 0.0}}'
... then both robots will start moving.
To fix this problem, any programmer will immediately suggest the following pattern: let's assign each robot an id. Now, all messages in the system that has anything to do with these robots, have to contain this id, so robots can choose "their" messages, and other modules and programs (like RViz) can distinguish them as well.
In ROS (before ROS2), there was a tf_prefix, that could be used for this
purpose, and also namespaces. Then, in ROS2, only namespaces
were left, while tf_prefix was deprecated. So now we are supposed to
pass to (whatever) namespace, and messages and everything else will be
(should be, note this note!) altered: cmd_vel to robot1/cmd_vel
etc.
Easy enough.
So far - so good... Wait! No. This is ROS2, remember? There are no easy ways.
First of all, not all ROS2 modules respect this approach, and even worse, not all of them respect it completely. For example, you can pass namespace to robot_state_publisher (one of the very important ROS2 modules!), but most likely (unless you spend a lot of time, but it is not guaranteed) it will not work. And it gets much worse with nav2 modules.
To make our life more entertaining, ROS2 developers also desided to make tf (transformations) and tf_static (static transformations) global. We are not supposed to add namespaces to them. I am not going to add links to this discussion, let's just say that the Internet is full of frustrated people, asking "how can we distinguish positions of two robots if tf is not prefixed?"
So they created a neat trick, we need to pass (to whatever) the following remappings:
remappings = [('/tf', 'tf'), ('/tf_static', 'tf_static')]
It maps global topics (note the "slash") to local ones, and in theory, now we can prefix them, using namespaces.
Except we can not. Not always. Not in all versions of ROS2. For example, in a Galactic tutorial you can find a (not exactly true) statement that namespacing is not supported in Galactic (!!!) It is supported. But... see above.
In this section I will show you an approach that doesn't use namespaces at all, which means that instead of relying on ROS2 modules to do their job properly, you will have complete control over your system and how it behaves.
I will also do my best to explain what is done, and why, so that when you want to add some additional modules to your system, you know what to do.
Make no mistake: this approach is complex and generally ugly, and it is possible to create ROS2 module (think controller_server) that will stumble on it. But at least it gives you an alternative. In other words, what I am going to do is emulating the use of namespaces, just with a bit more control on my side.
Screenshots for RViz and Gazebo:
The code for this section is located in multi_bot_01 part of an archive. Note that archive includes some additional folders, like maps and worlds, that are used by all projects and therefore are located outside of them. Also note that we use ROS2 Galactic now.
In this section I will create a minimal example: two robots, in a simple world, displayed and properly controlled both in Gazebo and RViz.
The Tree
Here is an idea of the hierarchy we will use:
world
├── odom/robot1
│ └── base_link/robot1
│ └── wheels, body and everything else
└── odom/robot1
└── base_link/robot1
└── wheels, body and everything else
As you can see, the world is at the root of the tree. It is not prefixed (or should I say "postfixed"); I use robot names as postfixes for a simple reason: it is easier to code.
Between the world and each robot we have odom, with name of the corresponding robot as a postfix. Odometry represents the "meter" between the initial position of a robot and its current position, so it should be the one connecting world and a robot.
Then we have a base_link (you can use base_footprint or whatever else you want, just make sure it is the top one in robot's tree). Note that we use postfix everywhere: we need to be able to distinguish robots, and that applies to robots' bodies, robots' wheels and so on.
Topics
This is a second part of the puzzle we need to build. Robots and their environment are exchanging messages, and according to ROS2 terminology, messages are published on topics. "Topic" is just the "type" of a message.
We want all topics that are produced (published) by a robot or consumed (subscribed to) by a robot to be distinct, if they are robot-specific. I can think of topic that is published by a robot but is not robot-specific (like EST time, if we want our robot to publish it), but mostly we need to add postfixes.
As I mentioned earlier, there is a huge mess in a way ROS2 uses namespaces, so I am not going to even look in this direction. Instead, I am going to add postfixes myself. Let's take a look at the code of multi_simulation_launch.py, which is the main launch file of this section, and the most altered one. You can get the full code from the archive, here I am going to focus only on important parts. First of all, please link to my site if you use the code:
# Copyright (c) 2023 robotics.snowcron.com
#
# Licensed under the Apache License, Version 2.0 (the "License")
# with the following addition: a direct link to robotics.snowcron.com
# should be provided if the code or derived code is used
# or quoted.
Then, we include the globals.py, which you should already be familiar with as I used it in prior sections:
# This is a way to move common code to a common include
import sys
sys.path.append("src/multi_bot_01/launch")
from globals import *
Next part is the "heart" of the approach I am using. Let's modify the robot's description (URDF) after we loaded it from the file but before we pass it to code that uses it. Modification, as you can guess, is all about adding postfixes.
By the way, here is the answer to a question "why postfixes". From ROS2 point of view, there is no difference (if you don't use namespaces, then it will expect prefixes). But from the programmer's point of view the difference exists. Let's take a look at the following string:
... that we want to modify by adding either prefix (~/out:=robot1/imu) or postfix (~/out:=imu/robot1). In both cases we can get the string "~/out:=imu" from the tag. If we want prefix, we have to split the string, add prefix and reassemble the string. In case of a postfix, we have to simply append the "/robot1" to the end of a string. So my choice of postfixes over prefixes can be explained by me being lazy.
Let's continue with the code. The following function adds postfixes to the URDF. To write it, I used "print(urdf)" to get the XML text, and had to go through it, looking for elements that had to be altered. It is important to keep in mind, as whenever you modify the URDF, you will have to revisit the code and maybe add some new cases.
Loading robots in cycle
A generate_launch_description() function. Here we use a cycle to create multiple robots. In this example I use robots of the same type, but nothing prevents you from using different URDFs for different robots.
Now the RViz part. The way this software is organized, you have to provide a config file. If you have one robot, and you want it to be displayed by default, you have to include it in the config file and to save the config. If you want to see two robots, you have to include two robots and save the config, and it will not work with one robot (or at lease you will have second robot displaying an error).
So if you want to see 20 robots in RViz (which probably is not a good idea, but still), you have a lot of manual work to do. And if the number of robots is non-constant... oh!
I haven't investigated the issue, but the first thing that comes to mind is to hack the RViz config format (which is a text file), and generate it on the fly. I am not going to do it, as I do not believe that having 20 robots in RViz is a good practice.
For our examole, I have multi_bot_01/rviz/robot_basic.rviz file. Copy it to robot.rviz file, and open it: it contains config for two robots, robot1 and robot2.
rviz_config_file = LaunchConfiguration('rviz_config_file')
Next we need to open Gazebo. Note the commented code, that in earlier sections I used a different mechanism to start Gazebo, because I loaded world differently. Now I pass the world explicitly:
# Start Gazebo with plugin providing the robot spawing service
gazebo = ExecuteProcess(
cmd=[simulator, '--verbose', '-s', 'libgazebo_ros_factory.so', world],
output='screen')
print(">>>", os.path.join(get_package_share_directory('multi_bot_01')))
# gazebo = IncludeLaunchDescription(
# PythonLaunchDescriptionSource([os.path.join(
# get_package_share_directory('gazebo_ros'), 'launch', 'gazebo.launch.py')]),
# launch_arguments={
# 'gui': 'True',
# 'server': 'True',
# #'models': os.path.join(get_package_share_directory('multi_bot_01'), 'models')
# }.items()
# )
Now we can run the cycle itself (and note that Gazebo was outside this cycle and before it):
Next, we need to start the RViz. Notice, that we have two options here. We can either call it in cycle (and get multiple instances of RViz, which can be convenient in some situations), or we can have one instance holding all robots. I haven't implemented the first case; to do it, you will have to move the code for RViz inside the cycle and to make sure each version opens its own config file (one for robot1, another one for robot2...)
Finally, we need to add all these actions to launch description and return it from the function. This is a standard approach that was used in earlier sections. The only thing to pay attention to is the cycle we use to access everything we placed into the spawn_robot_cmd array:
Useful commands
Here are few ROS2 command line instructions I found useful when writing this section.
# To see topic:
$ ros2 topic echo cmd_vel/robot1
$ ros2 topic echo tf/robot1
# To see all available topics
$ ros2 topic list
# To see what is published on topic
$ ros2 topic echo /amcl_pose
# To create static transform
$ ros2 run tf2_ros static_transform_publisher 1 0 0 0 0 0 world odom/robot1
# To make robot move:
$ ros2 topic pub cmd_vel/robot1 geometry_msgs/msg/Twist '{linear: {x: 0.5, y: 0.0, z: 0.0}, angular: {x: 0.0, y: 0.0, z: 0.0}}'
$ ros2 topic pub cmd_vel/robot2 geometry_msgs/msg/Twist '{linear: {x: 0.5, y: 0.0, z: 0.0}, angular: {x: 0.0, y: 0.0, z: 0.0}}'
$ ros2 run teleop_twist_keyboard teleop_twist_keyboard --remap cmd_vel:=cmd_vel/robot1
# to get a list of running nodes
$ ros2 node list
# Hardware interfaces
$ ros2 control list_hardware_interfaces
# Transformation tree
$ ros2 run rqt_tf_tree rqt_tf_tree
Compiling and Running
# Terminal 1, if you want to move robot, one of the following:
$ ros2 topic pub cmd_vel/robot1 geometry_msgs/msg/Twist '{linear: {x: 0.5, y: 0.0, z: 0.0}, angular: {x: 0.0, y: 0.0, z: 0.0}}'
$ ros2 topic pub cmd_vel/robot2 geometry_msgs/msg/Twist '{linear: {x: 0.5, y: 0.0, z: 0.0}, angular: {x: 0.0, y: 0.0, z: 0.0}}'
$ ros2 run teleop_twist_keyboard teleop_twist_keyboard --remap cmd_vel:=cmd_vel/robot1
# Terminal 2:
# echo $ROS_DISTRO
# galactic
# source /opt/ros/galactic/setup.bash
$ cd ~/SnowCron/ros_projects/harsh
$ colcon build --packages-select multi_bot_nav_01
$ source install/setup.bash
$ ros2 launch multi_bot_01 multi_simulation_launch.py world:=src/worlds/maze.sdf
Links
https://docs.ros.org/en/eloquent/Tutorials/tf2.html
https://bitbucket.org/theconstructcore/box_bot/src/foxy/
https://programtalk.com/python-more-examples/nav2_common.launch.RewrittenYaml/
https://github.com/ros-planning/navigation2/issues/2117
https://www.reddit.com/r/ROS/comments/uvw8ij/multiple_robots_gazebo_ros2/
https://osrf.github.io/ros2multirobotbook/
https://www.theconstructsim.com/ros2-qa-how-to-setup-identical-robots-for-multi-robot-navigation-236/
https://answers.ros.org/question/388550/a-definitive-guide-to-launching-multi-robot-operations-in-ros2/