Fix multiple robot spawn

docker/.env

@@ -1,2 +1,2 @@
 SERIAL_PORT=/dev/ttyUSB0
-ROS_NAMESPACE=robot1
+ROS_NAMESPACE=robot1

docker/Dockerfile

@@ -14,9 +14,8 @@ RUN apt-get update && apt-get install -y \
         ros-${ROS_DISTRO}-teleop-twist-keyboard
 
 RUN vcs import src < src/rosbot/rosbot_hardware.repos && \
-    # cp -r src/ros2_controllers/diff_drive_controller src/ && \
-    # cp -r src/ros2_controllers/imu_sensor_broadcaster src/ && \
-    # rm -rf src/ros2_controllers && \
+    cp -r src/ros2_controllers/imu_sensor_broadcaster src/ && \
+    rm -rf src/ros2_controllers && \
     rm -r src/rosbot_gazebo
 
 # Create a script to install runtime dependencies for final image
@@ -29,4 +28,4 @@ RUN apt update && \
     source /opt/ros/$ROS_DISTRO/setup.bash && \
     colcon build --cmake-args -DCMAKE_BUILD_TYPE=Release  && \
     echo $(cat /ros2_ws/src/rosbot/package.xml | grep '<version>' | sed -r 's/.*<version>([0-9]+.[0-9]+.[0-9]+)<\/version>/\1/g') >> /version.txt && \
-    rm -rf build log src
+    rm -rf build log src

docker/compose.yaml

@@ -16,4 +16,4 @@ services:
         mecanum:=${MECANUM:-False}
         serial_port:=$SERIAL_PORT
         serial_baudrate:=576000
-    # namespace:=${ROS_NAMESPACE:-rosbot}
+    # namespace:=${ROS_NAMESPACE:-rosbot}

docker/justfile

@@ -86,4 +86,4 @@ _install-yq:
         curl -L https://github.com/mikefarah/yq/releases/download/${YQ_VERSION}/yq_linux_${YQ_ARCH} -o /usr/bin/yq
         chmod +x /usr/bin/yq
         echo "yq installed successfully!"
-    fi
+    fi

rosbot/rosbot_hardware.repos

@@ -19,3 +19,7 @@ repositories:
     type: git
     url: https://github.com/micro-ROS/micro-ROS-Agent.git
     version: humble
+  ros2_controllers: # Bug: There is no nice way to change `sensor_name` imu_bradcaster param when spawning multiple robots -> ros2_control refer only to single imu entity
+    type: git
+    url: https://github.com/husarion/ros2_controllers/
+    version: 9da42a07a83bbf3faf5cad11793fff22f25068af

rosbot_controller/launch/controller.launch.py

@@ -27,6 +27,7 @@
 from launch_ros.actions import Node, SetParameter
 from launch_ros.substitutions import FindPackageShare
 
