Support for multi robot simulation

README.md

@@ -55,29 +55,61 @@ Once the package is built and sourced, you can start a simulation.
 
 _Note: You can use `--world_name` flag to indicate other [world](andino_gz/worlds/) to use. (For example: `depot.sdf`(default), `empty.sdf`)_
 
-If you'd like to work from ROS you can launch the ros bridge by adding the corresponding flag
+By default the ros bridge and rviz is initialized. In case you prefer to disable any of those you can do it via its flags:
 
   ```sh
-  ros2 launch andino_gz andino_gz.launch.py ros_bridge:=true
+  ros2 launch andino_gz andino_gz.launch.py ros_bridge:=false rviz:=false
   ```
 
-(Optional) Or launching it separately via:
-
+To see a complete list of available arguments for the launch file do:
   ```sh
-  ros2 launch andino_gz gz_ros_bridge.launch.py
+  ros2 launch andino_gz andino_gz.launch.py --show-args
   ```
 
 Make sure to review the required topics using `ign topics` and `ros2 topic` CLI tools.
 Also, consider using looking at the translation entries under `andino_gz/config/bridge_config.yaml`.
 
+### Multi robot simulation
+
+  This simulation also support multi robot simulation.
+
+  ```sh
+  ros2 launch andino_gz andino_gz.launch.py robots:="
+       andino1={x: 0.0, y: 0.0, z: 0.1, yaw: 0.};
+       andino2={x: -0.4, y: 0.1, z: 0.1, yaw: 0.};
+       andino3={x: -0.4, y: -0.1, z: 0.1, yaw: 0.};
+       andino4={x: -0.8, y: 0.2, z: 0.1, yaw: 0.};
+       andino5={x: -0.8, y: -0.2, z: 0.1, yaw: 0.};
+       andino6={x: -0.8, y: 0.0, z: 0.1, yaw: 0.};"
+  ```
+
+  _Note: You can add as many as you want_
+
+  <img src="./docs/media/andino_gz_multi_robot.png" width="800"/>
+
+  The launch file is in charge of:
+   - Start Gazebo simulator with a defined world (See '--world_name' flag)
+   - Spawn as many robots as commanded.
+   - Launch ros bridge for each robot.
+   - Launch rviz visualization for each robot.
+
+  The simulation allows spawn as many robots as you want via the `--robots` flags.
+  For that you can pass the information of the robots in some sort of YAML format via ROS2 cli:
+  ```yaml
+    <robot_name>={x: 0.0, y: 0.0, yaw: 0.0, roll: 0.0, pitch: 0.0, yaw: 0.0};
+  ```
+
+  Note a ROS Namespace is pushed for each robot so all the topics and nodes are called the same with a difference of a `<robot_name>` prefix.
+
+
 ### SLAM
 
 <img src="./docs/media/andino_slam.png" width="800"/>
 
 1. Run simulation with ros bridge and RViz.
 
     ```sh
-    ros2 launch andino_gz andino_gz.launch.py ros_bridge:=true rviz:=true
+    ros2 launch andino_gz andino_gz.launch.py
     ```
 
 2. Run slam toolbox
