diff --git a/README.md b/README.md index d65bfc9..c71fb67 100644 --- 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 index 18e0ecf..bac84e8 100644 --- 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) + ]) \ No newline at end of file diff --git a/discower_launch/launch/sitl_multi_agent_kth.launch.py b/discower_launch/launch/sitl_multi_agent_kth.launch.py new file mode 100644 index 0000000..04a4271 --- /dev/null +++ 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 \ No newline at end of file diff --git a/discower_launch/launch/sitl_single_agent_kth.launch.py b/discower_launch/launch/sitl_single_agent_kth.launch.py new file mode 100644 index 0000000..e3473fb --- /dev/null +++ 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() + ) + ]) \ No newline at end of file diff --git a/setup.py b/setup.py index 6699d8c..4754c6d 100644 --- a/setup.py +++ b/setup.py @@ -19,6 +19,8 @@ maintainer_email='jorisv@kth.se', 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={}, )