+
 def generate_launch_description():
     namespace = LaunchConfiguration("namespace")
     declare_namespace_arg = DeclareLaunchArgument(

rosbot_controller/test/test_xacro.py

@@ -45,6 +45,5 @@ def test_rosbot_description_parsing():
             xacro.process_file(xacro_path, mappings=mappings)
         except xacro.XacroException as e:
             assert False, (
-                f"xacro parsing failed: {str(e)} for mecanum: {mecanum}, use_sim:"
-                f" {use_sim}"
+                f"xacro parsing failed: {str(e)} for mecanum: {mecanum}, use_sim:" f" {use_sim}"
             )

rosbot_gazebo/config/gz_remappings.yaml

@@ -4,48 +4,58 @@
     gz_type_name: ignition.msgs.Clock
     direction: GZ_TO_ROS
 
+  - topic_name: <robot_namespace>/cmd_vel
+    ros_type_name: geometry_msgs/msg/Twist
+    gz_type_name: ignition.msgs.Twist
+    direction: GZ_TO_ROS
+
   - topic_name: <robot_namespace>/scan
     ros_type_name: sensor_msgs/msg/LaserScan
     gz_type_name: ignition.msgs.LaserScan
+    direction: GZ_TO_ROS
 
   - topic_name: <robot_namespace>/camera/color/camera_info
     ros_type_name: sensor_msgs/msg/CameraInfo
     gz_type_name: ignition.msgs.CameraInfo
-    lazy: true
+    direction: GZ_TO_ROS
 
   - topic_name: <robot_namespace>/camera/color/image_raw
     ros_type_name: sensor_msgs/msg/Image
     gz_type_name: ignition.msgs.Image
-    lazy: true
+    direction: GZ_TO_ROS
 
   - topic_name: <robot_namespace>/camera/depth/camera_info
     ros_type_name: sensor_msgs/msg/CameraInfo
     gz_type_name: ignition.msgs.CameraInfo
-    lazy: true
+    direction: GZ_TO_ROS
 
   - topic_name: <robot_namespace>/camera/depth/image_raw
     ros_type_name: sensor_msgs/msg/Image
     gz_type_name: ignition.msgs.Image
-    lazy: true
+    direction: GZ_TO_ROS
 
   - ros_topic_name: <robot_namespace>/camera/depth/points
     gz_topic_name: <robot_namespace>/camera/depth/image_raw/points
     ros_type_name: sensor_msgs/msg/PointCloud2
     gz_type_name: ignition.msgs.PointCloudPacked
-    lazy: true
+    direction: GZ_TO_ROS
 
   - topic_name: <robot_namespace>/range/fl
     ros_type_name: sensor_msgs/msg/LaserScan
     gz_type_name: ignition.msgs.LaserScan
+    direction: GZ_TO_ROS
 
   - topic_name: <robot_namespace>/range/fr
     ros_type_name: sensor_msgs/msg/LaserScan
     gz_type_name: ignition.msgs.LaserScan
+    direction: GZ_TO_ROS
 
   - topic_name: <robot_namespace>/range/rl
     ros_type_name: sensor_msgs/msg/LaserScan
     gz_type_name: ignition.msgs.LaserScan
+    direction: GZ_TO_ROS
 
   - topic_name: <robot_namespace>/range/rr
     ros_type_name: sensor_msgs/msg/LaserScan
     gz_type_name: ignition.msgs.LaserScan
+    direction: GZ_TO_ROS

rosbot_gazebo/launch/simulation.launch.py

@@ -13,13 +13,7 @@
 # limitations under the License.
 
 from launch import LaunchDescription
-from launch.actions import (
-    DeclareLaunchArgument,
-    GroupAction,
-    IncludeLaunchDescription,
-    LogInfo,
-    OpaqueFunction,
-)
+from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription, TimerAction
 from launch.launch_description_sources import PythonLaunchDescriptionSource
 from launch.substitutions import (
     EnvironmentVariable,
@@ -32,34 +26,46 @@
 from nav2_common.launch import ParseMultiRobotPose
 
 
-def launch_setup(context, *args, **kwargs):
-    namespace = LaunchConfiguration("namespace").perform(context)
-    mecanum = LaunchConfiguration("mecanum").perform(context)
-    world = LaunchConfiguration("world").perform(context)
-    headless = LaunchConfiguration("headless").perform(context)
-    x = LaunchConfiguration("x", default="0.0").perform(context)
-    y = LaunchConfiguration("y", default="2.0").perform(context)
-    z = LaunchConfiguration("z", default="0.0").perform(context)
-    roll = LaunchConfiguration("roll", default="0.0").perform(context)
-    pitch = LaunchConfiguration("pitch", default="0.0").perform(context)
-    yaw = LaunchConfiguration("yaw", default="0.0").perform(context)
+def generate_launch_description():
+    namespace = LaunchConfiguration("namespace")
+    mecanum = LaunchConfiguration("mecanum")
+    x = LaunchConfiguration("x", default="0.0")
+    y = LaunchConfiguration("y", default="2.0")
+    z = LaunchConfiguration("z", default="0.0")
+    roll = LaunchConfiguration("roll", default="0.0")
+    pitch = LaunchConfiguration("pitch", default="0.0")
+    yaw = LaunchConfiguration("yaw", default="0.0")
+
+    declare_namespace_arg = DeclareLaunchArgument(
+        "namespace",
+        default_value=EnvironmentVariable("ROBOT_NAMESPACE", default_value=""),
+        description="Namespace for all topics and tfs",
+    )
+
+    declare_mecanum_arg = DeclareLaunchArgument(
+        "mecanum",
+        default_value="False",
+        description=(
+            "Whether to use mecanum drive controller (otherwise diff drive controller is used)"
+        ),
+    )
 
-    gz_args = f"--headless-rendering -s -v 4 -r {world}" if eval(headless) else f"-r {world}"
+    declare_robots_arg = DeclareLaunchArgument(
+        "robots",
+        default_value="",
+        description=(
+            "Spawning multiple robots at positions with yaw orientations e.g."
+            "robots:='robot1={x: 0.0, y: -1.0}; robot2={x: 1.0, y: -1.0}; robot3={x: 2.0, y: -1.0};'"
+        ),
+    )
 
     gz_sim = IncludeLaunchDescription(
         PythonLaunchDescriptionSource(
             PathJoinSubstitution(
-                [
-                    FindPackageShare("ros_gz_sim"),
-                    "launch",
-                    "gz_sim.launch.py",
-                ]
+                [FindPackageShare("husarion_gz_worlds"), "launch", "gz_sim.launch.py"]
             )
         ),
-        launch_arguments={
-            "gz_args": gz_args,
-            "on_exit_shutdown": "True",
-        }.items(),
+        launch_arguments={"gz_log_level": "1"}.items(),
     )
 
     robots_list = ParseMultiRobotPose("robots").value()
@@ -74,94 +80,47 @@ def launch_setup(context, *args, **kwargs):
                 "yaw": yaw,
             }
         }
