Labyrinth escape migration to gazebo harmonic

CustomRobots/drone_assets/worlds/labyrinth_escape_harmonic.world

@@ -0,0 +1,122 @@
+<?xml version="1.0"?>
+<sdf version="1.8">
+	<world name="empty">
+		<physics name="4ms" type="ignored">
+			<max_step_size>0.004</max_step_size>
+			<real_time_factor>1.0</real_time_factor>
+		</physics>
+		<plugin
+			filename="gz-sim-physics-system"
+			name="gz::sim::systems::Physics">
+		</plugin>
+		<plugin
+			filename="gz-sim-user-commands-system"
+			name="gz::sim::systems::UserCommands">
+		</plugin>
+		<plugin
+			filename="gz-sim-scene-broadcaster-system"
+			name="gz::sim::systems::SceneBroadcaster">
+		</plugin>
+		<plugin
+			filename="gz-sim-sensors-system"
+			name="gz::sim::systems::Sensors">
+			<render_engine>ogre2</render_engine>
+		</plugin>
+
+		<!--light-->
+		<light type="directional" name="sun">
+			<cast_shadows>true</cast_shadows>
+			<pose>0 0 10 0 0 0</pose>
+			<diffuse>0.8 0.8 0.8 1</diffuse>
+			<specular>0.2 0.2 0.2 1</specular>
+			<attenuation>
+				<range>1000</range>
+				<constant>0.9</constant>
+				<linear>0.01</linear>
+				<quadratic>0.001</quadratic>
+			</attenuation>
+			<direction>-0.5 0.1 -0.9</direction>
+		</light>
+
+		<gui>
+			<camera name="user_camera">
+				<pose>-4.70385 10.895 16.2659 -0 0.921795 -1.12701</pose>
+			</camera>
+		</gui>
+
+		<!-- Arrows -->
+		<include>
+			<uri>model://arrow</uri>
+			<name>arrow_1</name>
+			<pose>-18 -8.5 0.1 0 0 0</pose>
+		</include>
+		<include>
+			<uri>model://arrow</uri>
+			<name>arrow_2</name>
+			<pose>-5.5 -8.5 0.1 0 0 0</pose>
+		</include>
+		<include>
+			<uri>model://arrow</uri>
+			<name>arrow_3</name>
+			<pose>7 -8.5 0.1 0 0 1.57</pose>
+		</include>
+		<include>
+			<uri>model://arrow</uri>
+			<name>arrow_4</name>
+			<pose>7 3 0.1 0 0 1.57</pose>
+		</include>
+		<include>
+			<uri>model://arrow</uri>
+			<name>arrow_5</name>
+			<pose>7 14.5 0.1 0 0 3.14</pose>
+		</include>
+		<include>
+			<uri>model://arrow</uri>
+			<name>arrow_6</name>
+			<pose>2.5 14.5 0.1 0 0 3.14</pose>
+		</include>
+		<include>
+			<uri>model://arrow</uri>
+			<name>arrow_7</name>
+			<pose>2.5 7.5 0.1 0 0 3.14</pose>
+		</include>
+		<include>
+			<uri>model://arrow</uri>
+			<name>arrow_8</name>
+			<pose>-3 7.5 0.1 0 0 1.57</pose>
+		</include>
+		<include>
+			<uri>model://arrow</uri>
+			<name>arrow_9</name>
+			<pose>-3 20.5 0.1 0 0 3.14</pose>
+		</include>
+		<include>
+			<uri>model://arrow</uri>
+			<name>arrow_10</name>
+			<pose>-13 20.5 0.1 0 0 3.14</pose>
+		</include>
+
+		<include>
+			<uri>model://simple_labyrinth_green</uri>
+		</include>
+
+		<!-- Grass plane -->
+		<include>
+			<uri>model://grass_plane</uri>
+		</include>
+
+		<!-- JdeRobot Logo -->
+		<include>
+			<name>logo</name>
+			<uri>model://logoJdeRobot</uri>
+			<pose>-26.0 0.0 0.0 0.0 0.0 0.0</pose>
+		</include>
+
+		<include>
+			<uri>model://quadrotor_dual_cam</uri>
+			<name>drone0</name>
+			<pose>-18 -8.5 0.3 0 0 0</pose>
+		</include>
+
+	</world>
+</sdf>

Launchers/labyrinth_escape.launch.py

@@ -0,0 +1,81 @@
+"""Entry point for the Labyrinth Escape exercise."""
+
+import os
+
+from ament_index_python.packages import get_package_share_directory
+
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription
+from launch.conditions import IfCondition
+from launch.launch_description_sources import PythonLaunchDescriptionSource
+from launch.substitutions import LaunchConfiguration
+from launch_ros.actions import Node
+
+
+def generate_launch_description():
+    """Generate the launch description for labyrinth escape simulation."""
+    custom_robots_share = get_package_share_directory("custom_robots")
+    # same bridges file as rescue_people exercise
+    bridges_path = os.path.join(custom_robots_share, "bridges", "rescue_people.yaml")
+    world_path = os.path.join(
+        custom_robots_share, "worlds", "labyrinth_escape_harmonic.world"
+    )
+
+    # YOU DO NOT NEED TO CHANGE ANYTHING BELOW THIS LINE #
+    declare_use_simulator_cmd = DeclareLaunchArgument(
+        name="use_simulator",
+        default_value="True",
+        description="Whether to start the simulator",
+    )
+
+    # Start Gazebo server
+    gzsim = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(
+            [
+                os.path.join(get_package_share_directory("jderobot_drones"), "launch"),
+                "/gz_sim.launch.py",
+            ]
+        ),
+        condition=IfCondition(LaunchConfiguration("use_simulator")),
+        launch_arguments={
+            "namespace": "drone0",
+            "bridges_file": bridges_path,
+            "world_file": world_path,
+        }.items(),
+    )
+
+    as2 = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(
+            [
+                os.path.join(get_package_share_directory("jderobot_drones"), "launch"),
+                "/as2_default_gazebo_sim.launch.py",
+            ]
+        ),
+        launch_arguments={
+            "namespace": "drone0",
+        }.items(),
+    )
+
+    start_gazebo_frontal_image_bridge_cmd = Node(
+        package="ros_gz_image",
+        executable="image_bridge",
+        arguments=["/drone0/frontal_cam/image_raw"],
+        output="screen",
+    )
+
+    start_gazebo_ventral_image_bridge_cmd = Node(
+        package="ros_gz_image",
+        executable="image_bridge",
+        arguments=["/drone0/ventral_cam/image_raw"],
+        output="screen",
+    )
+
+    # Create the launch description and populate
+    ld = LaunchDescription()
+    ld.add_action(declare_use_simulator_cmd)
+    ld.add_action(gzsim)
+    ld.add_action(as2)
+    ld.add_action(start_gazebo_frontal_image_bridge_cmd)
+    ld.add_action(start_gazebo_ventral_image_bridge_cmd)
+
+    return ld