@@ -93,22 +125,6 @@ Also, consider using looking at the translation entries under `andino_gz/config/
 
 3. Visualize in RViz: Add `map` panel to RViz and see how the map is being generated.
 
-### Spawn multiple Andinos
-
-Launch simulation as before:
-  ```sh
-  ros2 launch andino_gz andino_gz.launch.py
-  ```
-
-This will spawn only one Andino in the simulation
-
-For spawning more Andinos you can use the `spawn_robot` launch file. Make sure a different `entity` name is passed as argument as well as initial positions.
-  ```sh
-  ros2 launch andino_gz spawn_robot.launch.py entity:=andino_n initial_pose_x:=1 initial_pose_y:=1
-  ```
-
-_Note: Andino is spawned but no bridge to ROS2 bridge are spawned for those robots._
-
 ## :raised_hands: Contributing
 
 Issues or PRs are always welcome! Please refer to [CONTRIBUTING](CONTRIBUTING.md) doc.

andino_gz/config/bridge_config.yaml

@@ -1,46 +1,44 @@
-- ros_topic_name: "/clock"
-  gz_topic_name: "/clock"
-  ros_type_name: "rosgraph_msgs/msg/Clock"
-  gz_type_name: "gz.msgs.Clock"
-  direction: GZ_TO_ROS
-- ros_topic_name: "/odom"
-  gz_topic_name: "/model/andino/odometry"
+# Placeholders to be rewritten by launch file:
+# - <entity> : Should be replaced by entity name: For example "andino", "andino_1", "andino_2".
+
+- ros_topic_name: "odom"
+  gz_topic_name: "/model/<entity>/odometry"
   ros_type_name: "nav_msgs/msg/Odometry"
   gz_type_name: "gz.msgs.Odometry"
   direction: GZ_TO_ROS
 # odom <-> base_link transform
-- ros_topic_name: "/tf"
-  gz_topic_name: "/model/andino/pose"
+- ros_topic_name: "tf"
+  gz_topic_name: "/model/<entity>/pose"
   ros_type_name: "tf2_msgs/msg/TFMessage"
   gz_type_name: "gz.msgs.Pose_V"
   direction: GZ_TO_ROS
-- ros_topic_name: "/joint_states"
-  gz_topic_name: "/world/gazebo_world/model/andino/joint_state"
+- ros_topic_name: "joint_states"
+  gz_topic_name: "/world/gazebo_world/model/<entity>/joint_state"
   ros_type_name: "sensor_msgs/msg/JointState"
   gz_type_name: "gz.msgs.Model"
   direction: GZ_TO_ROS
-- ros_topic_name: "/camera_info"
-  gz_topic_name: "/world/gazebo_world/model/andino/link/base_link/sensor/camera/camera_info"
+- ros_topic_name: "camera_info"
+  gz_topic_name: "/world/gazebo_world/model/<entity>/link/base_link/sensor/camera/camera_info"
   ros_type_name: "sensor_msgs/msg/CameraInfo"
   gz_type_name: "gz.msgs.CameraInfo"
   direction: GZ_TO_ROS
-- ros_topic_name: "/image_raw"
-  gz_topic_name: "/world/gazebo_world/model/andino/link/base_link/sensor/camera/image"
+- ros_topic_name: "image_raw"
+  gz_topic_name: "/world/gazebo_world/model/<entity>/link/base_link/sensor/camera/image"
   ros_type_name: "sensor_msgs/msg/Image"
   gz_type_name: "gz.msgs.Image"
   direction: GZ_TO_ROS
-- ros_topic_name: "/scan"
-  gz_topic_name: "/world/gazebo_world/model/andino/link/base_link/sensor/sensor_ray_front/scan"
+- ros_topic_name: "scan"
+  gz_topic_name: "/world/gazebo_world/model/<entity>/link/base_link/sensor/sensor_ray_front/scan"
   ros_type_name: "sensor_msgs/msg/LaserScan"
   gz_type_name: "gz.msgs.LaserScan"
   direction: GZ_TO_ROS
-- ros_topic_name: "/scan/points"
-  gz_topic_name: "/world/gazebo_world/model/andino/link/base_link/sensor/sensor_ray_front/scan/points"
+- ros_topic_name: "scan/points"
+  gz_topic_name: "/world/gazebo_world/model/<entity>/link/base_link/sensor/sensor_ray_front/scan/points"
   ros_type_name: "sensor_msgs/msg/PointCloud2"
   gz_type_name: "gz.msgs.PointCloudPacked"
   direction: GZ_TO_ROS
-- ros_topic_name: "/cmd_vel"
-  gz_topic_name: "/model/andino/cmd_vel"
+- ros_topic_name: "cmd_vel"
+  gz_topic_name: "/model/<entity>/cmd_vel"
   ros_type_name: "geometry_msgs/msg/Twist"
   gz_type_name: "gz.msgs.Twist"
   direction: ROS_TO_GZ

andino_gz/launch/andino_gz.launch.py

@@ -3,75 +3,131 @@
 import os
 from ament_index_python.packages import get_package_share_directory
 from launch import LaunchDescription
-from launch.actions import DeclareLaunchArgument, ExecuteProcess, IncludeLaunchDescription
+from launch.actions import DeclareLaunchArgument, GroupAction, IncludeLaunchDescription, LogInfo
 from launch.conditions import IfCondition
 from launch.launch_description_sources import PythonLaunchDescriptionSource
-from launch.substitutions import LaunchConfiguration, PathJoinSubstitution
-from launch_ros.actions import Node
-import xacro
+from launch.substitutions import LaunchConfiguration, PathJoinSubstitution, PythonExpression, TextSubstitution
+from launch_ros.actions import Node, PushRosNamespace
+
+from nav2_common.launch import ParseMultiRobotPose
 
 
 def generate_launch_description():
     pkg_andino_gz = get_package_share_directory('andino_gz')
 
     ros_bridge_arg = DeclareLaunchArgument(
-        'ros_bridge', default_value='false', description = 'Run ROS bridge node.')
-    rviz_arg = DeclareLaunchArgument('rviz', default_value='false', description='Start RViz.')
-    world_name_arg = DeclareLaunchArgument('world_name', default_value='depot.sdf', description='Name of the world to load.')
+        'ros_bridge', default_value='true', description='Run ROS bridge node.')
+    rviz_arg = DeclareLaunchArgument('rviz', default_value='true', description='Start RViz.')
+    world_name_arg = DeclareLaunchArgument(
+        'world_name', default_value='depot.sdf', description='Name of the world to load.')
+    robots_arg = DeclareLaunchArgument(
+        'robots', default_value="andino={x: 0., y: 0., z: 0.1, yaw: 0.};",
+        description='Robots to spawn, multiple robots can be stated separated by a ; ')
 
+    # Variables of launch file.
+    rviz = LaunchConfiguration('rviz')
+    ros_bridge = LaunchConfiguration('ros_bridge')
     world_name = LaunchConfiguration('world_name')
-    world_path = PathJoinSubstitution([pkg_andino_gz, 'worlds', world_name])
 
-    # Gazebo Sim
-    gazebo = IncludeLaunchDescription(
-        PythonLaunchDescriptionSource(
-            os.path.join(get_package_share_directory('ros_gz_sim'), 'launch', 'gz_sim.launch.py')
-        ),
-        launch_arguments={'gz_args': world_path}.items(),
-    )
-
-    # Spawn the robot and the Robot State Publisher node.
-    spawn_robot_and_rsp = IncludeLaunchDescription(
-        PythonLaunchDescriptionSource(
-            os.path.join(pkg_andino_gz, 'launch', 'spawn_robot.launch.py')
-        ),
-        launch_arguments={
-            'entity': 'andino',
-            'initial_pose_x': '0',
-            'initial_pose_y': '0',
-            'initial_pose_z': '0.1',
-            'initial_pose_yaw': '0',
-            'robot_description_topic': '/robot_description',
-            'use_sim_time': 'true',
-        }.items(),
-    )
+    # Obtains world path.
+    world_path = PathJoinSubstitution([pkg_andino_gz, 'worlds', world_name])
 
-    # RViz
-    rviz = Node(
-        package='rviz2',
-        executable='rviz2',
-        arguments=['-d', os.path.join(pkg_andino_gz, 'rviz', 'andino_gz.rviz')],
-        parameters=[{'use_sim_time': True}],
-        condition=IfCondition(LaunchConfiguration('rviz'))
+    base_group = GroupAction(
+        scoped=True, forwarding=False,
+        launch_configurations={
+            'ros_bridge': ros_bridge,
+            'world_name': world_name
+        },
+        actions=[
+            # Gazebo Sim
+            IncludeLaunchDescription(
+                PythonLaunchDescriptionSource(
+                    os.path.join(get_package_share_directory('ros_gz_sim'), 'launch', 'gz_sim.launch.py')
+                ),
+                launch_arguments={'gz_args': world_path}.items(),
+            ),
+            # ROS Bridge for generic Gazebo stuff
+            Node(
+                package='ros_gz_bridge',
+                executable='parameter_bridge',
+                arguments=['/clock@rosgraph_msgs/msg/Clock[ignition.msgs.Clock'],
+                output='screen',
+                namespace='andino_gz_sim',
+                condition=IfCondition(ros_bridge),
+            ),
+        ]
     )
 
-    # Run ros_gz bridge
-    ros_bridge = IncludeLaunchDescription(
-        PythonLaunchDescriptionSource(
-            os.path.join(pkg_andino_gz, 'launch', 'gz_ros_bridge.launch.py')
-        ),
-        condition=IfCondition(LaunchConfiguration('ros_bridge'))
-    )
+    robots_list = ParseMultiRobotPose('robots').value()
+    # When no robots are specified, spawn a single robot at the origin.
+    # The default value isn't getting parsed correctly, so we need to check for an empty dictionary.
+    if (robots_list == {}):
+        robots_list = {"andino": {"x": 0., "y": 0., "z": 0.1, "yaw": 0.}}
+    log_number_robots = LogInfo(msg="Robots to spawn: " + str(robots_list))
+    spawn_robots_group = []
+    for robot_name in robots_list:
+        init_pose = robots_list[robot_name]
+        # As it is scoped and not forwarding, the launch configuration in this context gets cleared.
+        group = GroupAction(
+            scoped=True, forwarding=False,
+            launch_configurations={
+                'rviz': rviz,
+                'ros_bridge': ros_bridge
+            },
+            actions=[
+                LogInfo(msg="Group for robot: " + robot_name),
+                PushRosNamespace(
+                    condition=IfCondition(
+                        PythonExpression([TextSubstitution(text=str(len(robots_list.keys()))), ' > 1'])),
+                    namespace=robot_name),
+                # Spawn the robot and the Robot State Publisher node.
+                IncludeLaunchDescription(
+                    PythonLaunchDescriptionSource(
+                        os.path.join(pkg_andino_gz, 'launch', 'include', 'spawn_robot.launch.py')
+                    ),
+                    launch_arguments={
+                        'entity': robot_name,
+                        'initial_pose_x': str(init_pose['x']),
+                        'initial_pose_y': str(init_pose['y']),
+                        'initial_pose_z': str(init_pose['z']),
+                        'initial_pose_yaw': str(init_pose['yaw']),
+                        'robot_description_topic': 'robot_description',
+                        'use_sim_time': 'true',
+                    }.items(),
+                ),
+                # RViz
+                Node(
+                    condition=IfCondition(rviz),
+                    package='rviz2',
+                    executable='rviz2',
+                    arguments=['-d', os.path.join(pkg_andino_gz, 'rviz', 'andino_gz.rviz')],
+                    parameters=[{'use_sim_time': True}],
+                    remappings=[
+                        ('/tf', 'tf'),
+                        ('/tf_static', 'tf_static'),
+                    ],
+                ),
+                # Run ros_gz bridge
+                IncludeLaunchDescription(
+                    PythonLaunchDescriptionSource(
+                        os.path.join(pkg_andino_gz, 'launch', 'include', 'gz_ros_bridge.launch.py')
+                    ),
+                    launch_arguments={
+                        'entity': robot_name,
+                    }.items(),
+                    condition=IfCondition(LaunchConfiguration('ros_bridge')),
+                )
+            ]
+        )
+        spawn_robots_group.append(group)
 
-    return LaunchDescription(
-        [
-            # Arguments and Nodes
-            ros_bridge_arg,
-            rviz_arg,
-            world_name_arg,
-            gazebo,
-            ros_bridge,
-            spawn_robot_and_rsp,
-            rviz,
-        ]
-    )
+    ld = LaunchDescription()
+    ld.add_action(log_number_robots)
+    ld.add_action(ros_bridge_arg)
+    ld.add_action(rviz_arg)
+    ld.add_action(world_name_arg)
+    ld.add_action(robots_arg)
+    ld.add_action(base_group)
+    for group in spawn_robots_group:
+        ld.add_action(group)
+    return ld