DISCOWER · Pedro-Roque · Jul 3, 2025 · May 19, 2025 · May 19, 2025 · May 27, 2025
diff --git a/README.md b/README.md
@@ -1,42 +1,82 @@
 # discower_launch: SITL launch files for Single and Multi-agent experiments
 
-This repository contains launch files indicating how a single and multiple ATMOS space platform SITL simulation can be started.
-The dependencies for launching the SITL simulation are
+This repository contains launch files for running Single and Multi-agent ATMOS platform SITL simulations.
+
+## Dependencies
+
 - [PX4-Space-Systems](https://github.com/DISCOWER/PX4-Space-Systems)
 - [px4_msgs](https://github.com/DISCOWER/px4_msgs)
 
 As this repository contains simple examples to merely run the SITL, you might need to follow additional instructions in the `PX4-Space-Systems` repository to fully get started. In short, you additionally need to
+
 - build the workspace: `colcon build --symlink-install`
-- start the microros service: `ros2 run micro_ros_agent micro_ros_agent udp4 --port 8888`
-after which you can display the robot's topics with `ros2 topic list`. Then, you can arm/disarm the robot with [QGroundControl](https://github.com/DISCOWER/qgroundcontrol). 
+- start the microros service: `micro-xrce-dds-agent udp4 -p 8888`
+
+After that, you can display the robot's topics with `ros2 topic list`. Then, you can arm/disarm the robot with [QGroundControl](https://github.com/DISCOWER/qgroundcontrol). 
 
 Further details can be found on the [ATMOS website](https://atmos.discower.io/pages/Simulation/)
 
+## Launch files overview
+
+This package provides four main launch files:
+
+- `sitl_single_agent.launch.py`: Launches a single agent on the default PX4 world.
+- `sitl_multi_agent.launch.py`: Launches multiple agents on the default PX4 world.
+- `sitl_single_agent_kth.launch.py`: Launches a single agent in the KTH Space Lab world **with** Gazebo-ROS odometry bridging enabled.
+- `sitl_multi_agent_kth.launch.py`: Launches multiple agents in the KTH Space Lab world **with** Gazebo-ROS odometry bridging enabled.
+
+> **Note:** Gazebo-ROS odometry bridging requires an additional ROS package depending on your ROS2 distribution:
+>
+> - **ROS2 Humble:**
+>
+>   ```bash
+>   sudo apt install ros-humble-ros-gzharmonic-bridge
+>   ```
+>
+> - **ROS2 Jazzy:**
+>
+>   ```bash
+>   sudo apt install ros-jazzy-ros-gz-bridge
+>   ```
+
+The bridged odometry simulates the motion capture system used in the real KTH Space Lab and publishes ground-truth data from Gazebo to ROS.
+
 ## Testing a Multi-Agent setup
 
 First make sure that your environment variable for `PX4_SPACE_SYSTEMS_DIR` is set. This can be checked with:
+
 ```bash
 echo $PX4_SPACE_SYSTEMS_DIR
 ```
-If this is not set, you can set it at the end of your .bashrc (or .zshrc) file with:
+
+If this is not set, you can set it at the end of your `.bashrc` (or `.zshrc`) file with:
+
 ```bash
 export PX4_SPACE_SYSTEMS_DIR=/path/to/your/PX4-Space-Systems
 ```
 
 Then, test the multi-agent setup by running the following command:
+
 ```bash
 ros2 launch discower_launch sitl_multi_agent.launch.py
 ```
 
-## Adding extra ATMOS platforms 
+> Alternatively, if you want to simulate in the `kthspacelab` world and use the Gazebo-ROS bridge, you can run:
+
+```bash
+ros2 launch discower_launch sitl_multi_agent_kth.launch.py
+```
+
+## Adding extra ATMOS platforms
+
+In the launch file, for each vehicle, you can set the namespaces and pose for each vehicle. To add an extra vehicle, add another instance of the `px4.launch.py` with a different `id` and with five extra seconds of delay. Make sure to also change the `pose` variable to avoid agents being deployed in the same position. An example can be found below:
 
-In the launch file, for each vehicle, you can set the namespaces and pose for each vehicle. To add an extra vehicle, add another instance of the `px4.launch.py` with different `id` and with five extra seconds of delay. Make sure to also change the pose variable to avoid agents being deployd in the same position. An example can be found below:
 ```python
 lf_3 = IncludeLaunchDescription(
         PythonLaunchDescriptionSource(
             [get_package_share_directory('discower_launch'), '/px4.launch.py']),
 
-        launch_arguments={'id': '3', 'pose': '-1.0, 1.75, 0', 'name': 'snap', 'delay': '10'}.items()
+        launch_arguments={'id': '3', 'pose': '-1.0, 1.75, 0', 'name': 'snap', 'delay': '10', 'world': 'kthspacelab'}.items()
     )
 
 ld.add_action(lf_3)

diff --git a/discower_launch/launch/px4.launch.py b/discower_launch/launch/px4.launch.py
@@ -1,44 +1,69 @@
 #!/usr/bin/env python3
 from launch import LaunchDescription
-from launch.actions import ExecuteProcess, DeclareLaunchArgument
+from launch.actions import DeclareLaunchArgument, OpaqueFunction, ExecuteProcess
 from launch.substitutions import LaunchConfiguration
 import os
 
-def generate_launch_description():
-    """Launch Gazebo with a freeflyer running PX4 communicating over ROS 2."""
-
+def launch_px4(context, *args, **kwargs):
     px4_dir = os.getenv("PX4_SPACE_SYSTEMS_DIR")
     if not px4_dir:
         raise RuntimeError("PX4_SPACE_SYSTEMS_DIR is not set. Did you add it to your .bashrc file?")
 
-    return LaunchDescription(
-        [
-            # We have the first robot always start with id 0
-            DeclareLaunchArgument("id", default_value="0"),
-            DeclareLaunchArgument("pose", default_value="0,0,0"),
-            DeclareLaunchArgument("name", default_value="snap"),
-            DeclareLaunchArgument("delay", default_value="0"),
-            DeclareLaunchArgument("headless", default_value="1"),
+    id_ = LaunchConfiguration("id").perform(context)
+    pose = LaunchConfiguration("pose").perform(context)
+    name = LaunchConfiguration("name").perform(context)
+    delay = LaunchConfiguration("delay").perform(context)
+    model = LaunchConfiguration("model").perform(context) if "model" in context.launch_configurations else "spacecraft_2d"
+    world = LaunchConfiguration("world").perform(context)
+
+    model_name = f"{model}_{id_}"
+
+    use_odom_bridge = LaunchConfiguration("use_odom_bridge").perform(context).lower() == "true"
+
+    processes = [
+        ExecuteProcess(
+            cmd=[
+                "bash", "-c",
+                f"sleep {delay} && {px4_dir}/build/px4_sitl_default/bin/px4 -i {id_}"
+            ],
+            cwd=px4_dir,
+            env={
+                **os.environ,
+                "PX4_SIM_AUTOSTART": "71002",
+                "PX4_SIM_SPEED_FACTOR": "1",
+                "PX4_GZ_MODEL_POSE": pose,
+                "PX4_INSTANCE": id_,
+                "PX4_DELAY": delay,
+                "PX4_SIM_MODEL": f"gz_{model}",
+                "PX4_UXRCE_DDS_NS": name,
+                "PX4_GZ_WORLD": world
+            },
+            output="screen"
+        )
+    ]
+
+    if use_odom_bridge:
+        processes.append(
             ExecuteProcess(
                 cmd=[
-                    "xterm",        # or "gnome-terminal", "konsole", "xterm"
-                    "-hold",        # Keep terminal open for debugging
-                    "-e",
-                    "bash",
-                    "-c",
-                    "sleep $PX4_DELAY && " +px4_dir+"/build/px4_sitl_default/bin/px4 -i $PX4_INSTANCE",
+                    "ros2", "run", "ros_gz_bridge", "parameter_bridge",
+                    f"/model/{model_name}/odometry@nav_msgs/msg/Odometry[gz.msgs.Odometry",
+                    "--ros-args", "-r", f"/model/{model_name}/odometry:=/{name}/odom"
                 ],
-                cwd=px4_dir,
-                env={**os.environ,
-                    "PX4_SIM_AUTOSTART": "4001",
-                    "PX4_SIM_SPEED_FACTOR": "1",
-                    "PX4_GZ_MODEL_POSE": LaunchConfiguration("pose"),
-                    "PX4_INSTANCE": LaunchConfiguration("id"),
-                    "PX4_DELAY": LaunchConfiguration("delay"),
-                    "PX4_SIM_MODEL": "gz_spacecraft_2d",
-                    "PX4_UXRCE_DDS_NS": LaunchConfiguration("name")},
-                    # "HEADLESS": LaunchConfiguration("headless")},
-                output="screen",
-            ),
-        ]
-    )
+                output="screen"
+            )
+        )
+
+    return processes
+
+def generate_launch_description():
+    return LaunchDescription([
+        DeclareLaunchArgument("id", default_value="0"),
+        DeclareLaunchArgument("pose", default_value="0,0,0"),
+        DeclareLaunchArgument("name", default_value="snap"),
+        DeclareLaunchArgument("delay", default_value="0"),
+        DeclareLaunchArgument("world", default_value=""),
+        DeclareLaunchArgument("use_odom_bridge", default_value="false"),
+
+        OpaqueFunction(function=launch_px4)
+    ])
diff --git a/discower_launch/launch/sitl_multi_agent_kth.launch.py b/discower_launch/launch/sitl_multi_agent_kth.launch.py
@@ -0,0 +1,58 @@
+#!/usr/bin/env python
+from ament_index_python.packages import get_package_share_directory
+from launch import LaunchDescription
+from launch.actions import IncludeLaunchDescription
+from launch.launch_description_sources import PythonLaunchDescriptionSource
+
+def generate_launch_description():
+    """Launch multiple spacecrafts in the kthspacelab world with PX4 and ROS 2."""
+    ld = LaunchDescription()
+
+    # Path to the shared px4 launcher
+    px4_launch_path = [get_package_share_directory('discower_launch'), '/px4.launch.py']
+
+    # First agent
+    spacecraft_0 = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(px4_launch_path),
+        launch_arguments={
+            'id': '0',
+            'pose': '1,0,0.2',
+            'name': 'snap',
+            'delay': '0',
+            'world': 'kthspacelab',
+            'use_odom_bridge': 'true',
+        }.items()
+    )
+
+    # Second agent
+    spacecraft_1 = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(px4_launch_path),
+        launch_arguments={
+            'id': '1',
+            'pose': '2,0,0.2',
+            'name': 'crackle',
+            'delay': '5',
+            'world': 'kthspacelab',
+            'use_odom_bridge': 'true',
+        }.items()
+    )
+
+    # Third agent
+    spacecraft_2 = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(px4_launch_path),
+        launch_arguments={
+            'id': '2',
+            'pose': '3,0,0.2',
+            'name': 'pop',
+            'delay': '5',
+            'world': 'kthspacelab',
+            'use_odom_bridge': 'true',
+        }.items()
+    )
+
+    # Add all to launch description
+    ld.add_action(spacecraft_0)
+    ld.add_action(spacecraft_1)
+    ld.add_action(spacecraft_2)
+
+    return ld
diff --git a/discower_launch/launch/sitl_single_agent_kth.launch.py b/discower_launch/launch/sitl_single_agent_kth.launch.py
@@ -0,0 +1,23 @@
+#!/usr/bin/env python
+from ament_index_python.packages import get_package_share_directory
+from launch import LaunchDescription
+from launch.actions import IncludeLaunchDescription
+from launch.launch_description_sources import PythonLaunchDescriptionSource
+
+def generate_launch_description():
+    """Launch Gazebo with a spacecraft in the kthspacelab world and ROS bridge."""
+    px4_launch_path = [get_package_share_directory('discower_launch'), '/px4.launch.py']
+
+    return LaunchDescription([
+        IncludeLaunchDescription(
+            PythonLaunchDescriptionSource(px4_launch_path),
+            launch_arguments={
+                'id': '0',
+                'pose': '2,0,0',
+                'name': 'snap',
+                'delay': '0',
+                'world': 'kthspacelab',
+                'use_odom_bridge': 'true',
+            }.items()
+        )
+    ])
diff --git a/setup.py b/setup.py
@@ -19,6 +19,8 @@
     maintainer_email='[email protected]',
     description='simple SITL launch files for single- and multi-agent simulation of the ATMOS platform',
     license='BSD 3-Clause',
-    tests_require=['pytest'],
+    extras_require={
+        'test': ['pytest']
+    },
     entry_points={},
 )