From fdc0e7ddab52403c39ac846e6b508f38f52b9018 Mon Sep 17 00:00:00 2001 From: Javier Izquierdo Hernandez Date: Thu, 26 Jun 2025 10:58:52 +0200 Subject: [PATCH 1/5] Fix db --- Launchers/monaco_circuit.launch .py | 71 ------------------------ Launchers/monaco_circuit.launch.py | 84 +++++++++++++++++++++++++++++ Launchers/spa_circuit.launch.py | 75 +++++++++++++++----------- Worlds/monaco_circuit.world | 68 +++++++++++------------ 4 files changed, 160 insertions(+), 138 deletions(-) delete mode 100644 Launchers/monaco_circuit.launch .py create mode 100644 Launchers/monaco_circuit.launch.py diff --git a/Launchers/monaco_circuit.launch .py b/Launchers/monaco_circuit.launch .py deleted file mode 100644 index 0abae62dc..000000000 --- a/Launchers/monaco_circuit.launch .py +++ /dev/null @@ -1,71 +0,0 @@ -import os -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.substitutions import FindPackageShare - -def generate_launch_description(): - - # Aquí el paquete correcto para gazebo_ros - pkg_gazebo_ros = FindPackageShare(package='gazebo_launch').find('gazebo_launch') - - # Ruta a tu carpeta de modelos en tu workspace (ajusta si es necesario) - # Si tus modelos están en ~/gzROS/src/gazebo_launch/worlds/models o similar, pon esa ruta: - pkg_share = os.path.expanduser('~/gzROS/src/gazebo_launch/models') - - # Nombre del archivo del mundo - world_file_name = 'monaco_circuit.world' - world_path = os.path.join(pkg_share, world_file_name) - - # Añadir el path de modelos a la variable de entorno GAZEBO_MODEL_PATH - os.environ["GAZEBO_MODEL_PATH"] = f"{os.environ.get('GAZEBO_MODEL_PATH', '')}:{pkg_share}" - - # Configuraciones de lanzamiento - headless = LaunchConfiguration('headless') - use_sim_time = LaunchConfiguration('use_sim_time') - use_simulator = LaunchConfiguration('use_simulator') - world = LaunchConfiguration('world') - - declare_simulator_cmd = DeclareLaunchArgument( - name='headless', - default_value='False', - description='Whether to execute gzclient' - ) - - declare_use_sim_time_cmd = DeclareLaunchArgument( - name='use_sim_time', - default_value='true', - description='Use simulation (Gazebo) clock if true' - ) - - declare_use_simulator_cmd = DeclareLaunchArgument( - name='use_simulator', - default_value='True', - description='Whether to start the simulator' - ) - - declare_world_cmd = DeclareLaunchArgument( - name='world', - default_value=world_path, - description='Full path to the world model file to load' - ) - - # Lanzar el servidor de Gazebo - start_gazebo_server_cmd = IncludeLaunchDescription( - PythonLaunchDescriptionSource(os.path.join(pkg_gazebo_ros, 'launch', 'gazebo.launch.py')), - condition=IfCondition(use_simulator), - launch_arguments={'world': world}.items() - ) - - ld = LaunchDescription() - - ld.add_action(declare_simulator_cmd) - ld.add_action(declare_use_sim_time_cmd) - ld.add_action(declare_use_simulator_cmd) - ld.add_action(declare_world_cmd) - - ld.add_action(start_gazebo_server_cmd) - - return ld diff --git a/Launchers/monaco_circuit.launch.py b/Launchers/monaco_circuit.launch.py new file mode 100644 index 000000000..355a1645f --- /dev/null +++ b/Launchers/monaco_circuit.launch.py @@ -0,0 +1,84 @@ +import os +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription +from launch.conditions import IfCondition, UnlessCondition +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.substitutions import Command, LaunchConfiguration, PythonExpression +from launch_ros.actions import Node +from launch_ros.substitutions import FindPackageShare + + +def generate_launch_description(): + + # Set the path to the Gazebo ROS package + pkg_gazebo_ros = FindPackageShare(package="gazebo_ros").find("gazebo_ros") + + # Set the path to this package. + pkg_share = FindPackageShare(package="custom_robots").find("custom_robots") + + # Set the path to the world file + world_file_name = "monaco_circuit.world" + worlds_dir = "/opt/jderobot/Worlds" + world_path = os.path.join(worlds_dir, world_file_name) + + # Set the path to the SDF model files. + gazebo_models_path = os.path.join(pkg_share, "models") + os.environ["GAZEBO_MODEL_PATH"] = ( + f"{os.environ.get('GAZEBO_MODEL_PATH', '')}:{':'.join(gazebo_models_path)}" + ) + + ########### YOU DO NOT NEED TO CHANGE ANYTHING BELOW THIS LINE ############## + # Launch configuration variables specific to simulation + headless = LaunchConfiguration("headless") + use_sim_time = LaunchConfiguration("use_sim_time") + use_simulator = LaunchConfiguration("use_simulator") + world = LaunchConfiguration("world") + + declare_simulator_cmd = DeclareLaunchArgument( + name="headless", + default_value="False", + description="Whether to execute gzclient", + ) + + declare_use_sim_time_cmd = DeclareLaunchArgument( + name="use_sim_time", + default_value="true", + description="Use simulation (Gazebo) clock if true", + ) + + declare_use_simulator_cmd = DeclareLaunchArgument( + name="use_simulator", + default_value="True", + description="Whether to start the simulator", + ) + + declare_world_cmd = DeclareLaunchArgument( + name="world", + default_value=world_path, + description="Full path to the world model file to load", + ) + + # Specify the actions + + # Start Gazebo server + start_gazebo_server_cmd = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(pkg_gazebo_ros, "launch", "gzserver.launch.py") + ), + condition=IfCondition(use_simulator), + launch_arguments={"world": world}.items(), + ) + + # Create the launch description and populate + ld = LaunchDescription() + + # Declare the launch options + ld.add_action(declare_simulator_cmd) + ld.add_action(declare_use_sim_time_cmd) + ld.add_action(declare_use_simulator_cmd) + ld.add_action(declare_world_cmd) + + # Add any actions + ld.add_action(start_gazebo_server_cmd) + + return ld diff --git a/Launchers/spa_circuit.launch.py b/Launchers/spa_circuit.launch.py index 7fa192496..02ced9680 100644 --- a/Launchers/spa_circuit.launch.py +++ b/Launchers/spa_circuit.launch.py @@ -1,71 +1,84 @@ import os from launch import LaunchDescription from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription -from launch.conditions import IfCondition +from launch.conditions import IfCondition, UnlessCondition from launch.launch_description_sources import PythonLaunchDescriptionSource -from launch.substitutions import LaunchConfiguration +from launch.substitutions import Command, LaunchConfiguration, PythonExpression +from launch_ros.actions import Node from launch_ros.substitutions import FindPackageShare + def generate_launch_description(): - # Aquí el paquete correcto para gazebo_ros - pkg_gazebo_ros = FindPackageShare(package='gazebo_launch').find('gazebo_launch') + # Set the path to the Gazebo ROS package + pkg_gazebo_ros = FindPackageShare(package="gazebo_ros").find("gazebo_ros") - # Ruta a tu carpeta de modelos en tu workspace (ajusta si es necesario) - # Si tus modelos están en ~/gzROS/src/gazebo_launch/worlds/models o similar, pon esa ruta: - pkg_share = os.path.expanduser('~/gzROS/src/gazebo_launch/models') + # Set the path to this package. + pkg_share = FindPackageShare(package="custom_robots").find("custom_robots") - # Nombre del archivo del mundo - world_file_name = 'spa_circuit.world' - world_path = os.path.join(pkg_share, world_file_name) + # Set the path to the world file + world_file_name = "spa_circuit.world" + worlds_dir = "/opt/jderobot/Worlds" + world_path = os.path.join(worlds_dir, world_file_name) - # Añadir el path de modelos a la variable de entorno GAZEBO_MODEL_PATH - os.environ["GAZEBO_MODEL_PATH"] = f"{os.environ.get('GAZEBO_MODEL_PATH', '')}:{pkg_share}" + # Set the path to the SDF model files. + gazebo_models_path = os.path.join(pkg_share, "models") + os.environ["GAZEBO_MODEL_PATH"] = ( + f"{os.environ.get('GAZEBO_MODEL_PATH', '')}:{':'.join(gazebo_models_path)}" + ) - # Configuraciones de lanzamiento - headless = LaunchConfiguration('headless') - use_sim_time = LaunchConfiguration('use_sim_time') - use_simulator = LaunchConfiguration('use_simulator') - world = LaunchConfiguration('world') + ########### YOU DO NOT NEED TO CHANGE ANYTHING BELOW THIS LINE ############## + # Launch configuration variables specific to simulation + headless = LaunchConfiguration("headless") + use_sim_time = LaunchConfiguration("use_sim_time") + use_simulator = LaunchConfiguration("use_simulator") + world = LaunchConfiguration("world") declare_simulator_cmd = DeclareLaunchArgument( - name='headless', - default_value='False', - description='Whether to execute gzclient' + name="headless", + default_value="False", + description="Whether to execute gzclient", ) declare_use_sim_time_cmd = DeclareLaunchArgument( - name='use_sim_time', - default_value='true', - description='Use simulation (Gazebo) clock if true' + name="use_sim_time", + default_value="true", + description="Use simulation (Gazebo) clock if true", ) declare_use_simulator_cmd = DeclareLaunchArgument( - name='use_simulator', - default_value='True', - description='Whether to start the simulator' + name="use_simulator", + default_value="True", + description="Whether to start the simulator", ) declare_world_cmd = DeclareLaunchArgument( - name='world', + name="world", default_value=world_path, - description='Full path to the world model file to load' + description="Full path to the world model file to load", ) - # Lanzar el servidor de Gazebo + # Specify the actions + + # Start Gazebo server start_gazebo_server_cmd = IncludeLaunchDescription( - PythonLaunchDescriptionSource(os.path.join(pkg_gazebo_ros, 'launch', 'gazebo.launch.py')), + PythonLaunchDescriptionSource( + os.path.join(pkg_gazebo_ros, "launch", "gzserver.launch.py") + ), condition=IfCondition(use_simulator), - launch_arguments={'world': world}.items() + launch_arguments={"world": world}.items(), ) + # Create the launch description and populate ld = LaunchDescription() + # Declare the launch options ld.add_action(declare_simulator_cmd) ld.add_action(declare_use_sim_time_cmd) ld.add_action(declare_use_simulator_cmd) ld.add_action(declare_world_cmd) + # Add any actions ld.add_action(start_gazebo_server_cmd) return ld diff --git a/Worlds/monaco_circuit.world b/Worlds/monaco_circuit.world index 25a77d32e..dcdc31f30 100644 --- a/Worlds/monaco_circuit.world +++ b/Worlds/monaco_circuit.world @@ -1,20 +1,25 @@ - - - - 0.001 - 1 - 1000 - - - - - - 0 0 -9.8 - + + + + + + /demo + model_states:=model_states_demo + + 1.0 + + + + + f1_renault_camera + true + true + 0 -5 1.4 + true + + + - 0.4 0.4 0.4 1 - 0.7 0.7 0.7 1 - true false @@ -22,29 +27,20 @@ + + + model://sun + sun_1 + 0 0 1 0 0 0 + model://monaco - monaco 0 0 0 0 0 0 - - 0 0 10 0 0 0 - true - 1 - -0.5 0.1 -0.9 - 0.8 0.8 0.8 1 - 0.2 0.2 0.2 1 - - 1000 - 0.01 - 0.9 - 0.001 - - - 0 - 0 - 0 - - + + model://f1_renault_camera + 5 5 10 0 0 1.56 + + From 6bcd262ce69cde77ba0a198fff8913ea147b515e Mon Sep 17 00:00:00 2001 From: Javier Izquierdo Hernandez Date: Thu, 26 Jun 2025 10:59:00 +0200 Subject: [PATCH 2/5] Fix db --- database/universes.sql | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/database/universes.sql b/database/universes.sql index d6fd0f8d9..001f2e757 100644 --- a/database/universes.sql +++ b/database/universes.sql @@ -148,8 +148,8 @@ COPY public.universes (id, name, world_id, robot_id) FROM stdin; 32 Follow Road Harmonic 32 0 33 Laser Mapping Warehouse 12 0 34 Pick And Place World 34 0 -35 Spa Circuit 33 0 -36 Monaco Circuit 34 0 +35 Spa Circuit 35 0 +36 Monaco Circuit 36 0 \. From 4bb9aa4b33ee1d225289b7fc941e13489cfcb335 Mon Sep 17 00:00:00 2001 From: Javier Izquierdo Hernandez Date: Thu, 26 Jun 2025 11:06:03 +0200 Subject: [PATCH 3/5] Change launchers and world --- Launchers/monaco_circuit.launch.py | 131 ++++++++++-------- .../robot_state_publisher.launch.py | 58 ++++++++ .../monaco_circuit/spawn_robot.launch.py | 107 ++++++++++++++ Launchers/spa_circuit.launch.py | 131 ++++++++++-------- .../robot_state_publisher.launch.py | 58 ++++++++ Launchers/spa_circuit/spawn_robot.launch.py | 107 ++++++++++++++ Worlds/monaco_circuit.world | 77 ++++++---- 7 files changed, 531 insertions(+), 138 deletions(-) create mode 100644 Launchers/monaco_circuit/robot_state_publisher.launch.py create mode 100644 Launchers/monaco_circuit/spawn_robot.launch.py create mode 100644 Launchers/spa_circuit/robot_state_publisher.launch.py create mode 100644 Launchers/spa_circuit/spawn_robot.launch.py diff --git a/Launchers/monaco_circuit.launch.py b/Launchers/monaco_circuit.launch.py index 355a1645f..9be09dbf1 100644 --- a/Launchers/monaco_circuit.launch.py +++ b/Launchers/monaco_circuit.launch.py @@ -1,84 +1,105 @@ 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, UnlessCondition +from launch.actions import ( + DeclareLaunchArgument, + IncludeLaunchDescription, + SetEnvironmentVariable, + AppendEnvironmentVariable, +) from launch.launch_description_sources import PythonLaunchDescriptionSource -from launch.substitutions import Command, LaunchConfiguration, PythonExpression +from launch.substitutions import LaunchConfiguration, Command +from launch_ros.actions import Node +from launch.substitutions import LaunchConfiguration from launch_ros.actions import Node -from launch_ros.substitutions import FindPackageShare def generate_launch_description(): - # Set the path to the Gazebo ROS package - pkg_gazebo_ros = FindPackageShare(package="gazebo_ros").find("gazebo_ros") + x = LaunchConfiguration("x") + y = LaunchConfiguration("y") + z = LaunchConfiguration("z") + roll = LaunchConfiguration("R") + pitch = LaunchConfiguration("P") + yaw = LaunchConfiguration("Y") + + package_dir = get_package_share_directory("custom_robots") + ros_gz_sim = get_package_share_directory("ros_gz_sim") + + gazebo_models_path = os.path.join(package_dir, "models") - # Set the path to this package. - pkg_share = FindPackageShare(package="custom_robots").find("custom_robots") + robot_launch_dir = "/opt/jderobot/Launchers/monaco_circuit" - # Set the path to the world file + use_sim_time = LaunchConfiguration("use_sim_time", default="true") + x_pose = LaunchConfiguration("x_pose", default="1.0") + y_pose = LaunchConfiguration("y_pose", default="-1.5") + z_pose = LaunchConfiguration("z_pose", default="7.1") world_file_name = "monaco_circuit.world" worlds_dir = "/opt/jderobot/Worlds" world_path = os.path.join(worlds_dir, world_file_name) - # Set the path to the SDF model files. - gazebo_models_path = os.path.join(pkg_share, "models") - os.environ["GAZEBO_MODEL_PATH"] = ( - f"{os.environ.get('GAZEBO_MODEL_PATH', '')}:{':'.join(gazebo_models_path)}" + gazebo_server = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(ros_gz_sim, "launch", "gz_sim.launch.py") + ), + launch_arguments={ + "gz_args": ["-r -s -v4 ", world_path], + "on_exit_shutdown": "true", + }.items(), ) - ########### YOU DO NOT NEED TO CHANGE ANYTHING BELOW THIS LINE ############## - # Launch configuration variables specific to simulation - headless = LaunchConfiguration("headless") - use_sim_time = LaunchConfiguration("use_sim_time") - use_simulator = LaunchConfiguration("use_simulator") - world = LaunchConfiguration("world") - - declare_simulator_cmd = DeclareLaunchArgument( - name="headless", - default_value="False", - description="Whether to execute gzclient", - ) + declare_x_cmd = DeclareLaunchArgument("x", default_value="1.0") - declare_use_sim_time_cmd = DeclareLaunchArgument( - name="use_sim_time", - default_value="true", - description="Use simulation (Gazebo) clock if true", - ) + declare_y_cmd = DeclareLaunchArgument("y", default_value="-1.5") - declare_use_simulator_cmd = DeclareLaunchArgument( - name="use_simulator", - default_value="True", - description="Whether to start the simulator", - ) + declare_z_cmd = DeclareLaunchArgument("z", default_value="7.1") - declare_world_cmd = DeclareLaunchArgument( - name="world", - default_value=world_path, - description="Full path to the world model file to load", - ) + declare_roll_cmd = DeclareLaunchArgument("R", default_value="0.0") + + declare_pitch_cmd = DeclareLaunchArgument("P", default_value="0.0") - # Specify the actions + declare_yaw_cmd = DeclareLaunchArgument("Y", default_value="1.57079") - # Start Gazebo server - start_gazebo_server_cmd = IncludeLaunchDescription( + robot_state_publisher_cmd = IncludeLaunchDescription( PythonLaunchDescriptionSource( - os.path.join(pkg_gazebo_ros, "launch", "gzserver.launch.py") + os.path.join(robot_launch_dir, "robot_state_publisher.launch.py") ), - condition=IfCondition(use_simulator), - launch_arguments={"world": world}.items(), + launch_arguments={"use_sim_time": use_sim_time}.items(), ) - # Create the launch description and populate - ld = LaunchDescription() + spawn_robot_cmd = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(robot_launch_dir, "spawn_robot.launch.py") + ), + launch_arguments={"x_pose": x_pose, "y_pose": y_pose, "z_pose": z_pose}.items(), + ) - # Declare the launch options - ld.add_action(declare_simulator_cmd) - ld.add_action(declare_use_sim_time_cmd) - ld.add_action(declare_use_simulator_cmd) - ld.add_action(declare_world_cmd) + world_entity_cmd = Node( + package="ros_gz_sim", + executable="create", + arguments=["-name", "world", "-file", world_path], + output="screen", + ) - # Add any actions - ld.add_action(start_gazebo_server_cmd) + ld = LaunchDescription() + + ld.add_action(SetEnvironmentVariable("GZ_SIM_RESOURCE_PATH", gazebo_models_path)) + set_env_vars_resources = AppendEnvironmentVariable( + "GZ_SIM_RESOURCE_PATH", os.path.join(package_dir, "models") + ) + ld.add_action(set_env_vars_resources) + ld.add_action(gazebo_server) + # ld.add_action(gazebo_client) + ld.add_action(declare_x_cmd) + ld.add_action(declare_y_cmd) + ld.add_action(declare_z_cmd) + ld.add_action(declare_roll_cmd) + ld.add_action(declare_pitch_cmd) + ld.add_action(declare_yaw_cmd) + ld.add_action(world_entity_cmd) + ld.add_action(robot_state_publisher_cmd) + ld.add_action(spawn_robot_cmd) return ld diff --git a/Launchers/monaco_circuit/robot_state_publisher.launch.py b/Launchers/monaco_circuit/robot_state_publisher.launch.py new file mode 100644 index 000000000..731207c12 --- /dev/null +++ b/Launchers/monaco_circuit/robot_state_publisher.launch.py @@ -0,0 +1,58 @@ +#!/usr/bin/env python3 +# +# Copyright 2019 ROBOTIS CO., LTD. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# +# Authors: Darby Lim + +import os + +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument +from launch.substitutions import LaunchConfiguration +from launch_ros.actions import Node + + +def generate_launch_description(): + use_sim_time = LaunchConfiguration("use_sim_time", default="true") + urdf_file_name = "turtlebot3_waffle.urdf" + + print("urdf_file_name : {}".format(urdf_file_name)) + + urdf_path = os.path.join( + get_package_share_directory("custom_robots"), "urdf", urdf_file_name + ) + + with open(urdf_path, "r") as infp: + robot_desc = infp.read() + + return LaunchDescription( + [ + DeclareLaunchArgument( + "use_sim_time", + default_value="true", + description="Use simulation (Gazebo) clock if true", + ), + Node( + package="robot_state_publisher", + executable="robot_state_publisher", + name="robot_state_publisher", + output="screen", + parameters=[ + {"use_sim_time": use_sim_time, "robot_description": robot_desc} + ], + ), + ] + ) diff --git a/Launchers/monaco_circuit/spawn_robot.launch.py b/Launchers/monaco_circuit/spawn_robot.launch.py new file mode 100644 index 000000000..5cf708d02 --- /dev/null +++ b/Launchers/monaco_circuit/spawn_robot.launch.py @@ -0,0 +1,107 @@ +# Copyright 2019 Open Source Robotics Foundation, Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +import os + +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument +from launch.substitutions import LaunchConfiguration +from launch_ros.actions import Node + + +def generate_launch_description(): + # Get the urdf file + model_folder = "turtlebot3_waffle" + urdf_path = os.path.join( + get_package_share_directory("custom_robots"), + "models", + model_folder, + "model.sdf", + ) + + # Launch configuration variables specific to simulation + # x_pose = LaunchConfiguration('x_pose', default='1.0') + # y_pose = LaunchConfiguration('y_pose', default='-1.5') + # z_pose = LaunchConfiguration('z_pose', default='7.1') + + # Declare the launch arguments + # declare_x_position_cmd = DeclareLaunchArgument( + # 'x_pose', default_value='1.0', + # description='Specify namespace of the robot') + + # declare_y_position_cmd = DeclareLaunchArgument( + # 'y_pose', default_value='-1.5', + # description='Specify namespace of the robot') + + # declare_z_position_cmd = DeclareLaunchArgument( + # 'z_pose', default_value='7.1', + # description='Specify namespace of the robot') + + # start_gazebo_ros_spawner_cmd = Node( + # package='ros_gz_sim', + # executable='create', + # arguments=[ + # '-name', 'waffle', + # '-file', urdf_path, + # '-x', x_pose, + # '-y', y_pose, + # '-z', z_pose + # ], + # output='screen', + # ) + + bridge_params = os.path.join( + get_package_share_directory("custom_robots"), "params", "robot_params.yaml" + ) + + start_gazebo_ros_bridge_cmd = Node( + package="ros_gz_bridge", + executable="parameter_bridge", + arguments=[ + "--ros-args", + "-p", + f"config_file:={bridge_params}", + ], + output="screen", + ) + + start_gazebo_ros_image_bridge_cmd = Node( + package="ros_gz_image", + executable="image_bridge", + arguments=["/turtlebot3/camera/image_raw"], + output="screen", + ) + + start_gazebo_ros_depth_bridge_cmd = Node( + package="ros_gz_image", + executable="image_bridge", + arguments=["/turtlebot3/camera/depth"], + output="screen", + ) + + ld = LaunchDescription() + + # Declare the launch options + # ld.add_action(declare_x_position_cmd) + # ld.add_action(declare_y_position_cmd) + # ld.add_action(declare_z_position_cmd) + + # Add any conditioned actions + # ld.add_action(start_gazebo_ros_spawner_cmd) + ld.add_action(start_gazebo_ros_bridge_cmd) + ld.add_action(start_gazebo_ros_image_bridge_cmd) + ld.add_action(start_gazebo_ros_depth_bridge_cmd) + + return ld diff --git a/Launchers/spa_circuit.launch.py b/Launchers/spa_circuit.launch.py index 02ced9680..0395150b6 100644 --- a/Launchers/spa_circuit.launch.py +++ b/Launchers/spa_circuit.launch.py @@ -1,84 +1,105 @@ 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, UnlessCondition +from launch.actions import ( + DeclareLaunchArgument, + IncludeLaunchDescription, + SetEnvironmentVariable, + AppendEnvironmentVariable, +) from launch.launch_description_sources import PythonLaunchDescriptionSource -from launch.substitutions import Command, LaunchConfiguration, PythonExpression +from launch.substitutions import LaunchConfiguration, Command +from launch_ros.actions import Node +from launch.substitutions import LaunchConfiguration from launch_ros.actions import Node -from launch_ros.substitutions import FindPackageShare def generate_launch_description(): - # Set the path to the Gazebo ROS package - pkg_gazebo_ros = FindPackageShare(package="gazebo_ros").find("gazebo_ros") + x = LaunchConfiguration("x") + y = LaunchConfiguration("y") + z = LaunchConfiguration("z") + roll = LaunchConfiguration("R") + pitch = LaunchConfiguration("P") + yaw = LaunchConfiguration("Y") + + package_dir = get_package_share_directory("custom_robots") + ros_gz_sim = get_package_share_directory("ros_gz_sim") + + gazebo_models_path = os.path.join(package_dir, "models") - # Set the path to this package. - pkg_share = FindPackageShare(package="custom_robots").find("custom_robots") + robot_launch_dir = "/opt/jderobot/Launchers/spa_circuit" - # Set the path to the world file + use_sim_time = LaunchConfiguration("use_sim_time", default="true") + x_pose = LaunchConfiguration("x_pose", default="1.0") + y_pose = LaunchConfiguration("y_pose", default="-1.5") + z_pose = LaunchConfiguration("z_pose", default="7.1") world_file_name = "spa_circuit.world" worlds_dir = "/opt/jderobot/Worlds" world_path = os.path.join(worlds_dir, world_file_name) - # Set the path to the SDF model files. - gazebo_models_path = os.path.join(pkg_share, "models") - os.environ["GAZEBO_MODEL_PATH"] = ( - f"{os.environ.get('GAZEBO_MODEL_PATH', '')}:{':'.join(gazebo_models_path)}" + gazebo_server = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(ros_gz_sim, "launch", "gz_sim.launch.py") + ), + launch_arguments={ + "gz_args": ["-r -s -v4 ", world_path], + "on_exit_shutdown": "true", + }.items(), ) - ########### YOU DO NOT NEED TO CHANGE ANYTHING BELOW THIS LINE ############## - # Launch configuration variables specific to simulation - headless = LaunchConfiguration("headless") - use_sim_time = LaunchConfiguration("use_sim_time") - use_simulator = LaunchConfiguration("use_simulator") - world = LaunchConfiguration("world") - - declare_simulator_cmd = DeclareLaunchArgument( - name="headless", - default_value="False", - description="Whether to execute gzclient", - ) + declare_x_cmd = DeclareLaunchArgument("x", default_value="1.0") - declare_use_sim_time_cmd = DeclareLaunchArgument( - name="use_sim_time", - default_value="true", - description="Use simulation (Gazebo) clock if true", - ) + declare_y_cmd = DeclareLaunchArgument("y", default_value="-1.5") - declare_use_simulator_cmd = DeclareLaunchArgument( - name="use_simulator", - default_value="True", - description="Whether to start the simulator", - ) + declare_z_cmd = DeclareLaunchArgument("z", default_value="7.1") - declare_world_cmd = DeclareLaunchArgument( - name="world", - default_value=world_path, - description="Full path to the world model file to load", - ) + declare_roll_cmd = DeclareLaunchArgument("R", default_value="0.0") + + declare_pitch_cmd = DeclareLaunchArgument("P", default_value="0.0") - # Specify the actions + declare_yaw_cmd = DeclareLaunchArgument("Y", default_value="1.57079") - # Start Gazebo server - start_gazebo_server_cmd = IncludeLaunchDescription( + robot_state_publisher_cmd = IncludeLaunchDescription( PythonLaunchDescriptionSource( - os.path.join(pkg_gazebo_ros, "launch", "gzserver.launch.py") + os.path.join(robot_launch_dir, "robot_state_publisher.launch.py") ), - condition=IfCondition(use_simulator), - launch_arguments={"world": world}.items(), + launch_arguments={"use_sim_time": use_sim_time}.items(), ) - # Create the launch description and populate - ld = LaunchDescription() + spawn_robot_cmd = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(robot_launch_dir, "spawn_robot.launch.py") + ), + launch_arguments={"x_pose": x_pose, "y_pose": y_pose, "z_pose": z_pose}.items(), + ) - # Declare the launch options - ld.add_action(declare_simulator_cmd) - ld.add_action(declare_use_sim_time_cmd) - ld.add_action(declare_use_simulator_cmd) - ld.add_action(declare_world_cmd) + world_entity_cmd = Node( + package="ros_gz_sim", + executable="create", + arguments=["-name", "world", "-file", world_path], + output="screen", + ) - # Add any actions - ld.add_action(start_gazebo_server_cmd) + ld = LaunchDescription() + + ld.add_action(SetEnvironmentVariable("GZ_SIM_RESOURCE_PATH", gazebo_models_path)) + set_env_vars_resources = AppendEnvironmentVariable( + "GZ_SIM_RESOURCE_PATH", os.path.join(package_dir, "models") + ) + ld.add_action(set_env_vars_resources) + ld.add_action(gazebo_server) + # ld.add_action(gazebo_client) + ld.add_action(declare_x_cmd) + ld.add_action(declare_y_cmd) + ld.add_action(declare_z_cmd) + ld.add_action(declare_roll_cmd) + ld.add_action(declare_pitch_cmd) + ld.add_action(declare_yaw_cmd) + ld.add_action(world_entity_cmd) + ld.add_action(robot_state_publisher_cmd) + ld.add_action(spawn_robot_cmd) return ld diff --git a/Launchers/spa_circuit/robot_state_publisher.launch.py b/Launchers/spa_circuit/robot_state_publisher.launch.py new file mode 100644 index 000000000..731207c12 --- /dev/null +++ b/Launchers/spa_circuit/robot_state_publisher.launch.py @@ -0,0 +1,58 @@ +#!/usr/bin/env python3 +# +# Copyright 2019 ROBOTIS CO., LTD. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# +# Authors: Darby Lim + +import os + +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument +from launch.substitutions import LaunchConfiguration +from launch_ros.actions import Node + + +def generate_launch_description(): + use_sim_time = LaunchConfiguration("use_sim_time", default="true") + urdf_file_name = "turtlebot3_waffle.urdf" + + print("urdf_file_name : {}".format(urdf_file_name)) + + urdf_path = os.path.join( + get_package_share_directory("custom_robots"), "urdf", urdf_file_name + ) + + with open(urdf_path, "r") as infp: + robot_desc = infp.read() + + return LaunchDescription( + [ + DeclareLaunchArgument( + "use_sim_time", + default_value="true", + description="Use simulation (Gazebo) clock if true", + ), + Node( + package="robot_state_publisher", + executable="robot_state_publisher", + name="robot_state_publisher", + output="screen", + parameters=[ + {"use_sim_time": use_sim_time, "robot_description": robot_desc} + ], + ), + ] + ) diff --git a/Launchers/spa_circuit/spawn_robot.launch.py b/Launchers/spa_circuit/spawn_robot.launch.py new file mode 100644 index 000000000..5cf708d02 --- /dev/null +++ b/Launchers/spa_circuit/spawn_robot.launch.py @@ -0,0 +1,107 @@ +# Copyright 2019 Open Source Robotics Foundation, Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +import os + +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument +from launch.substitutions import LaunchConfiguration +from launch_ros.actions import Node + + +def generate_launch_description(): + # Get the urdf file + model_folder = "turtlebot3_waffle" + urdf_path = os.path.join( + get_package_share_directory("custom_robots"), + "models", + model_folder, + "model.sdf", + ) + + # Launch configuration variables specific to simulation + # x_pose = LaunchConfiguration('x_pose', default='1.0') + # y_pose = LaunchConfiguration('y_pose', default='-1.5') + # z_pose = LaunchConfiguration('z_pose', default='7.1') + + # Declare the launch arguments + # declare_x_position_cmd = DeclareLaunchArgument( + # 'x_pose', default_value='1.0', + # description='Specify namespace of the robot') + + # declare_y_position_cmd = DeclareLaunchArgument( + # 'y_pose', default_value='-1.5', + # description='Specify namespace of the robot') + + # declare_z_position_cmd = DeclareLaunchArgument( + # 'z_pose', default_value='7.1', + # description='Specify namespace of the robot') + + # start_gazebo_ros_spawner_cmd = Node( + # package='ros_gz_sim', + # executable='create', + # arguments=[ + # '-name', 'waffle', + # '-file', urdf_path, + # '-x', x_pose, + # '-y', y_pose, + # '-z', z_pose + # ], + # output='screen', + # ) + + bridge_params = os.path.join( + get_package_share_directory("custom_robots"), "params", "robot_params.yaml" + ) + + start_gazebo_ros_bridge_cmd = Node( + package="ros_gz_bridge", + executable="parameter_bridge", + arguments=[ + "--ros-args", + "-p", + f"config_file:={bridge_params}", + ], + output="screen", + ) + + start_gazebo_ros_image_bridge_cmd = Node( + package="ros_gz_image", + executable="image_bridge", + arguments=["/turtlebot3/camera/image_raw"], + output="screen", + ) + + start_gazebo_ros_depth_bridge_cmd = Node( + package="ros_gz_image", + executable="image_bridge", + arguments=["/turtlebot3/camera/depth"], + output="screen", + ) + + ld = LaunchDescription() + + # Declare the launch options + # ld.add_action(declare_x_position_cmd) + # ld.add_action(declare_y_position_cmd) + # ld.add_action(declare_z_position_cmd) + + # Add any conditioned actions + # ld.add_action(start_gazebo_ros_spawner_cmd) + ld.add_action(start_gazebo_ros_bridge_cmd) + ld.add_action(start_gazebo_ros_image_bridge_cmd) + ld.add_action(start_gazebo_ros_depth_bridge_cmd) + + return ld diff --git a/Worlds/monaco_circuit.world b/Worlds/monaco_circuit.world index dcdc31f30..5e6b832f0 100644 --- a/Worlds/monaco_circuit.world +++ b/Worlds/monaco_circuit.world @@ -1,25 +1,30 @@ - + + - - - /demo - model_states:=model_states_demo - - 1.0 + + + + + - - - - f1_renault_camera - true - true - 0 -5 1.4 - true - - - + + ogre2 + + + + 0.4 0.4 0.4 1 + 0.7 0.7 0.7 1 + true false @@ -27,20 +32,36 @@ - - - model://sun - sun_1 - 0 0 1 0 0 0 - model://monaco + monaco 0 0 0 0 0 0 + + 0 0 10 0 0 0 + true + 1 + -0.5 0.1 -0.9 + 0.8 0.8 0.8 1 + 0.2 0.2 0.2 1 + + 1000 + 0.01 + 0.9 + 0.001 + + + 0 + 0 + 0 + + + + - model://f1_renault_camera - 5 5 10 0 0 1.56 + 1 -1.5 10 0 0 0 + turtlebot3_waffle + model://turtlebot3_waffle - - + \ No newline at end of file From 1dc8341103e64624495c30e8f0c27e502d16d811 Mon Sep 17 00:00:00 2001 From: Javier Izquierdo Hernandez Date: Thu, 26 Jun 2025 11:20:55 +0200 Subject: [PATCH 4/5] Bump spa --- Worlds/spa_circuit.world | 41 ++++++++++++++++++++++++++++------------ database/universes.sql | 4 ++-- 2 files changed, 31 insertions(+), 14 deletions(-) diff --git a/Worlds/spa_circuit.world b/Worlds/spa_circuit.world index 1c7a9283f..02317e1f9 100644 --- a/Worlds/spa_circuit.world +++ b/Worlds/spa_circuit.world @@ -1,16 +1,26 @@ - - - - 0.001 - 1 - 1000 - - - - + + + + + + + + + + + + ogre2 + - 0 0 -9.8 - + 0.4 0.4 0.4 1 0.7 0.7 0.7 1 @@ -46,5 +56,12 @@ 0 + + + + 1 -1.5 -9990 0 0 0 + turtlebot3_waffle + model://turtlebot3_waffle + \ No newline at end of file diff --git a/database/universes.sql b/database/universes.sql index 001f2e757..5cc452dd5 100644 --- a/database/universes.sql +++ b/database/universes.sql @@ -194,8 +194,8 @@ COPY public.worlds (id, name, launch_file_path, visualization_config_path, ros_v 32 Follow Road Harmonic /opt/jderobot/Launchers/follow_road.launch.py /opt/jderobot/Launchers/visualization/follow_road.config ROS2 gzsim_rae gazebo {0.0,0.0,0.0,0.0,0.0,0.0} 33 Small Laser Mapping Warehouse /opt/jderobot/Launchers/small_laser_mapping.launch.py /opt/jderobot/Launchers/visualization/small_laser_mapping.config ROS2 gzsim_rae gazebo {0.0,0.0,0.0,0.0,0.0,0.0} 34 Pick And Place Arm /home/dev_ws/src/IndustrialRobots/ros2_SimRealRobotControl/ros2srrc_launch/moveit2/moveit2.launch.py None ROS2 gazebo_rae gazebo {0.0,0.0,0.0,0.0,0.0,0.0} -35 Spa Circuit /opt/jderobot/Launchers/spa_circuit.launch.py None ROS2 gazebo_rae gazebo {0.0,0.0,0.0,0.0,0.0,0.0} -36 Monaco Circuit /opt/jderobot/Launchers/monaco_circuit.launch.py None ROS2 gazebo_rae gazebo {0.0,0.0,0.0,0.0,0.0,0.0} +35 Spa Circuit /opt/jderobot/Launchers/spa_circuit.launch.py None ROS2 gzsim_rae gazebo {0.0,0.0,0.0,0.0,0.0,0.0} +36 Monaco Circuit /opt/jderobot/Launchers/monaco_circuit.launch.py None ROS2 gzsim_rae gazebo {0.0,0.0,0.0,0.0,0.0,0.0} \. -- 34 Pick And Place Arm /opt/jderobot/Launchers/pick_place.launch.py None ROS2 gazebo_rae gazebo {0.0,0.0,0.0,0.0,0.0,0.0} From 446440a1aced7d5b485636130625ac3f34459fe0 Mon Sep 17 00:00:00 2001 From: Javier Izquierdo Hernandez Date: Mon, 30 Jun 2025 13:16:01 +0200 Subject: [PATCH 5/5] Linter --- Launchers/spa_circuit.launch.py | 1 - 1 file changed, 1 deletion(-) diff --git a/Launchers/spa_circuit.launch.py b/Launchers/spa_circuit.launch.py index f3aee9919..0395150b6 100644 --- a/Launchers/spa_circuit.launch.py +++ b/Launchers/spa_circuit.launch.py @@ -16,7 +16,6 @@ from launch_ros.actions import Node - def generate_launch_description(): x = LaunchConfiguration("x")