+    else:
+        for robot_name in robots_list:
+            init_pose = robots_list[robot_name]
+            for key, value in init_pose.items():
+                init_pose[key] = TextSubstitution(text=str(value))
 
     spawn_group = []
-    for robot_name in robots_list:
+    for idx, robot_name in enumerate(robots_list):
         init_pose = robots_list[robot_name]
-        group = GroupAction(
-            [
-                LogInfo(
-                    msg=[
-                        "Launching namespace=",
-                        robot_name,
-                        " init_pose=",
-                        str(init_pose),
+        spawn_robot = IncludeLaunchDescription(
+            PythonLaunchDescriptionSource(
+                PathJoinSubstitution(
+                    [
+                        FindPackageShare("rosbot_gazebo"),
+                        "launch",
+                        "spawn.launch.py",
                     ]
-                ),
-                IncludeLaunchDescription(
-                    PythonLaunchDescriptionSource(
-                        PathJoinSubstitution(
-                            [
-                                FindPackageShare("rosbot_gazebo"),
-                                "launch",
-                                "spawn.launch.py",
-                            ]
-                        )
-                    ),
-                    launch_arguments={
-                        "mecanum": mecanum,
-                        "use_sim": "True",
-                        "namespace": TextSubstitution(text=robot_name),
-                        "x": TextSubstitution(text=str(init_pose["x"])),
-                        "y": TextSubstitution(text=str(init_pose["y"])),
-                        "z": TextSubstitution(text=str(init_pose["z"])),
-                        "roll": TextSubstitution(text=str(init_pose["roll"])),
-                        "pitch": TextSubstitution(text=str(init_pose["pitch"])),
-                        "yaw": TextSubstitution(text=str(init_pose["yaw"])),
-                    }.items(),
-                ),
-            ]
+                )
+            ),
+            launch_arguments={
+                "mecanum": mecanum,
+                "use_sim": "True",
+                "namespace": robot_name,
+                "x": init_pose["x"],
+                "y": init_pose["y"],
+                "z": init_pose["z"],
+                "roll": init_pose["roll"],
+                "pitch": init_pose["pitch"],
+                "yaw": init_pose["yaw"],
+            }.items(),
         )
-        spawn_group.append(group)
-
-    return [gz_sim, *spawn_group]
-
-
-def generate_launch_description():
-    declare_namespace_arg = DeclareLaunchArgument(
-        "namespace",
-        default_value=EnvironmentVariable("ROBOT_NAMESPACE", default_value=""),
-        description="Namespace for all topics and tfs",
-    )
-
-    declare_mecanum_arg = DeclareLaunchArgument(
-        "mecanum",
-        default_value="False",
-        description=(
-            "Whether to use mecanum drive controller (otherwise diff drive controller is used)"
-        ),
-    )
-
-    world_package = FindPackageShare("husarion_gz_worlds")
-    world_file = PathJoinSubstitution([world_package, "worlds", "husarion_world.sdf"])
-    declare_world_arg = DeclareLaunchArgument(
-        "world", default_value=world_file, description="SDF world file"
-    )
-
-    declare_headless_arg = DeclareLaunchArgument(
-        "headless",
-        default_value="False",
-        description="Run Gazebo Ignition in the headless mode",
-        choices=["True", "False"],
-    )
-
-    declare_robots_arg = DeclareLaunchArgument(
-        "robots",
-        default_value="",
-        description=(
-            "Spawning multiple robots at positions with yaw orientations e. g. robots:='robot1={x:"
-            " 0.0, y: -1.0}; robot2={x: 1.0, y: -1.0}; robot3={x: 2.0, y: -1.0};'"
-        ),
-    )
+        spawn_robot_delay = TimerAction(period=5.0 * idx, actions=[spawn_robot])
+        spawn_group.append(spawn_robot_delay)
 
     return LaunchDescription(
         [
             declare_namespace_arg,
             declare_mecanum_arg,
-            declare_world_arg,
-            declare_headless_arg,
             declare_robots_arg,
             SetParameter(name="use_sim_time", value=True),
-            OpaqueFunction(function=launch_setup),
+            gz_sim,
+            *spawn_group,
         ]
     )