From 14d67bd38c55f2f5a052403315d623b37e6b77c1 Mon Sep 17 00:00:00 2001 From: Carlos Date: Fri, 8 May 2026 16:27:03 +0200 Subject: [PATCH] Add rover 4wd noisy variants for segment_mapper_pr exercise --- .../urdf/rover_4wd_noisy_high.urdf | 226 ++++++++++++++++++ .../urdf/rover_4wd_noisy_low.urdf | 226 ++++++++++++++++++ .../rover_4wd_warehouse_noise_high.launch.py | 87 +++++++ .../rover_4wd_warehouse_noise_low.launch.py | 87 +++++++ database/universes.sql | 4 + 5 files changed, 630 insertions(+) create mode 100644 CustomRobots/rover_4wd_description/urdf/rover_4wd_noisy_high.urdf create mode 100644 CustomRobots/rover_4wd_description/urdf/rover_4wd_noisy_low.urdf create mode 100644 Launchers/rover_4wd_warehouse_noise_high.launch.py create mode 100644 Launchers/rover_4wd_warehouse_noise_low.launch.py diff --git a/CustomRobots/rover_4wd_description/urdf/rover_4wd_noisy_high.urdf b/CustomRobots/rover_4wd_description/urdf/rover_4wd_noisy_high.urdf new file mode 100644 index 000000000..b461a8c5f --- /dev/null +++ b/CustomRobots/rover_4wd_description/urdf/rover_4wd_noisy_high.urdf @@ -0,0 +1,226 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + 2 + rl_wheel_to_chassis + fl_wheel_to_chassis + rr_wheel_to_chassis + fr_wheel_to_chassis + 0.24 + 0.122 + 0.5 + -0.5 + 0.8 + -0.8 + odom + base_link + /cmd_vel + /odometry/wheels + /tf_gazebo + + + + /joint_states + + + + odom + base_link + /odom + 3 + + + + /odom + /cmd_vel + odom + base_link + 0.005 + 0.005 + 0.003 + 0.003 + + + + + true + true + true + true + + + diff --git a/CustomRobots/rover_4wd_description/urdf/rover_4wd_noisy_low.urdf b/CustomRobots/rover_4wd_description/urdf/rover_4wd_noisy_low.urdf new file mode 100644 index 000000000..b4d97a1ae --- /dev/null +++ b/CustomRobots/rover_4wd_description/urdf/rover_4wd_noisy_low.urdf @@ -0,0 +1,226 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + 2 + rl_wheel_to_chassis + fl_wheel_to_chassis + rr_wheel_to_chassis + fr_wheel_to_chassis + 0.24 + 0.122 + 0.5 + -0.5 + 0.8 + -0.8 + odom + base_link + /cmd_vel + /odometry/wheels + /tf_gazebo + + + + /joint_states + + + + odom + base_link + /odom + 3 + + + + /odom + /cmd_vel + odom + base_link + 0.0005 + 0.0005 + 0.0002 + 0.0002 + + + + + true + true + true + true + + + diff --git a/Launchers/rover_4wd_warehouse_noise_high.launch.py b/Launchers/rover_4wd_warehouse_noise_high.launch.py new file mode 100644 index 000000000..b0b95fac5 --- /dev/null +++ b/Launchers/rover_4wd_warehouse_noise_high.launch.py @@ -0,0 +1,87 @@ +import os +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch.actions import ( + IncludeLaunchDescription, + SetEnvironmentVariable, + AppendEnvironmentVariable, +) +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.substitutions import LaunchConfiguration, Command +from launch_ros.actions import Node +from launch_ros.parameter_descriptions import ParameterValue + + +def generate_launch_description(): + ros_gz_sim_pkg = get_package_share_directory("ros_gz_sim") + custom_share = get_package_share_directory("custom_robots") + package_dir = custom_share + urdf_file = os.path.join(custom_share, "urdf", "rover_4wd_noisy_high.urdf") + bridge_yaml = os.path.join(custom_share, "config", "ros_gz_bridge.yaml") + gazebo_models_path = os.path.join(package_dir, "models") + world_file_name = "rover4wd_warehouse.world" + worlds_dir = "/opt/jderobot/Worlds" + world_path = os.path.join(worlds_dir, world_file_name) + use_sim_time = LaunchConfiguration("use_sim_time", default="true") + robot_description = ParameterValue( + Command(["xacro", " ", urdf_file]), + value_type=str, + ) + robot_state_publisher_node = 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_description} + ], + ) + gz_sim_launch = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(ros_gz_sim_pkg, "launch", "gz_sim.launch.py") + ), + launch_arguments={ + "gz_args": ["-r -s -v4 ", world_path], + "on_exit_shutdown": "true", + }.items(), + ) + world_entity_cmd = Node( + package="ros_gz_sim", + executable="create", + arguments=["-name", "world", "-file", world_path], + output="screen", + ) + gz_spawn_entity = Node( + package="ros_gz_sim", + executable="create", + arguments=[ + "-topic", + "/robot_description", + "-name", + "rover_4wd", + "-allow_renaming", + "true", + "-z", + "0.15", + ], + output="screen", + ) + gz_ros2_bridge = Node( + package="ros_gz_bridge", + executable="parameter_bridge", + arguments=["--ros-args", "-p", f"config_file:={bridge_yaml}"], + output="screen", + ) + 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(gz_sim_launch) + ld.add_action(world_entity_cmd) + ld.add_action(robot_state_publisher_node) + ld.add_action(gz_spawn_entity) + ld.add_action(gz_ros2_bridge) + return ld diff --git a/Launchers/rover_4wd_warehouse_noise_low.launch.py b/Launchers/rover_4wd_warehouse_noise_low.launch.py new file mode 100644 index 000000000..8cc0bd424 --- /dev/null +++ b/Launchers/rover_4wd_warehouse_noise_low.launch.py @@ -0,0 +1,87 @@ +import os +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch.actions import ( + IncludeLaunchDescription, + SetEnvironmentVariable, + AppendEnvironmentVariable, +) +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.substitutions import LaunchConfiguration, Command +from launch_ros.actions import Node +from launch_ros.parameter_descriptions import ParameterValue + + +def generate_launch_description(): + ros_gz_sim_pkg = get_package_share_directory("ros_gz_sim") + custom_share = get_package_share_directory("custom_robots") + package_dir = custom_share + urdf_file = os.path.join(custom_share, "urdf", "rover_4wd_noisy_low.urdf") + bridge_yaml = os.path.join(custom_share, "config", "ros_gz_bridge.yaml") + gazebo_models_path = os.path.join(package_dir, "models") + world_file_name = "rover4wd_warehouse.world" + worlds_dir = "/opt/jderobot/Worlds" + world_path = os.path.join(worlds_dir, world_file_name) + use_sim_time = LaunchConfiguration("use_sim_time", default="true") + robot_description = ParameterValue( + Command(["xacro", " ", urdf_file]), + value_type=str, + ) + robot_state_publisher_node = 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_description} + ], + ) + gz_sim_launch = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(ros_gz_sim_pkg, "launch", "gz_sim.launch.py") + ), + launch_arguments={ + "gz_args": ["-r -s -v4 ", world_path], + "on_exit_shutdown": "true", + }.items(), + ) + world_entity_cmd = Node( + package="ros_gz_sim", + executable="create", + arguments=["-name", "world", "-file", world_path], + output="screen", + ) + gz_spawn_entity = Node( + package="ros_gz_sim", + executable="create", + arguments=[ + "-topic", + "/robot_description", + "-name", + "rover_4wd", + "-allow_renaming", + "true", + "-z", + "0.15", + ], + output="screen", + ) + gz_ros2_bridge = Node( + package="ros_gz_bridge", + executable="parameter_bridge", + arguments=["--ros-args", "-p", f"config_file:={bridge_yaml}"], + output="screen", + ) + 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(gz_sim_launch) + ld.add_action(world_entity_cmd) + ld.add_action(robot_state_publisher_node) + ld.add_action(gz_spawn_entity) + ld.add_action(gz_ros2_bridge) + return ld diff --git a/database/universes.sql b/database/universes.sql index 6e0850b40..18f07beac 100644 --- a/database/universes.sql +++ b/database/universes.sql @@ -193,6 +193,8 @@ COPY public.universes (id, name, world_id, robot_id) FROM stdin; 65 Small Laser Mapping Warehouse High Noise 65 0 66 Follow Person Harmonic 66 0 67 Follow Person Teleop Harmonic 67 0 +68 Rover 4wd Warehouse Low Noise 68 0 +69 Rover 4wd Warehouse High Noise 69 0 \. -- @@ -269,6 +271,8 @@ COPY public.worlds (id, name, launch_file_path, tools_config, ros_version, type, 65 Small Laser Mapping Warehouse High Noise /opt/jderobot/Launchers/small_laser_mapping_noise_high.launch.py {"gzsim":"/opt/jderobot/Launchers/visualization/laser_mapping.config"} ROS2 gz {0.0,0.0,0.0,0.0,0.0,0.0} 66 Follow Person Harmonic /opt/jderobot/Launchers/follow_person_harmonic.launch.py {"gzsim":"/opt/jderobot/Launchers/visualization/follow_person.config"} ROS2 gz {0.0,0.0,0.0,0.0,0.0,0.0} 67 Follow Person Teleop Harmonic /opt/jderobot/Launchers/follow_person_teleop_harmonic.launch.py {"gzsim":"/opt/jderobot/Launchers/visualization/follow_person.config"} ROS2 gz {0.0,0.0,0.0,0.0,0.0,0.0} +68 Rover 4wd Warehouse Low Noise /opt/jderobot/Launchers/rover_4wd_warehouse_noise_low.launch.py None ROS2 gz {0.0,0.0,0.0,0.0,0.0,0.0} +69 Rover 4wd Warehouse High Noise /opt/jderobot/Launchers/rover_4wd_warehouse_noise_high.launch.py None ROS2 gz {0.0,0.0,0.0,0.0,0.0,0.0} \. --