From ae7053c4a880b309c98d49969f77a1f0df234d5d Mon Sep 17 00:00:00 2001 From: jepear19 Date: Mon, 23 Mar 2026 20:04:30 +0100 Subject: [PATCH 01/32] Add universe machine vision in gazebo harmonic --- .../machine_vision_harmonic.launch.py | 23 +++ .../machine_vision_harmonic/robot.launch.py | 71 +++++++ .../machine_vision_harmonic/world.launch.py | 32 +++ Worlds/machine_vision_harmonic.world | 193 ++++++++++++++++++ database/universes.sql | 2 + 5 files changed, 321 insertions(+) create mode 100644 Launchers/machine_vision_harmonic/machine_vision_harmonic.launch.py create mode 100644 Launchers/machine_vision_harmonic/robot.launch.py create mode 100644 Launchers/machine_vision_harmonic/world.launch.py create mode 100644 Worlds/machine_vision_harmonic.world diff --git a/Launchers/machine_vision_harmonic/machine_vision_harmonic.launch.py b/Launchers/machine_vision_harmonic/machine_vision_harmonic.launch.py new file mode 100644 index 000000000..d766d2864 --- /dev/null +++ b/Launchers/machine_vision_harmonic/machine_vision_harmonic.launch.py @@ -0,0 +1,23 @@ +""" +Machine Vision Harmonic - Main Launcher +""" + +import os +from launch import LaunchDescription +from launch.actions import IncludeLaunchDescription +from launch.launch_description_sources import PythonLaunchDescriptionSource + + +def generate_launch_description(): + + base_dir = os.path.dirname(__file__) + + world_launch = IncludeLaunchDescription( + PythonLaunchDescriptionSource(os.path.join(base_dir, "world.launch.py")) + ) + + robot_launch = IncludeLaunchDescription( + PythonLaunchDescriptionSource(os.path.join(base_dir, "robot.launch.py")) + ) + + return LaunchDescription([world_launch, robot_launch]) \ No newline at end of file diff --git a/Launchers/machine_vision_harmonic/robot.launch.py b/Launchers/machine_vision_harmonic/robot.launch.py new file mode 100644 index 000000000..2afeb0a42 --- /dev/null +++ b/Launchers/machine_vision_harmonic/robot.launch.py @@ -0,0 +1,71 @@ +""" +Machine Vision Harmonic - Robot Launcher +""" + +import os +import xacro +from launch import LaunchDescription +from launch_ros.actions import Node +from ament_index_python.packages import get_package_share_directory + + +def generate_launch_description(): + + PACKAGE_NAME = "ros2srrc_ur5" + + pkg_path = get_package_share_directory(PACKAGE_NAME + "_gazebo") + + # URDF (como en classic) + xacro_file = os.path.join(pkg_path, "urdf", "ur5.urdf.xacro") + + doc = xacro.parse(open(xacro_file)) + xacro.process_doc(doc) + + robot_description = {"robot_description": doc.toxml()} + + # Robot State Publisher + robot_state_publisher = Node( + package="robot_state_publisher", + executable="robot_state_publisher", + output="screen", + parameters=[robot_description, {"use_sim_time": True}], + ) + + # Static TF + static_tf = Node( + package="tf2_ros", + executable="static_transform_publisher", + arguments=["0", "0", "0", "0", "0", "0", "world", "base_link"], + ) + + # Spawn robot en GZ (adaptado a harmonic) + spawn_entity = Node( + package="ros_gz_sim", + executable="create", + arguments=[ + "-name", "ur5", + "-topic", "robot_description" + ], + output="screen", + ) + + # Controllers (mínimos) + joint_state_broadcaster = Node( + package="controller_manager", + executable="spawner", + arguments=["joint_state_broadcaster"], + ) + + joint_trajectory_controller = Node( + package="controller_manager", + executable="spawner", + arguments=["joint_trajectory_controller"], + ) + + return LaunchDescription([ + robot_state_publisher, + static_tf, + spawn_entity, + joint_state_broadcaster, + joint_trajectory_controller, + ]) \ No newline at end of file diff --git a/Launchers/machine_vision_harmonic/world.launch.py b/Launchers/machine_vision_harmonic/world.launch.py new file mode 100644 index 000000000..5c843e147 --- /dev/null +++ b/Launchers/machine_vision_harmonic/world.launch.py @@ -0,0 +1,32 @@ +""" +Machine Vision Harmonic - World Launcher +""" + +import os +from launch import LaunchDescription +from launch.actions import ExecuteProcess, SetEnvironmentVariable + + +def generate_launch_description(): + + base_dir = os.path.dirname(__file__) + + world_path = os.path.join(base_dir, "machine_vision_harmonic.world") + + gazebo = ExecuteProcess( + cmd=["gz", "sim", "-r", "-v", "4", world_path], + output="screen", + ) + + ld = LaunchDescription() + + ld.add_action( + SetEnvironmentVariable( + "GZ_SIM_RESOURCE_PATH", + os.environ.get("GZ_SIM_RESOURCE_PATH", "") + ) + ) + + ld.add_action(gazebo) + + return ld \ No newline at end of file diff --git a/Worlds/machine_vision_harmonic.world b/Worlds/machine_vision_harmonic.world new file mode 100644 index 000000000..6d6930eb6 --- /dev/null +++ b/Worlds/machine_vision_harmonic.world @@ -0,0 +1,193 @@ + + + + + + true + 0 0 10 0 0 0 + 0.8 0.8 0.8 1 + 0.2 0.2 0.2 1 + -0.5 0.1 -0.9 + + + + + true + + + + + 0 0 1 + 100 100 + + + + + + + 0 0 1 + 100 100 + + + + + + + + + true + 0.6 0 0 0 0 1.57 + + + 0 0 0.95 0 0 0 + + + 2.4 0.8 0.1 + + + + + 0 0 0.95 0 0 0 + + + 2.4 0.8 0.1 + + + + + + + + + true + -0.8 0 0 0 0 0 + + + 0 0 0.73 0 0 0 + + + 1.6 0.8 0.06 + + + + + 0 0 0.73 0 0 0 + + + 1.6 0.8 0.06 + + + + + + + + + + + 0.45 -0.25 1.01 0 0 0 + + + + 0.03 + + + 1 0 0 1 + + + + + + + 0.45 0.25 1.01 0 0 0 + + + + 0.03 + + + 0 0 1 1 + + + + + + + 0.45 0.1 1.01 0 0 0 + + + + 0.03 + + + 0 1 0 1 + + + + + + + + 0.65 -0.25 1.01 0 0 0 + + + + + 0.03 + 0.07 + + + + 0 0 1 1 + + + + + + + 0.65 0.1 1.01 0 0 0 + + + + + 0.03 + 0.07 + + + + 1 0 0 1 + + + + + + + + 1.2 0 2 0 1.2 3.14 + + + + 1.047 + + 640 + 480 + + + 0.1 + 10 + + + true + 30 + + + + + + + 0.001 + 1 + + + + \ No newline at end of file diff --git a/database/universes.sql b/database/universes.sql index 821cd3f35..2e383a551 100644 --- a/database/universes.sql +++ b/database/universes.sql @@ -191,6 +191,7 @@ COPY public.universes (id, name, world_id, robot_id) FROM stdin; 63 Laser Mapping Warehouse High Noise 63 0 64 Small Laser Mapping Warehouse Medium Noise 64 0 65 Small Laser Mapping Warehouse High Noise 65 0 +66 Machine vision Harmonic world 66 1 \. -- @@ -265,6 +266,7 @@ COPY public.worlds (id, name, launch_file_path, tools_config, ros_version, type, 63 Laser Mapping Warehouse High Noise /opt/jderobot/Launchers/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} 64 Small Laser Mapping Warehouse Medium Noise /opt/jderobot/Launchers/small_laser_mapping_noise_med.launch.py {"gzsim":"/opt/jderobot/Launchers/visualization/laser_mapping.config"} ROS2 gz {0.0,0.0,0.0,0.0,0.0,0.0} 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 Machine Vision Harmonic /opt/jderobot/Launchers/machine_vision_harmonic/machine_vision_harmonic.launch.py {"rviz":"/home/dev_ws/src/IndustrialRobots/ros2_SimRealRobotControl/ros2srrc_launch/moveit2/machine_vision_rviz.launch.py"} ROS2 gz {0.0,0.0,0.0,0.0,0.0,0.0} \. -- From 3c1cafa1c0f764a7b80ab34adf7587e3e9046b49 Mon Sep 17 00:00:00 2001 From: jepear19 Date: Tue, 24 Mar 2026 09:23:19 +0100 Subject: [PATCH 02/32] Fix database --- database/universes.sql | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/database/universes.sql b/database/universes.sql index 2e383a551..981862017 100644 --- a/database/universes.sql +++ b/database/universes.sql @@ -191,7 +191,7 @@ COPY public.universes (id, name, world_id, robot_id) FROM stdin; 63 Laser Mapping Warehouse High Noise 63 0 64 Small Laser Mapping Warehouse Medium Noise 64 0 65 Small Laser Mapping Warehouse High Noise 65 0 -66 Machine vision Harmonic world 66 1 +66 Machine vision Harmonic world 66 0 \. -- From bb444e901b91f3549d23c38da0e5c3e3c82a7228 Mon Sep 17 00:00:00 2001 From: jepear19 Date: Tue, 24 Mar 2026 10:08:39 +0100 Subject: [PATCH 03/32] Fix world path at launcher --- Launchers/machine_vision_harmonic/world.launch.py | 4 +--- 1 file changed, 1 insertion(+), 3 deletions(-) diff --git a/Launchers/machine_vision_harmonic/world.launch.py b/Launchers/machine_vision_harmonic/world.launch.py index 5c843e147..fe3ef9462 100644 --- a/Launchers/machine_vision_harmonic/world.launch.py +++ b/Launchers/machine_vision_harmonic/world.launch.py @@ -9,9 +9,7 @@ def generate_launch_description(): - base_dir = os.path.dirname(__file__) - - world_path = os.path.join(base_dir, "machine_vision_harmonic.world") + world_path = "/opt/jderobot/Worlds/machine_vision_harmonic.world" gazebo = ExecuteProcess( cmd=["gz", "sim", "-r", "-v", "4", world_path], From 199913b61948ee041af2a31fd0f2c82cc3750bdc Mon Sep 17 00:00:00 2001 From: jepear19 Date: Tue, 24 Mar 2026 10:50:19 +0100 Subject: [PATCH 04/32] Change .world --- Worlds/machine_vision_harmonic.world | 1596 ++++++++++++++++++++++++-- 1 file changed, 1501 insertions(+), 95 deletions(-) diff --git a/Worlds/machine_vision_harmonic.world b/Worlds/machine_vision_harmonic.world index 6d6930eb6..a3dcfa93f 100644 --- a/Worlds/machine_vision_harmonic.world +++ b/Worlds/machine_vision_harmonic.world @@ -1,193 +1,1599 @@ - - - - - - true - 0 0 10 0 0 0 + + + + 1 + 0 0 10 0 -0 0 0.8 0.8 0.8 1 0.2 0.2 0.2 1 + + 1000 + 0.9 + 0.01 + 0.001 + -0.5 0.1 -0.9 - - - - true - - + + 1 + + 0 0 1 100 100 + + + 65535 + + + + + 100 + 50 + + + + + + + + 10 - + + 0 0 0 1 100 100 + + + + 0 + 0 + 0 - - - - true - 0.6 0 0 0 0 1.57 - - - 0 0 0.95 0 0 0 + 0 0 -9.8 + 6e-06 2.3e-05 -4.2e-05 + + + 0.001 + 1 + 1000 + + + 0.4 0.4 0.4 1 + 0.7 0.7 0.7 1 + 1 + + + + EARTH_WGS84 + 0 + 0 + 0 + 0 + + + + 0 + 0 + 0 + 0 0 0 0 -0 0 + 1 + + 1 + 0 0 0 0 -0 0 + + 1 + 0 + 0 + 1 + 0 + 1 + + + + 0 0 0.95 0 -0 0 - - 2.4 0.8 0.1 - + + 0.45 0.35 0.5 + model://conveyor/meshes/conveyor.dae + - - 0 0 0.95 0 0 0 + + 0 + 10 + 0 0 0.95 0 -0 0 2.4 0.8 0.1 + + + + 1 + 1 + 0 0 0 + 0 + 0 + + + 1 + 0 + 0 + 1 + + 0 + + + + + 0 + 1e+06 + + + 0 + 1 + 1 + + 0 + 0.2 + 1e+13 + 1 + 0.01 + 0 + + + 1 + -0.01 + 0 + 0.2 + 1e+13 + 1 + + + + 1 + 1 + 0.335211 0.377742 0 0 -0 0 + + + 1 + + + 0 0 0.001 0 -0 0 + + + 1 1 1 + model://ground_logo_big/meshes/ground_logo.dae + + + + 0 + 0 + 0 + + -0.163565 0.817691 0 0 -0 0 + + 3373 189000000 + 330 840055555 + 1595801396 179696441 + 329598 + + 0.6 0 0 0 -0 1.57 + 1 1 1 + + 0.6 0 0 0 -0 1.57 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + + -0.5 0 0 0 -0 1.57 + 1 1 1 + + -0.5 0 0 0 -0 1.57 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + + 0 0 0 0 -0 0 + 1 1 1 + + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + - - - true - -0.8 0 0 0 0 0 - - - 0 0 0.73 0 0 0 + + -0.5 0 0.76 0 -0 0 + 1 1 1 + + -0.5 0 0.76 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + + -0.6 0 0 0 -0 1.57 + 1 1 1 + + -0.6 0 0 0 -0 1.57 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + + -0.7 -0.435456 0.927284 0 -2e-06 -0.000979 + 0.4 0.04449 0.5 + + -0.7 -0.435456 0.927284 0 -2e-06 -0.000979 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + + -0.3 0.38 0.897305 -1.3e-05 -8.1e-05 2.9e-05 + 0.170141 0.170141 0.4 + + -0.3 0.38 0.897305 -1.3e-05 -8.1e-05 2.9e-05 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + + 0 0 10 0 -0 0 + + + + + 4.32698 1.43011 2.70567 0 0.293795 -2.85637 + orbit + perspective + + + + + 0 + 0 + 0 + 0 0 0 0 -0 0 + 1 + + 1 + 0 0 0 0 -0 0 + + 1 + 0 + 0 + 1 + 0 + 1 + + + + 0 0 0.73 0 -0 1.57 + + + 1 1 1 + model://target_table/meshes/target_table.dae + + + + + 0 + 10 + 0 0 0.73 0 -0 0 1.6 0.8 0.06 + + + + 1 + 1 + 0 0 0 + 0 + 0 + + + 1 + 0 + 0 + 1 + + 0 + + + + + 0 + 1e+06 + + + 0 + 1 + 1 + + 0 + 0.2 + 1e+13 + 1 + 0.01 + 0 + + + 1 + -0.01 + 0 + 0.2 + 1e+13 + 1 + + + + + + 1 + 1 + -0.877342 -1.1194 0 0 -0 0 + + + + + 0 + 0 + 0 + 0 0 0 0 -0 0 + 1 + + 1 + 0 0 0 0 -0 0 + + 1 + 0 + 0 + 1 + 0 + 1 + + + + 0 0 0.0075 0 -0 0 + + + 0.25 0.25 0.25 + model://target_4x4/meshes/target_4x4.dae + + - - 0 0 0.73 0 0 0 + + 0 + 10 + -0.235 0 0 0 -0 0 - 1.6 0.8 0.06 + 0.015 0.5 0.335 + + + + + + 1 + 1 + 0 0 0 + 0 + 0 + + + 1 + 0 + 0 + 1 + + 0 + + + + + 0 + 1e+06 + + + 0 + 1 + 1 + + 0 + 0.2 + 1e+13 + 1 + 0.01 + 0 + + + 1 + -0.01 + 0 + 0.2 + 1e+13 + 1 + + + + + + 0 + 10 + 0.0025 0 0 0 -0 0 + + + 0.005 0.5 0.325 + + + + 1 + 1 + 0 0 0 + 0 + 0 + + + 1 + 0 + 0 + 1 + + 0 + + + + + 0 + 1e+06 + + + 0 + 1 + 1 + + 0 + 0.2 + 1e+13 + 1 + 0.01 + 0 + + + 1 + -0.01 + 0 + 0.2 + 1e+13 + 1 + + + + + + 0 + 10 + 0.12 0 0 0 -0 0 + + + 0.005 0.5 0.325 + + + + + + 1 + 1 + 0 0 0 + 0 + 0 + + + 1 + 0 + 0 + 1 + + 0 + + + + + 0 + 1e+06 + + + 0 + 1 + 1 + + 0 + 0.2 + 1e+13 + 1 + 0.01 + 0 + + + 1 + -0.01 + 0 + 0.2 + 1e+13 + 1 + + + + + + 0 + 10 + 0.235 0 0 0 -0 0 + + + 0.015 0.5 0.335 + + + + + + 1 + 1 + 0 0 0 + 0 + 0 + + + 1 + 0 + 0 + 1 + + 0 + + + + + 0 + 1e+06 + + + 0 + 1 + 1 + + 0 + 0.2 + 1e+13 + 1 + 0.01 + 0 + + + 1 + -0.01 + 0 + 0.2 + 1e+13 + 1 + + + + + + 0 + 10 + 0 0.235 0 0 -0 0 + + + 0.5 0.015 0.335 + + + + + + 1 + 1 + 0 0 0 + 0 + 0 + + + 1 + 0 + 0 + 1 + + 0 + + + + + 0 + 1e+06 + + + 0 + 1 + 1 + + 0 + 0.2 + 1e+13 + 1 + 0.01 + 0 + + + 1 + -0.01 + 0 + 0.2 + 1e+13 + 1 + + + + + + 0 + 10 + 0 -0.235 0 0 -0 0 + + + 0.5 0.015 0.335 + + + + + + 1 + 1 + 0 0 0 + 0 + 0 + + + 1 + 0 + 0 + 1 + + 0 + + + + + 0 + 1e+06 + + + 0 + 1 + 1 + + 0 + 0.2 + 1e+13 + 1 + 0.01 + 0 + + + 1 + -0.01 + 0 + 0.2 + 1e+13 + 1 + + + + + + 0 + 10 + 0 0 0 0 -0 0 + + + 0.5 0.5 0.02 + + + + + + 1 + 1 + 0 0 0 + 0 + 0 + + + 1 + 0 + 0 + 1 + + 0 + + + + + 0 + 1e+06 + + + 0 + 1 + 1 + + 0 + 0.2 + 1e+13 + 1 + 0.01 + 0 + + + 1 + -0.01 + 0 + 0.2 + 1e+13 + 1 + + + + + + 0 + 10 + -0.115 0 0 0 -0 0 + + + 0.015 0.5 0.325 + + + + + + 1 + 1 + 0 0 0 + 0 + 0 + + + 1 + 0 + 0 + 1 + + 0 + + + + + 0 + 1e+06 + + + 0 + 1 + 1 + + 0 + 0.2 + 1e+13 + 1 + 0.01 + 0 + + + 1 + -0.01 + 0 + 0.2 + 1e+13 + 1 + + + + + + 0 + 10 + 0 0.0025 0 0 -0 0 + + + 0.5 0.005 0.325 + + + + + + 1 + 1 + 0 0 0 + 0 + 0 + + + 1 + 0 + 0 + 1 + + 0 + + + + + 0 + 1e+06 + + + 0 + 1 + 1 + + 0 + 0.2 + 1e+13 + 1 + 0.01 + 0 + + + 1 + -0.01 + 0 + 0.2 + 1e+13 + 1 + + + + + + 0 + 10 + 0 0.12 0 0 -0 0 + + + 0.5 0.005 0.325 + + + + + + 1 + 1 + 0 0 0 + 0 + 0 + + + 1 + 0 + 0 + 1 + + 0 + + + + + 0 + 1e+06 + + + 0 + 1 + 1 + + 0 + 0.2 + 1e+13 + 1 + 0.01 + 0 + + + 1 + -0.01 + 0 + 0.2 + 1e+13 + 1 + + + + + + 0 + 10 + 0 -0.115 0 0 -0 0 + + + 0.5 0.005 0.325 + + + + + + 1 + 1 + 0 0 0 + 0 + 0 + + + 1 + 0 + 0 + 1 + + 0 + + + + + 0 + 1e+06 + + + 0 + 1 + 1 + + 0 + 0.2 + 1e+13 + 1 + 0.01 + 0 + + + 1 + -0.01 + 0 + 0.2 + 1e+13 + 1 + + + + + + 1 + 1 + -1.16917 0.373609 0 0 -0 0 + + + -1.8531 -0.100865 0.5 0 -0 0 + + + 1 + + 0.166667 + 0 + 0 + 0.166667 + 0 + 0.166667 + + 0 0 0 0 -0 0 + + + + + 1 1 0.999999 + + + 10 + + + + + + + + + + + + + + + + + 1 1 0.999999 + + + + + + + 0 + 0 + 0 + + 1 + + + -1.40372 -3.12264 0.5 0 -0 0 + + + 1 + + 0.145833 + 0 + 0 + 0.145833 + 0 + 0.125 + + 0 0 0 0 -0 0 + + + + + 0.499999 + 1 + + + 10 + + + + + + + + + + + + + + + + + 0.499999 + 1 + + + + + + + 0 + 0 + 0 + + 1 + + + + + + 0 0 0 0 -0 0 + 0.01 + + 0 + 0 + 0 + 0 + 0 + 0 + + + + 0 0 0 0 -0 0 + + + 0.03 + + + + + + + + + + + + + + + 10 + + 0 0 0 0 -0 0 + + + 0.03 + + + + + + + 0 + 0 + 0 + 0.45 0.25 1.01 0 -0 0 - - - - 0.45 -0.25 1.01 0 0 0 - - + + + + 0 0 0 0 -0 0 + 0.01 + + 0 + 0 + 0 + 0 + 0 + 0 + + + + 0 0 0 0 -0 0 - 0.03 + + 0.03 + + + + + + + + + + + + + + + 10 + + + 0 0 0 0 -0 0 + + + 0.03 + - 1 0 0 1 + + 0 + 0 + 0 + 0.45 -0.25 1.01 0 -0 0 - - 0.45 0.25 1.01 0 0 0 - - + + + + + 0 0 0 0 -0 0 + 0.01 + + 0 + 0 + 0 + 0 + 0 + 0 + + + + 0 0 0 0 -0 0 + + + 0.03 + + + + + + + + + + + + + + + 10 + + + 0 0 0 0 -0 0 - 0.03 + + 0.03 + - 0 0 1 1 + + 0 + 0 + 0 + 0.45 0.09 1.01 0 -0 0 - - 0.45 0.1 1.01 0 0 0 - - + + + + 0 0 0 0 -0 0 + 0.01 + + 0 + 0 + 0 + 0 + 0 + 0 + + + + 0 0 0 0 -0 0 - 0.03 + + 0.03 + + + + + + + + + + + + + + + 10 + + + 0 0 0 0 -0 0 + + + 0.03 + - 0 1 0 1 + + 0 + 0 + 0 + 0.45 -0.09 1.01 0 -0 0 - - - 0.65 -0.25 1.01 0 0 0 - - + + + + + + + + 0 0 0 0 -0 0 + 0.01 + + 0 + 0 + 0 + 0 + 0 + 0 + + + + 0 0 0 0 -0 0 0.03 - 0.07 + 0.05 + + + + + + + + + + + + + + + 10 + + + 0 0 0 0 -0 0 + + + 0.03 + 0.05 - 0 0 1 1 + + 0 + 0 + 0 + 0.65 -0.09 1.01 0 -0 0 - - 0.65 0.1 1.01 0 0 0 - - + + + + 0 0 0 0 -0 0 + 0.01 + + 0 + 0 + 0 + 0 + 0 + 0 + + + + 0 0 0 0 -0 0 - 0.03 - 0.07 + 0.02 + 0.06 + + + + + + + + + + + + + + + 10 + + + 0 0 0 0 -0 0 + + + 0.02 + 0.06 - 1 0 0 1 + + 0 + 0 + 0 + 0.65 0.25 1.01 0 -0 0 - - - 1.2 0 2 0 1.2 3.14 - - - - 1.047 - - 640 - 480 - - - 0.1 - 10 - - - true - 30 - + + + + 0 0 0 0 -0 0 + 0.01 + + 0 + 0 + 0 + 0 + 0 + 0 + + + + 0 0 0 0 -0 0 + + + 0.02 + 0.06 + + + + + + + + + + + + + + + 10 + + + 0 0 0 0 -0 0 + + + 0.02 + 0.06 + + + + + + + 0 + 0 + 0 + 0.65 0.09 1.01 0 -0 0 - - - 0.001 - 1 - + + + + + 0 0 0 0 -0 0 + 0.01 + + 0 + 0 + 0 + 0 + 0 + 0 + + + + 0 0 0 0 -0 0 + + + 0.03 + 0.07 + + + + + + + + + + + + + + + 10 + + + 0 0 0 0 -0 0 + + + 0.03 + 0.07 + + + + + + + 0 + 0 + 0 + + 0.65 -0.25 1.01 0 -0 0 + + + + + + + + \ No newline at end of file From b45367f716970501ea9fad432761b7701a46ef35 Mon Sep 17 00:00:00 2001 From: jepear19 Date: Tue, 24 Mar 2026 11:32:58 +0100 Subject: [PATCH 05/32] Delate state from .world --- Worlds/machine_vision_harmonic.world | 91 +--------------------------- 1 file changed, 1 insertion(+), 90 deletions(-) diff --git a/Worlds/machine_vision_harmonic.world b/Worlds/machine_vision_harmonic.world index a3dcfa93f..2369d1ef4 100644 --- a/Worlds/machine_vision_harmonic.world +++ b/Worlds/machine_vision_harmonic.world @@ -188,96 +188,7 @@ -0.163565 0.817691 0 0 -0 0 - - 3373 189000000 - 330 840055555 - 1595801396 179696441 - 329598 - - 0.6 0 0 0 -0 1.57 - 1 1 1 - - 0.6 0 0 0 -0 1.57 - 0 0 0 0 -0 0 - 0 0 0 0 -0 0 - 0 0 0 0 -0 0 - - - - -0.5 0 0 0 -0 1.57 - 1 1 1 - - -0.5 0 0 0 -0 1.57 - 0 0 0 0 -0 0 - 0 0 0 0 -0 0 - 0 0 0 0 -0 0 - - - - 0 0 0 0 -0 0 - 1 1 1 - - 0 0 0 0 -0 0 - 0 0 0 0 -0 0 - 0 0 0 0 -0 0 - 0 0 0 0 -0 0 - - - - - - -0.5 0 0.76 0 -0 0 - 1 1 1 - - -0.5 0 0.76 0 -0 0 - 0 0 0 0 -0 0 - 0 0 0 0 -0 0 - 0 0 0 0 -0 0 - - - - -0.6 0 0 0 -0 1.57 - 1 1 1 - - -0.6 0 0 0 -0 1.57 - 0 0 0 0 -0 0 - 0 0 0 0 -0 0 - 0 0 0 0 -0 0 - - - - -0.7 -0.435456 0.927284 0 -2e-06 -0.000979 - 0.4 0.04449 0.5 - - -0.7 -0.435456 0.927284 0 -2e-06 -0.000979 - 0 0 0 0 -0 0 - 0 0 0 0 -0 0 - 0 0 0 0 -0 0 - - - - -0.3 0.38 0.897305 -1.3e-05 -8.1e-05 2.9e-05 - 0.170141 0.170141 0.4 - - -0.3 0.38 0.897305 -1.3e-05 -8.1e-05 2.9e-05 - 0 0 0 0 -0 0 - 0 0 0 0 -0 0 - 0 0 0 0 -0 0 - - - - 0 0 10 0 -0 0 - - + 4.32698 1.43011 2.70567 0 0.293795 -2.85637 From 9c02d08bcd309d6c5418757cdb5c200bdaf72ca5 Mon Sep 17 00:00:00 2001 From: jepear19 Date: Tue, 24 Mar 2026 11:59:57 +0100 Subject: [PATCH 06/32] Force pose to world in .world --- Worlds/machine_vision_harmonic.world | 155 +++++++++++++++++++++------ 1 file changed, 122 insertions(+), 33 deletions(-) diff --git a/Worlds/machine_vision_harmonic.world b/Worlds/machine_vision_harmonic.world index 2369d1ef4..178451d5c 100644 --- a/Worlds/machine_vision_harmonic.world +++ b/Worlds/machine_vision_harmonic.world @@ -2,7 +2,7 @@ 1 - 0 0 10 0 -0 0 + 0 0 10 0 -0 0 0.8 0.8 0.8 1 0.2 0.2 0.2 1 @@ -87,11 +87,11 @@ 0 0 0 - 0 0 0 0 -0 0 + 0 0 0 0 -0 0 1 1 - 0 0 0 0 -0 0 + 0 0 0 0 -0 0 1 0 @@ -102,7 +102,7 @@ - 0 0 0.95 0 -0 0 + 0 0 0.95 0 -0 0 0.45 0.35 0.5 @@ -113,7 +113,7 @@ 0 10 - 0 0 0.95 0 -0 0 + 0 0 0.95 0 -0 0 2.4 0.8 0.1 @@ -168,13 +168,13 @@ 1 1 - 0.335211 0.377742 0 0 -0 0 + 0.335211 0.377742 0 0 -0 0 1 - 0 0 0.001 0 -0 0 + 0 0 0.001 0 -0 0 1 1 1 @@ -186,12 +186,101 @@ 0 0 - -0.163565 0.817691 0 0 -0 0 + -0.163565 0.817691 0 0 -0 0 - + + 3373 189000000 + 330 840055555 + 1595801396 179696441 + 329598 + + 0.6 0 0 0 -0 1.57 + 1 1 1 + + 0.6 0 0 0 -0 1.57 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + + -0.5 0 0 0 -0 1.57 + 1 1 1 + + -0.5 0 0 0 -0 1.57 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + + 0 0 0 0 -0 0 + 1 1 1 + + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + + + + -0.5 0 0.76 0 -0 0 + 1 1 1 + + -0.5 0 0.76 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + + -0.6 0 0 0 -0 1.57 + 1 1 1 + + -0.6 0 0 0 -0 1.57 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + + -0.7 -0.435456 0.927284 0 -2e-06 -0.000979 + 0.4 0.04449 0.5 + + -0.7 -0.435456 0.927284 0 -2e-06 -0.000979 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + + -0.3 0.38 0.897305 -1.3e-05 -8.1e-05 2.9e-05 + 0.170141 0.170141 0.4 + + -0.3 0.38 0.897305 -1.3e-05 -8.1e-05 2.9e-05 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + + 0 0 10 0 -0 0 + + - 4.32698 1.43011 2.70567 0 0.293795 -2.85637 + 4.32698 1.43011 2.70567 0 0.293795 -2.85637 orbit perspective @@ -201,11 +290,11 @@ 0 0 0 - 0 0 0 0 -0 0 + 0 0 0 0 -0 0 1 1 - 0 0 0 0 -0 0 + 0 0 0 0 -0 0 1 0 @@ -216,7 +305,7 @@ - 0 0 0.73 0 -0 1.57 + 0 0 0.73 0 -0 1.57 1 1 1 @@ -227,7 +316,7 @@ 0 10 - 0 0 0.73 0 -0 0 + 0 0 0.73 0 -0 0 1.6 0.8 0.06 @@ -282,7 +371,7 @@ 1 1 - -0.877342 -1.1194 0 0 -0 0 + -0.877342 -1.1194 0 0 -0 0 @@ -290,11 +379,11 @@ 0 0 0 - 0 0 0 0 -0 0 + 0 0 0 0 -0 0 1 1 - 0 0 0 0 -0 0 + 0 0 0 0 -0 0 1 0 @@ -305,7 +394,7 @@ - 0 0 0.0075 0 -0 0 + 0 0 0.0075 0 -0 0 0.25 0.25 0.25 @@ -316,7 +405,7 @@ 0 10 - -0.235 0 0 0 -0 0 + -0.235 0 0 0 -0 0 0.015 0.5 0.335 @@ -371,7 +460,7 @@ 0 10 - 0.0025 0 0 0 -0 0 + 0.0025 0 0 0 -0 0 0.005 0.5 0.325 @@ -426,7 +515,7 @@ 0 10 - 0.12 0 0 0 -0 0 + 0.12 0 0 0 -0 0 0.005 0.5 0.325 @@ -481,7 +570,7 @@ 0 10 - 0.235 0 0 0 -0 0 + 0.235 0 0 0 -0 0 0.015 0.5 0.335 @@ -536,7 +625,7 @@ 0 10 - 0 0.235 0 0 -0 0 + 0 0.235 0 0 -0 0 0.5 0.015 0.335 @@ -591,7 +680,7 @@ 0 10 - 0 -0.235 0 0 -0 0 + 0 -0.235 0 0 -0 0 0.5 0.015 0.335 @@ -646,7 +735,7 @@ 0 10 - 0 0 0 0 -0 0 + 0 0 0 0 -0 0 0.5 0.5 0.02 @@ -701,7 +790,7 @@ 0 10 - -0.115 0 0 0 -0 0 + -0.115 0 0 0 -0 0 0.015 0.5 0.325 @@ -756,7 +845,7 @@ 0 10 - 0 0.0025 0 0 -0 0 + 0 0.0025 0 0 -0 0 0.5 0.005 0.325 @@ -811,7 +900,7 @@ 0 10 - 0 0.12 0 0 -0 0 + 0 0.12 0 0 -0 0 0.5 0.005 0.325 @@ -866,7 +955,7 @@ 0 10 - 0 -0.115 0 0 -0 0 + 0 -0.115 0 0 -0 0 0.5 0.005 0.325 @@ -921,10 +1010,10 @@ 1 1 - -1.16917 0.373609 0 0 -0 0 + -1.16917 0.373609 0 0 -0 0 - -1.8531 -0.100865 0.5 0 -0 0 + -1.8531 -0.100865 0.5 0 -0 0 1 @@ -936,7 +1025,7 @@ 0 0.166667 - 0 0 0 0 -0 0 + 0 0 0 0 -0 0 @@ -978,7 +1067,7 @@ 1 - -1.40372 -3.12264 0.5 0 -0 0 + -1.40372 -3.12264 0.5 0 -0 0 1 From fbb034e23729ae18461af739082bb7cdfc3684b1 Mon Sep 17 00:00:00 2001 From: jepear19 Date: Tue, 24 Mar 2026 12:28:42 +0100 Subject: [PATCH 07/32] Change .world --- Worlds/machine_vision_harmonic.world | 1547 +------------------------- 1 file changed, 48 insertions(+), 1499 deletions(-) diff --git a/Worlds/machine_vision_harmonic.world b/Worlds/machine_vision_harmonic.world index 178451d5c..538af86fe 100644 --- a/Worlds/machine_vision_harmonic.world +++ b/Worlds/machine_vision_harmonic.world @@ -1,20 +1,18 @@ + + - 1 - 0 0 10 0 -0 0 + true + 0 0 10 0 0 0 0.8 0.8 0.8 1 0.2 0.2 0.2 1 - - 1000 - 0.9 - 0.01 - 0.001 - -0.5 0.1 -0.9 + + - 1 + true @@ -23,86 +21,28 @@ 100 100 - - - 65535 - - - - - 100 - 50 - - - - - - - - 10 - 0 0 0 1 100 100 - - - - 0 - 0 - 0 - 0 0 -9.8 - 6e-06 2.3e-05 -4.2e-05 - - - 0.001 - 1 - 1000 - - - 0.4 0.4 0.4 1 - 0.7 0.7 0.7 1 - 1 - - - - EARTH_WGS84 - 0 - 0 - 0 - 0 - + + + true + 0.335211 0.377742 0 0 0 0 + - 0 - 0 - 0 - 0 0 0 0 -0 0 - 1 - - 1 - 0 0 0 0 -0 0 - - 1 - 0 - 0 - 1 - 0 - 1 - - + 0 0 0 0 0 0 + - 0 0 0.95 0 -0 0 + 0 0 0.95 0 0 0 0.45 0.35 0.5 @@ -110,1490 +50,99 @@ - - 0 - 10 - 0 0 0.95 0 -0 0 + + + 0 0 0.95 0 0 0 2.4 0.8 0.1 - - - - 1 - 1 - 0 0 0 - 0 - 0 - - - 1 - 0 - 0 - 1 - - 0 - - - - - 0 - 1e+06 - - - 0 - 1 - 1 - - 0 - 0.2 - 1e+13 - 1 - 0.01 - 0 - - - 1 - -0.01 - 0 - 0.2 - 1e+13 - 1 - - - + - 1 - 1 - 0.335211 0.377742 0 0 -0 0 - - - 1 - - - 0 0 0.001 0 -0 0 - - - 1 1 1 - model://ground_logo_big/meshes/ground_logo.dae - - - - 0 - 0 - 0 - - -0.163565 0.817691 0 0 -0 0 - - 3373 189000000 - 330 840055555 - 1595801396 179696441 - 329598 - - 0.6 0 0 0 -0 1.57 - 1 1 1 - - 0.6 0 0 0 -0 1.57 - 0 0 0 0 -0 0 - 0 0 0 0 -0 0 - 0 0 0 0 -0 0 - - - - -0.5 0 0 0 -0 1.57 - 1 1 1 - - -0.5 0 0 0 -0 1.57 - 0 0 0 0 -0 0 - 0 0 0 0 -0 0 - 0 0 0 0 -0 0 - - - - 0 0 0 0 -0 0 - 1 1 1 - - 0 0 0 0 -0 0 - 0 0 0 0 -0 0 - 0 0 0 0 -0 0 - 0 0 0 0 -0 0 - - - - - -0.5 0 0.76 0 -0 0 - 1 1 1 - - -0.5 0 0.76 0 -0 0 - 0 0 0 0 -0 0 - 0 0 0 0 -0 0 - 0 0 0 0 -0 0 - - - - -0.6 0 0 0 -0 1.57 - 1 1 1 - - -0.6 0 0 0 -0 1.57 - 0 0 0 0 -0 0 - 0 0 0 0 -0 0 - 0 0 0 0 -0 0 - - - - -0.7 -0.435456 0.927284 0 -2e-06 -0.000979 - 0.4 0.04449 0.5 - - -0.7 -0.435456 0.927284 0 -2e-06 -0.000979 - 0 0 0 0 -0 0 - 0 0 0 0 -0 0 - 0 0 0 0 -0 0 - - - - -0.3 0.38 0.897305 -1.3e-05 -8.1e-05 2.9e-05 - 0.170141 0.170141 0.4 - - -0.3 0.38 0.897305 -1.3e-05 -8.1e-05 2.9e-05 - 0 0 0 0 -0 0 - 0 0 0 0 -0 0 - 0 0 0 0 -0 0 - - - - 0 0 10 0 -0 0 - - - - - 4.32698 1.43011 2.70567 0 0.293795 -2.85637 - orbit - perspective - - + + true + -0.877342 -1.1194 0 0 0 0 + - 0 - 0 - 0 - 0 0 0 0 -0 0 - 1 - - 1 - 0 0 0 0 -0 0 - - 1 - 0 - 0 - 1 - 0 - 1 - - - 0 0 0.73 0 -0 1.57 + 0 0 0.73 0 0 1.57 - 1 1 1 model://target_table/meshes/target_table.dae - - 0 - 10 - 0 0 0.73 0 -0 0 - - - 1.6 0.8 0.06 - - - - - - 1 - 1 - 0 0 0 - 0 - 0 - - - 1 - 0 - 0 - 1 - - 0 - - - - - 0 - 1e+06 - - - 0 - 1 - 1 - - 0 - 0.2 - 1e+13 - 1 - 0.01 - 0 - - - 1 - -0.01 - 0 - 0.2 - 1e+13 - 1 - - - - - - 1 - 1 - -0.877342 -1.1194 0 0 -0 0 - - - - - 0 - 0 - 0 - 0 0 0 0 -0 0 - 1 - - 1 - 0 0 0 0 -0 0 - - 1 - 0 - 0 - 1 - 0 - 1 - - - - 0 0 0.0075 0 -0 0 - - - 0.25 0.25 0.25 - model://target_4x4/meshes/target_4x4.dae - - - - - 0 - 10 - -0.235 0 0 0 -0 0 - - - 0.015 0.5 0.335 - - - - - - 1 - 1 - 0 0 0 - 0 - 0 - - - 1 - 0 - 0 - 1 - - 0 - - - - - 0 - 1e+06 - - - 0 - 1 - 1 - - 0 - 0.2 - 1e+13 - 1 - 0.01 - 0 - - - 1 - -0.01 - 0 - 0.2 - 1e+13 - 1 - - - - - - 0 - 10 - 0.0025 0 0 0 -0 0 - - - 0.005 0.5 0.325 - - - - - - 1 - 1 - 0 0 0 - 0 - 0 - - - 1 - 0 - 0 - 1 - - 0 - - - - - 0 - 1e+06 - - - 0 - 1 - 1 - - 0 - 0.2 - 1e+13 - 1 - 0.01 - 0 - - - 1 - -0.01 - 0 - 0.2 - 1e+13 - 1 - - - - - - 0 - 10 - 0.12 0 0 0 -0 0 - - - 0.005 0.5 0.325 - - - - - - 1 - 1 - 0 0 0 - 0 - 0 - - - 1 - 0 - 0 - 1 - - 0 - - - - - 0 - 1e+06 - - - 0 - 1 - 1 - - 0 - 0.2 - 1e+13 - 1 - 0.01 - 0 - - - 1 - -0.01 - 0 - 0.2 - 1e+13 - 1 - - - - - - 0 - 10 - 0.235 0 0 0 -0 0 - - - 0.015 0.5 0.335 - - - - - - 1 - 1 - 0 0 0 - 0 - 0 - - - 1 - 0 - 0 - 1 - - 0 - - - - - 0 - 1e+06 - - - 0 - 1 - 1 - - 0 - 0.2 - 1e+13 - 1 - 0.01 - 0 - - - 1 - -0.01 - 0 - 0.2 - 1e+13 - 1 - - - - - - 0 - 10 - 0 0.235 0 0 -0 0 - - - 0.5 0.015 0.335 - - - - - - 1 - 1 - 0 0 0 - 0 - 0 - - - 1 - 0 - 0 - 1 - - 0 - - - - - 0 - 1e+06 - - - 0 - 1 - 1 - - 0 - 0.2 - 1e+13 - 1 - 0.01 - 0 - - - 1 - -0.01 - 0 - 0.2 - 1e+13 - 1 - - - - - - 0 - 10 - 0 -0.235 0 0 -0 0 - - - 0.5 0.015 0.335 - - - - - - 1 - 1 - 0 0 0 - 0 - 0 - - - 1 - 0 - 0 - 1 - - 0 - - - - - 0 - 1e+06 - - - 0 - 1 - 1 - - 0 - 0.2 - 1e+13 - 1 - 0.01 - 0 - - - 1 - -0.01 - 0 - 0.2 - 1e+13 - 1 - - - - - - 0 - 10 - 0 0 0 0 -0 0 - - - 0.5 0.5 0.02 - - - - - - 1 - 1 - 0 0 0 - 0 - 0 - - - 1 - 0 - 0 - 1 - - 0 - - - - - 0 - 1e+06 - - - 0 - 1 - 1 - - 0 - 0.2 - 1e+13 - 1 - 0.01 - 0 - - - 1 - -0.01 - 0 - 0.2 - 1e+13 - 1 - - - - - - 0 - 10 - -0.115 0 0 0 -0 0 - - - 0.015 0.5 0.325 - - - - - - 1 - 1 - 0 0 0 - 0 - 0 - - - 1 - 0 - 0 - 1 - - 0 - - - - - 0 - 1e+06 - - - 0 - 1 - 1 - - 0 - 0.2 - 1e+13 - 1 - 0.01 - 0 - - - 1 - -0.01 - 0 - 0.2 - 1e+13 - 1 - - - - - - 0 - 10 - 0 0.0025 0 0 -0 0 - - - 0.5 0.005 0.325 - - - - - - 1 - 1 - 0 0 0 - 0 - 0 - - - 1 - 0 - 0 - 1 - - 0 - - - - - 0 - 1e+06 - - - 0 - 1 - 1 - - 0 - 0.2 - 1e+13 - 1 - 0.01 - 0 - - - 1 - -0.01 - 0 - 0.2 - 1e+13 - 1 - - - - - - 0 - 10 - 0 0.12 0 0 -0 0 - - - 0.5 0.005 0.325 - - - - - - 1 - 1 - 0 0 0 - 0 - 0 - - - 1 - 0 - 0 - 1 - - 0 - - - - - 0 - 1e+06 - - - 0 - 1 - 1 - - 0 - 0.2 - 1e+13 - 1 - 0.01 - 0 - - - 1 - -0.01 - 0 - 0.2 - 1e+13 - 1 - - - - - - 0 - 10 - 0 -0.115 0 0 -0 0 - - - 0.5 0.005 0.325 - - - - - - 1 - 1 - 0 0 0 - 0 - 0 - - - 1 - 0 - 0 - 1 - - 0 - - - - - 0 - 1e+06 - - - 0 - 1 - 1 - - 0 - 0.2 - 1e+13 - 1 - 0.01 - 0 - - - 1 - -0.01 - 0 - 0.2 - 1e+13 - 1 - - - - - - 1 - 1 - -1.16917 0.373609 0 0 -0 0 - - - -1.8531 -0.100865 0.5 0 -0 0 - - - 1 - - 0.166667 - 0 - 0 - 0.166667 - 0 - 0.166667 - - 0 0 0 0 -0 0 - - - - - 1 1 0.999999 - - - 10 - - - - - - - - - - - - - - - - - 1 1 0.999999 - - - - - - - 0 - 0 - 0 - 1 - - -1.40372 -3.12264 0.5 0 -0 0 + + + + 0.45 0.25 1.01 0 0 0 - - 1 - - 0.145833 - 0 - 0 - 0.145833 - 0 - 0.125 - - 0 0 0 0 -0 0 - - - - - 0.499999 - 1 - - - 10 - - - - - - - - - - - - - - - 0.499999 - 1 - - - - - - - 0 - 0 - 0 - - 1 - - - - - - 0 0 0 0 -0 0 - 0.01 - - 0 - 0 - 0 - 0 - 0 - 0 - - - - 0 0 0 0 -0 0 - - - 0.03 - + 0.03 - - - - - - - - - - - - - 10 - - - 0 0 0 0 -0 0 - - - 0.03 - - - - - - 0 - 0 - 0 - 0.45 0.25 1.01 0 -0 0 - - - - 0 0 0 0 -0 0 - 0.01 - - 0 - 0 - 0 - 0 - 0 - 0 - - - - 0 0 0 0 -0 0 - - - 0.03 - - - - - - - - - - - - - - - 10 - - - 0 0 0 0 -0 0 + 0.45 -0.25 1.01 0 0 0 + + - - 0.03 - + 0.03 - - - - 0 - 0 - 0 - 0.45 -0.25 1.01 0 -0 0 - - - - 0 0 0 0 -0 0 - 0.01 - - 0 - 0 - 0 - 0 - 0 - 0 - - - - 0 0 0 0 -0 0 - - - 0.03 - - - - - - - - - - - - - - - 10 - - - 0 0 0 0 -0 0 + 0.45 0.09 1.01 0 0 0 + + - - 0.03 - + 0.03 - - - - 0 - 0 - 0 - 0.45 0.09 1.01 0 -0 0 - - - 0 0 0 0 -0 0 - 0.01 - - 0 - 0 - 0 - 0 - 0 - 0 - - - - 0 0 0 0 -0 0 - - - 0.03 - - - - - - - - - - - - - - - 10 - - - 0 0 0 0 -0 0 - - - 0.03 - - - - - - - 0 - 0 - 0 - - 0.45 -0.09 1.01 0 -0 0 - - - - - - - - - - 0 0 0 0 -0 0 - 0.01 - - 0 - 0 - 0 - 0 - 0 - 0 - - - - 0 0 0 0 -0 0 - - - 0.03 - 0.05 - - - - - - - - - - - - - - - 10 - - - 0 0 0 0 -0 0 - - - 0.03 - 0.05 - - - - - - - 0 - 0 - 0 - - 0.65 -0.09 1.01 0 -0 0 - - - - - - 0 0 0 0 -0 0 - 0.01 - - 0 - 0 - 0 - 0 - 0 - 0 - - - - 0 0 0 0 -0 0 - - - 0.02 - 0.06 - - - - - - - - - - - - - - - 10 - - - 0 0 0 0 -0 0 - - - 0.02 - 0.06 - - - - - - - 0 - 0 - 0 - - 0.65 0.25 1.01 0 -0 0 - - - - - - 0 0 0 0 -0 0 - 0.01 - - 0 - 0 - 0 - 0 - 0 - 0 - - - - 0 0 0 0 -0 0 - - - 0.02 - 0.06 - - - - - - - - - - - - - - - 10 - - - 0 0 0 0 -0 0 + 0.45 -0.09 1.01 0 0 0 + + - - 0.02 - 0.06 - + 0.03 - - - - 0 - 0 - 0 - 0.65 0.09 1.01 0 -0 0 - + - - - 0 0 0 0 -0 0 - 0.01 - - 0 - 0 - 0 - 0 - 0 - 0 - - - - 0 0 0 0 -0 0 - - - 0.03 - 0.07 - - - - - - - - - - - - - - - 10 - - - 0 0 0 0 -0 0 + 0.65 -0.25 1.01 0 0 0 + + 0.03 0.07 - - - - 0 - 0 - 0 - 0.65 -0.25 1.01 0 -0 0 - - - - - - + + \ No newline at end of file From 1cb8d15c7d889ee87621f5c3fabeb082b93e65f6 Mon Sep 17 00:00:00 2001 From: jepear19 Date: Tue, 24 Mar 2026 12:49:27 +0100 Subject: [PATCH 08/32] Fix world --- Worlds/machine_vision_harmonic.world | 75 +++++++++++++++++++--------- 1 file changed, 51 insertions(+), 24 deletions(-) diff --git a/Worlds/machine_vision_harmonic.world b/Worlds/machine_vision_harmonic.world index 538af86fe..30abfd663 100644 --- a/Worlds/machine_vision_harmonic.world +++ b/Worlds/machine_vision_harmonic.world @@ -1,7 +1,6 @@ - true 0 0 10 0 0 0 @@ -10,7 +9,6 @@ -0.5 0.1 -0.9 - true @@ -33,14 +31,11 @@ - true - 0.335211 0.377742 0 0 0 0 + 0.6 0 0 0 0 1.57 - 0 0 0 0 0 0 - 0 0 0.95 0 0 0 @@ -59,14 +54,12 @@ - - true - -0.877342 -1.1194 0 0 0 0 + -0.6 0 0 0 0 1.57 @@ -80,52 +73,87 @@ - - - 0.45 0.25 1.01 0 0 0 + + true + -0.5 0 0.76 0 0 0 + 0 0 0.0075 0 0 0 - 0.03 + + 0.25 0.25 0.25 + model://target_4x4/meshes/target_4x4.dae + - - 0.45 -0.25 1.01 0 0 0 + + true + -0.7 -0.435456 0.927284 0 0 0 - 0.03 + + 0.4 0.04449 0.5 + - - 0.45 0.09 1.01 0 0 0 + + true + -0.3 0.38 0.897305 0 0 0 - 0.03 + + 0.170141 + 0.4 + + + 0.45 0.25 1.01 0 0 0 + + + 0.03 + + + + + + 0.45 -0.25 1.01 0 0 0 + + + 0.03 + + + + + + 0.45 0.09 1.01 0 0 0 + + + 0.03 + + + + 0.45 -0.09 1.01 0 0 0 - - 0.03 - + 0.03 - 0.65 -0.25 1.01 0 0 0 @@ -140,7 +168,6 @@ - From 11de41b4011ded7368beb89e136e29b6e6d8c929 Mon Sep 17 00:00:00 2001 From: jepear19 Date: Tue, 24 Mar 2026 13:14:41 +0100 Subject: [PATCH 09/32] Fix world --- Worlds/machine_vision_harmonic.world | 173 ++++++++++++++++----------- 1 file changed, 106 insertions(+), 67 deletions(-) diff --git a/Worlds/machine_vision_harmonic.world b/Worlds/machine_vision_harmonic.world index 30abfd663..6c6adebb6 100644 --- a/Worlds/machine_vision_harmonic.world +++ b/Worlds/machine_vision_harmonic.world @@ -1,68 +1,82 @@ - - - - - true + + + + + + + + + ogre2 + + + + + + 0.4 0.4 0.4 1 + 0.7 0.7 0.7 1 + true + false + + + 0 0 10 0 0 0 0.8 0.8 0.8 1 - 0.2 0.2 0.2 1 -0.5 0.1 -0.9 - + true - - + + - - 0 0 1 - 100 100 - + 0 0 1100 100 - + - - 0 0 1 - 100 100 - + 0 0 1100 100 - + true - 0.6 0 0 0 0 1.57 - - - - 0 0 0.95 0 0 0 + -0.5 0 0 0 0 1.57 + + + 0 0 0.001 0 0 0 - 0.45 0.35 0.5 - model://conveyor/meshes/conveyor.dae + model://ground_logo_big/meshes/ground_logo.dae + + - + + true + 0.6 0 0 0 0 1.57 + + 0 0 0.95 0 0 0 - - 2.4 0.8 0.1 - + + 0.45 0.35 0.5 + model://conveyor/meshes/conveyor.dae + - + - + true -0.6 0 0 0 0 1.57 - - - + + 0 0 0.73 0 0 1.57 @@ -73,11 +87,11 @@ - + true -0.5 0 0.76 0 0 0 - - + + 0 0 0.0075 0 0 0 @@ -89,11 +103,10 @@ - - true + -0.7 -0.435456 0.927284 0 0 0 - - + + 0.4 0.04449 0.5 @@ -103,11 +116,10 @@ - - true + -0.3 0.38 0.897305 0 0 0 - - + + 0.170141 @@ -118,58 +130,85 @@ - + 0.45 0.25 1.01 0 0 0 - - + + 0.03 - + 0.45 -0.25 1.01 0 0 0 - - + + 0.03 - + 0.45 0.09 1.01 0 0 0 - - + + 0.03 - + 0.45 -0.09 1.01 0 0 0 - - + + 0.03 - + 0.65 -0.25 1.01 0 0 0 - - + + - - 0.03 - 0.07 - + 0.030.07 + + + + + + + 0.65 -0.09 1.01 0 0 0 + + + + 0.030.05 + + + + + + + 0.65 0.09 1.01 0 0 0 + + + + 0.020.06 - + + 0.65 0.25 1.01 0 0 0 + + + + 0.020.06 + + + + \ No newline at end of file From 984dce1db1f168b614183be2356446ca8924a4b5 Mon Sep 17 00:00:00 2001 From: jepear19 Date: Tue, 24 Mar 2026 13:41:53 +0100 Subject: [PATCH 10/32] Fix world --- Worlds/machine_vision_harmonic.world | 71 ++++++++++++++++++---------- 1 file changed, 47 insertions(+), 24 deletions(-) diff --git a/Worlds/machine_vision_harmonic.world b/Worlds/machine_vision_harmonic.world index 6c6adebb6..b4b697af7 100644 --- a/Worlds/machine_vision_harmonic.world +++ b/Worlds/machine_vision_harmonic.world @@ -2,6 +2,7 @@ + @@ -9,22 +10,26 @@ ogre2 + + - 0.4 0.4 0.4 1 + 0.6 0.6 0.6 1 0.7 0.7 0.7 1 true false + 0 0 10 0 0 0 0.8 0.8 0.8 1 -0.5 0.1 -0.9 + true @@ -37,10 +42,15 @@ 0 0 1100 100 + + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + + true -0.5 0 0 0 0 1.57 @@ -56,6 +66,7 @@ + true 0.6 0 0 0 0 1.57 @@ -72,6 +83,7 @@ + true -0.6 0 0 0 0 1.57 @@ -87,6 +99,7 @@ + true -0.5 0 0.76 0 0 0 @@ -103,19 +116,23 @@ + -0.7 -0.435456 0.927284 0 0 0 - - 0.4 0.04449 0.5 - + 0.4 0.04449 0.5 + + 0.7 0.7 0.7 1 + 0.7 0.7 0.7 1 + + -0.3 0.38 0.897305 0 0 0 @@ -126,15 +143,21 @@ 0.4 + + 0.6 0.6 0.6 1 + 0.7 0.7 0.7 1 + + 0.45 0.25 1.01 0 0 0 - + 0.03 + 0 0 1 10 0 1 1 @@ -142,8 +165,9 @@ 0.45 -0.25 1.01 0 0 0 - + 0.03 + 1 0 0 11 0 0 1 @@ -151,8 +175,9 @@ 0.45 0.09 1.01 0 0 0 - + 0.03 + 0 1 0 10 1 0 1 @@ -160,19 +185,20 @@ 0.45 -0.09 1.01 0 0 0 - + 0.03 + 0.5 0 0.5 10.5 0 0.5 1 + 0.65 -0.25 1.01 0 0 0 - - - 0.030.07 - + + 0.030.07 + 0 0 1 10 0 1 1 @@ -180,10 +206,9 @@ 0.65 -0.09 1.01 0 0 0 - - - 0.030.05 - + + 0.030.05 + 0 1 0 10 1 0 1 @@ -191,10 +216,9 @@ 0.65 0.09 1.01 0 0 0 - - - 0.020.06 - + + 0.020.06 + 1 0 0 11 0 0 1 @@ -202,10 +226,9 @@ 0.65 0.25 1.01 0 0 0 - - - 0.020.06 - + + 0.020.06 + 0.5 0 0.5 10.5 0 0.5 1 From 05204932a2200f557deb998ccc07efb444145871 Mon Sep 17 00:00:00 2001 From: jepear19 Date: Tue, 24 Mar 2026 17:50:03 +0100 Subject: [PATCH 11/32] Fix world --- Worlds/machine_vision_harmonic.world | 1553 +++++++++++++++++++++++--- 1 file changed, 1412 insertions(+), 141 deletions(-) diff --git a/Worlds/machine_vision_harmonic.world b/Worlds/machine_vision_harmonic.world index b4b697af7..7489b8858 100644 --- a/Worlds/machine_vision_harmonic.world +++ b/Worlds/machine_vision_harmonic.world @@ -1,111 +1,310 @@ - - - - - - - - - - ogre2 - - - - - - - - 0.6 0.6 0.6 1 - 0.7 0.7 0.7 1 - true - false - - - - - 0 0 10 0 0 0 + + + + 1 + 0 0 10 0 -0 0 0.8 0.8 0.8 1 + 0.2 0.2 0.2 1 + + 1000 + 0.9 + 0.01 + 0.001 + -0.5 0.1 -0.9 - - - - true - - + + 1 + + - 0 0 1100 100 + + 0 0 1 + 100 100 + + + + 65535 + + + + + 100 + 50 + + + + + + + + 10 - + + 0 - 0 0 1100 100 + + 0 0 1 + 100 100 + - 0.8 0.8 0.8 1 - 0.8 0.8 0.8 1 + + 0 + 0 + 0 - - - - true - -0.5 0 0 0 0 1.57 - - - 0 0 0.001 0 0 0 + 0 0 -9.8 + 6e-06 2.3e-05 -4.2e-05 + + + 0.001 + 1 + 1000 + + + 0.4 0.4 0.4 1 + 0.7 0.7 0.7 1 + 1 + + + + EARTH_WGS84 + 0 + 0 + 0 + 0 + + + + 0 + 0 + 0 + 0 0 0 0 -0 0 + 1 + + 1 + 0 0 0 0 -0 0 + + 1 + 0 + 0 + 1 + 0 + 1 + + + + 0 0 0.95 0 -0 0 - model://ground_logo_big/meshes/ground_logo.dae + 0.45 0.35 0.5 + model://conveyor/meshes/conveyor.dae + + 0 + 10 + 0 0 0.95 0 -0 0 + + + 2.4 0.8 0.1 + + + + + + 1 + 1 + 0 0 0 + 0 + 0 + + + 1 + 0 + 0 + 1 + + 0 + + + + + 0 + 1e+06 + + + 0 + 1 + 1 + + 0 + 0.2 + 1e+13 + 1 + 0.01 + 0 + + + 1 + -0.01 + 0 + 0.2 + 1e+13 + 1 + + + + - - - - - true + 1 + 1 0.6 0 0 0 0 1.57 - - - 0 0 0.95 0 0 0 + + + 1 + + + 0 0 0.001 0 -0 0 - 0.45 0.35 0.5 - model://conveyor/meshes/conveyor.dae + 1 1 1 + model://ground_logo_big/meshes/ground_logo.dae + 0 + 0 + 0 + -0.5 0 0 0 0 1.57 - - - - true - -0.6 0 0 0 0 1.57 - - - 0 0 0.73 0 0 1.57 + + + 4.32698 1.43011 2.70567 0 0.293795 -2.85637 + orbit + perspective + + + + + 0 + 0 + 0 + 0 0 0 0 -0 0 + 1 + + 1 + 0 0 0 0 -0 0 + + 1 + 0 + 0 + 1 + 0 + 1 + + + + 0 0 0.73 0 -0 1.57 + 1 1 1 model://target_table/meshes/target_table.dae + + 0 + 10 + 0 0 0.73 0 -0 0 + + + 1.6 0.8 0.06 + + + + + + 1 + 1 + 0 0 0 + 0 + 0 + + + 1 + 0 + 0 + 1 + + 0 + + + + + 0 + 1e+06 + + + 0 + 1 + 1 + + 0 + 0.2 + 1e+13 + 1 + 0.01 + 0 + + + 1 + -0.01 + 0 + 0.2 + 1e+13 + 1 + + + + + 1 + 1 + -0.6 0 0 0 0 1.57 - - - - true - -0.5 0 0.76 0 0 0 - - - 0 0 0.0075 0 0 0 + + + + 0 + 0 + 0 + 0 0 0 0 -0 0 + 1 + + 1 + 0 0 0 0 -0 0 + + 1 + 0 + 0 + 1 + 0 + 1 + + + + 0 0 0.0075 0 -0 0 0.25 0.25 0.25 @@ -113,125 +312,1197 @@ + + 0 + 10 + -0.235 0 0 0 -0 0 + + + 0.015 0.5 0.335 + + + + + + 1 + 1 + 0 0 0 + 0 + 0 + + + 1 + 0 + 0 + 1 + + 0 + + + + + 0 + 1e+06 + + + 0 + 1 + 1 + + 0 + 0.2 + 1e+13 + 1 + 0.01 + 0 + + + 1 + -0.01 + 0 + 0.2 + 1e+13 + 1 + + + + + + 0 + 10 + 0.0025 0 0 0 -0 0 + + + 0.005 0.5 0.325 + + + + + + 1 + 1 + 0 0 0 + 0 + 0 + + + 1 + 0 + 0 + 1 + + 0 + + + + + 0 + 1e+06 + + + 0 + 1 + 1 + + 0 + 0.2 + 1e+13 + 1 + 0.01 + 0 + + + 1 + -0.01 + 0 + 0.2 + 1e+13 + 1 + + + + + + 0 + 10 + 0.12 0 0 0 -0 0 + + + 0.005 0.5 0.325 + + + + + + 1 + 1 + 0 0 0 + 0 + 0 + + + 1 + 0 + 0 + 1 + + 0 + + + + + 0 + 1e+06 + + + 0 + 1 + 1 + + 0 + 0.2 + 1e+13 + 1 + 0.01 + 0 + + + 1 + -0.01 + 0 + 0.2 + 1e+13 + 1 + + + + + + 0 + 10 + 0.235 0 0 0 -0 0 + + + 0.015 0.5 0.335 + + + + + + 1 + 1 + 0 0 0 + 0 + 0 + + + 1 + 0 + 0 + 1 + + 0 + + + + + 0 + 1e+06 + + + 0 + 1 + 1 + + 0 + 0.2 + 1e+13 + 1 + 0.01 + 0 + + + 1 + -0.01 + 0 + 0.2 + 1e+13 + 1 + + + + + + 0 + 10 + 0 0.235 0 0 -0 0 + + + 0.5 0.015 0.335 + + + + + + 1 + 1 + 0 0 0 + 0 + 0 + + + 1 + 0 + 0 + 1 + + 0 + + + + + 0 + 1e+06 + + + 0 + 1 + 1 + + 0 + 0.2 + 1e+13 + 1 + 0.01 + 0 + + + 1 + -0.01 + 0 + 0.2 + 1e+13 + 1 + + + + + + 0 + 10 + 0 -0.235 0 0 -0 0 + + + 0.5 0.015 0.335 + + + + + + 1 + 1 + 0 0 0 + 0 + 0 + + + 1 + 0 + 0 + 1 + + 0 + + + + + 0 + 1e+06 + + + 0 + 1 + 1 + + 0 + 0.2 + 1e+13 + 1 + 0.01 + 0 + + + 1 + -0.01 + 0 + 0.2 + 1e+13 + 1 + + + + + + 0 + 10 + 0 0 0 0 -0 0 + + + 0.5 0.5 0.02 + + + + + + 1 + 1 + 0 0 0 + 0 + 0 + + + 1 + 0 + 0 + 1 + + 0 + + + + + 0 + 1e+06 + + + 0 + 1 + 1 + + 0 + 0.2 + 1e+13 + 1 + 0.01 + 0 + + + 1 + -0.01 + 0 + 0.2 + 1e+13 + 1 + + + + + + 0 + 10 + -0.115 0 0 0 -0 0 + + + 0.015 0.5 0.325 + + + + + + 1 + 1 + 0 0 0 + 0 + 0 + + + 1 + 0 + 0 + 1 + + 0 + + + + + 0 + 1e+06 + + + 0 + 1 + 1 + + 0 + 0.2 + 1e+13 + 1 + 0.01 + 0 + + + 1 + -0.01 + 0 + 0.2 + 1e+13 + 1 + + + + + + 0 + 10 + 0 0.0025 0 0 -0 0 + + + 0.5 0.005 0.325 + + + + + + 1 + 1 + 0 0 0 + 0 + 0 + + + 1 + 0 + 0 + 1 + + 0 + + + + + 0 + 1e+06 + + + 0 + 1 + 1 + + 0 + 0.2 + 1e+13 + 1 + 0.01 + 0 + + + 1 + -0.01 + 0 + 0.2 + 1e+13 + 1 + + + + + + 0 + 10 + 0 0.12 0 0 -0 0 + + + 0.5 0.005 0.325 + + + + + + 1 + 1 + 0 0 0 + 0 + 0 + + + 1 + 0 + 0 + 1 + + 0 + + + + + 0 + 1e+06 + + + 0 + 1 + 1 + + 0 + 0.2 + 1e+13 + 1 + 0.01 + 0 + + + 1 + -0.01 + 0 + 0.2 + 1e+13 + 1 + + + + + + 0 + 10 + 0 -0.115 0 0 -0 0 + + + 0.5 0.005 0.325 + + + + + + 1 + 1 + 0 0 0 + 0 + 0 + + + 1 + 0 + 0 + 1 + + 0 + + + + + 0 + 1e+06 + + + 0 + 1 + 1 + + 0 + 0.2 + 1e+13 + 1 + 0.01 + 0 + + + 1 + -0.01 + 0 + 0.2 + 1e+13 + 1 + + + + + 1 + 1 + -0.5 0 0.76 0 0 0 - - - + -0.7 -0.435456 0.927284 0 0 0 - - + + + 1 + + 0.166667 + 0 + 0 + 0.166667 + 0 + 0.166667 + + 0 0 0 0 -0 0 + + + + + 1 1 0.999999 + + + 10 + + + + + + + + + + + + + + - 0.4 0.04449 0.5 + + 1 1 0.999999 + - 0.7 0.7 0.7 1 - 0.7 0.7 0.7 1 + + 0 + 0 + 0 + 1 - - - + -0.3 0.38 0.897305 0 0 0 - - + + + 1 + + 0.145833 + 0 + 0 + 0.145833 + 0 + 0.125 + + 0 0 0 0 -0 0 + + + + + 0.499999 + 1 + + + 10 + + + + + + + + + + + + + + - 0.170141 - 0.4 + 0.499999 + 1 - 0.6 0.6 0.6 1 - 0.7 0.7 0.7 1 + + 0 + 0 + 0 + 1 - - - - 0.45 0.25 1.01 0 0 0 - - - 0.03 - 0 0 1 10 0 1 1 + + + + + 0 0 0 0 -0 0 + 0.01 + + 0 + 0 + 0 + 0 + 0 + 0 + + + + 0 0 0 0 -0 0 + + + 0.03 + + + + + + + + + + + + + + + 10 + + + 0 0 0 0 -0 0 + + + 0.03 + + + + + + 0 + 0 + 0 + 0.45 0.25 1.01 0 -0 0 - - 0.45 -0.25 1.01 0 0 0 - - - 0.03 - 1 0 0 11 0 0 1 + + + + + 0 0 0 0 -0 0 + 0.01 + + 0 + 0 + 0 + 0 + 0 + 0 + + + + 0 0 0 0 -0 0 + + + 0.03 + + + + + + + + + + + + + + + 10 + + + 0 0 0 0 -0 0 + + + 0.03 + + + + + + 0 + 0 + 0 + 0.45 -0.25 1.01 0 -0 0 - - 0.45 0.09 1.01 0 0 0 - - - 0.03 - 0 1 0 10 1 0 1 + + + + + 0 0 0 0 -0 0 + 0.01 + + 0 + 0 + 0 + 0 + 0 + 0 + + + + 0 0 0 0 -0 0 + + + 0.03 + + + + + + + + + + + + + + + 10 + + + 0 0 0 0 -0 0 + + + 0.03 + + + + + + 0 + 0 + 0 + 0.45 0.09 1.01 0 -0 0 - - 0.45 -0.09 1.01 0 0 0 - - - 0.03 - 0.5 0 0.5 10.5 0 0.5 1 + + + + 0 0 0 0 -0 0 + 0.01 + + 0 + 0 + 0 + 0 + 0 + 0 + + + + 0 0 0 0 -0 0 + + + 0.03 + + + + + + + + + + + + + + + 10 + + + 0 0 0 0 -0 0 + + + 0.03 + + + + + + 0 + 0 + 0 + 0.45 -0.09 1.01 0 -0 0 - - - 0.65 -0.25 1.01 0 0 0 - - - 0.030.07 - 0 0 1 10 0 1 1 + + + + + + + + 0 0 0 0 -0 0 + 0.01 + + 0 + 0 + 0 + 0 + 0 + 0 + + + + 0 0 0 0 -0 0 + + + 0.03 + 0.05 + + + + + + + + + + + + + + + 10 + + + 0 0 0 0 -0 0 + + + 0.03 + 0.05 + + + + + + 0 + 0 + 0 + 0.65 -0.09 1.01 0 -0 0 - - 0.65 -0.09 1.01 0 0 0 - - - 0.030.05 - 0 1 0 10 1 0 1 + + + + 0 0 0 0 -0 0 + 0.01 + + 0 + 0 + 0 + 0 + 0 + 0 + + + + 0 0 0 0 -0 0 + + + 0.02 + 0.06 + + + + + + + + + + + + + + + 10 + + + 0 0 0 0 -0 0 + + + 0.02 + 0.06 + + + + + + 0 + 0 + 0 + 0.65 0.25 1.01 0 -0 0 - - 0.65 0.09 1.01 0 0 0 - - - 0.020.06 - 1 0 0 11 0 0 1 + + + + 0 0 0 0 -0 0 + 0.01 + + 0 + 0 + 0 + 0 + 0 + 0 + + + + 0 0 0 0 -0 0 + + + 0.02 + 0.06 + + + + + + + + + + + + + + + 10 + + + 0 0 0 0 -0 0 + + + 0.02 + 0.06 + + + + + + 0 + 0 + 0 + 0.65 0.09 1.01 0 -0 0 - - 0.65 0.25 1.01 0 0 0 - - - 0.020.06 - 0.5 0 0.5 10.5 0 0.5 1 + + + + + 0 0 0 0 -0 0 + 0.01 + + 0 + 0 + 0 + 0 + 0 + 0 + + + + 0 0 0 0 -0 0 + + + 0.03 + 0.07 + + + + + + + + + + + + + + + 10 + + + 0 0 0 0 -0 0 + + + 0.03 + 0.07 + + + + + + 0 + 0 + 0 + 0.65 -0.25 1.01 0 -0 0 + + + + + + + \ No newline at end of file From a7a1dadcf45caeab6d2f9b891fccdeb7f7cbee6b Mon Sep 17 00:00:00 2001 From: jepear19 Date: Tue, 24 Mar 2026 18:10:15 +0100 Subject: [PATCH 12/32] Fix arm and objects in .world --- Worlds/machine_vision_harmonic.world | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/Worlds/machine_vision_harmonic.world b/Worlds/machine_vision_harmonic.world index 7489b8858..4c3102c17 100644 --- a/Worlds/machine_vision_harmonic.world +++ b/Worlds/machine_vision_harmonic.world @@ -168,7 +168,7 @@ 1 1 - 0.6 0 0 0 0 1.57 + 0.6 0 0 1.57 0 0 1 @@ -648,7 +648,7 @@ 0 0 0 0 -0 0 - 0.5 0.5 0.02 + 0.25 0.25 0.02 @@ -994,7 +994,7 @@ - 0.499999 + 0.17 1 From 1127e43b4741966925bc8be3d377d5548be0a07f Mon Sep 17 00:00:00 2001 From: jepear19 Date: Tue, 24 Mar 2026 18:32:08 +0100 Subject: [PATCH 13/32] Fix arm and objects in .world --- Worlds/machine_vision_harmonic.world | 14 ++++++++++---- 1 file changed, 10 insertions(+), 4 deletions(-) diff --git a/Worlds/machine_vision_harmonic.world b/Worlds/machine_vision_harmonic.world index 4c3102c17..f7bbe312b 100644 --- a/Worlds/machine_vision_harmonic.world +++ b/Worlds/machine_vision_harmonic.world @@ -168,7 +168,7 @@ 1 1 - 0.6 0 0 1.57 0 0 + 0.6 0 0 0 0 1.57 1 @@ -648,7 +648,7 @@ 0 0 0 0 -0 0 - 0.25 0.25 0.02 + 0.5 0.5 0.02 @@ -940,7 +940,7 @@ - 1 1 0.999999 + <0.4 0.04 0.5 10 @@ -1015,7 +1015,7 @@ - 0.499999 + 0.17 1 @@ -1497,6 +1497,12 @@ + + ogre2 + + + From d8dac8db48bb9ffcd8e02c10745bddd798bc2656 Mon Sep 17 00:00:00 2001 From: jepear19 Date: Tue, 24 Mar 2026 18:46:47 +0100 Subject: [PATCH 14/32] Fix .world --- Worlds/machine_vision_harmonic.world | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/Worlds/machine_vision_harmonic.world b/Worlds/machine_vision_harmonic.world index f7bbe312b..0c3e07ede 100644 --- a/Worlds/machine_vision_harmonic.world +++ b/Worlds/machine_vision_harmonic.world @@ -940,7 +940,7 @@ - <0.4 0.04 0.5 + 0.4 0.04 0.5 10 @@ -960,7 +960,7 @@ - 1 1 0.999999 + 0.4 0.04 0.5 @@ -1497,7 +1497,7 @@ - ogre2 From c5a558a3546b6160fe4c7a330d7e2084276d2995 Mon Sep 17 00:00:00 2001 From: jepear19 Date: Tue, 24 Mar 2026 19:13:31 +0100 Subject: [PATCH 15/32] Fix cylinder and arm --- Launchers/machine_vision_harmonic/robot.launch.py | 13 +++++++------ Launchers/machine_vision_harmonic/world.launch.py | 10 ++++++---- Worlds/machine_vision_harmonic.world | 8 ++++---- 3 files changed, 17 insertions(+), 14 deletions(-) diff --git a/Launchers/machine_vision_harmonic/robot.launch.py b/Launchers/machine_vision_harmonic/robot.launch.py index 2afeb0a42..4ed79df4a 100644 --- a/Launchers/machine_vision_harmonic/robot.launch.py +++ b/Launchers/machine_vision_harmonic/robot.launch.py @@ -15,7 +15,6 @@ def generate_launch_description(): pkg_path = get_package_share_directory(PACKAGE_NAME + "_gazebo") - # URDF (como en classic) xacro_file = os.path.join(pkg_path, "urdf", "ur5.urdf.xacro") doc = xacro.parse(open(xacro_file)) @@ -23,7 +22,6 @@ def generate_launch_description(): robot_description = {"robot_description": doc.toxml()} - # Robot State Publisher robot_state_publisher = Node( package="robot_state_publisher", executable="robot_state_publisher", @@ -31,25 +29,28 @@ def generate_launch_description(): parameters=[robot_description, {"use_sim_time": True}], ) - # Static TF static_tf = Node( package="tf2_ros", executable="static_transform_publisher", arguments=["0", "0", "0", "0", "0", "0", "world", "base_link"], ) - # Spawn robot en GZ (adaptado a harmonic) spawn_entity = Node( package="ros_gz_sim", executable="create", arguments=[ "-name", "ur5", - "-topic", "robot_description" + "-topic", "robot_description", + "-x", "0", + "-y", "0", + "-z", "0", + "-R", "0", + "-P", "1.57", + "-Y", "0", ], output="screen", ) - # Controllers (mínimos) joint_state_broadcaster = Node( package="controller_manager", executable="spawner", diff --git a/Launchers/machine_vision_harmonic/world.launch.py b/Launchers/machine_vision_harmonic/world.launch.py index fe3ef9462..257d1ad6b 100644 --- a/Launchers/machine_vision_harmonic/world.launch.py +++ b/Launchers/machine_vision_harmonic/world.launch.py @@ -18,10 +18,12 @@ def generate_launch_description(): ld = LaunchDescription() - ld.add_action( - SetEnvironmentVariable( - "GZ_SIM_RESOURCE_PATH", - os.environ.get("GZ_SIM_RESOURCE_PATH", "") + SetEnvironmentVariable( + "GZ_SIM_RESOURCE_PATH", + os.path.join( + os.environ.get("GZ_SIM_RESOURCE_PATH", ""), + "/opt/ros/humble/share", + "/usr/share/gz", ) ) diff --git a/Worlds/machine_vision_harmonic.world b/Worlds/machine_vision_harmonic.world index 0c3e07ede..b0acf5d2c 100644 --- a/Worlds/machine_vision_harmonic.world +++ b/Worlds/machine_vision_harmonic.world @@ -994,8 +994,8 @@ - 0.17 - 1 + 0.08 + 0.4 10 @@ -1015,8 +1015,8 @@ - 0.17 - 1 + 0.08 + 0.04 From e6305ef095d317ed58dd5811bb81759224de70ed Mon Sep 17 00:00:00 2001 From: jepear19 Date: Tue, 24 Mar 2026 19:26:14 +0100 Subject: [PATCH 16/32] Fix arm orientation --- Launchers/machine_vision_harmonic/robot.launch.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/Launchers/machine_vision_harmonic/robot.launch.py b/Launchers/machine_vision_harmonic/robot.launch.py index 4ed79df4a..fd675c6b2 100644 --- a/Launchers/machine_vision_harmonic/robot.launch.py +++ b/Launchers/machine_vision_harmonic/robot.launch.py @@ -44,8 +44,8 @@ def generate_launch_description(): "-x", "0", "-y", "0", "-z", "0", - "-R", "0", - "-P", "1.57", + "-R", "1.57", + "-P", "0", "-Y", "0", ], output="screen", From e510a1d3a55bae712783fb2290710762f99f4551 Mon Sep 17 00:00:00 2001 From: jepear19 Date: Tue, 24 Mar 2026 19:43:02 +0100 Subject: [PATCH 17/32] Fix launchers --- .../machine_vision_harmonic.launch.py | 15 ++++++--- .../machine_vision_harmonic/robot.launch.py | 17 +++++++--- .../machine_vision_harmonic/world.launch.py | 31 +++++++++---------- 3 files changed, 38 insertions(+), 25 deletions(-) diff --git a/Launchers/machine_vision_harmonic/machine_vision_harmonic.launch.py b/Launchers/machine_vision_harmonic/machine_vision_harmonic.launch.py index d766d2864..b9a95e7ae 100644 --- a/Launchers/machine_vision_harmonic/machine_vision_harmonic.launch.py +++ b/Launchers/machine_vision_harmonic/machine_vision_harmonic.launch.py @@ -1,5 +1,5 @@ """ -Machine Vision Harmonic - Main Launcher +Machine Vision Harmonic - Main Launcher (OK) """ import os @@ -13,11 +13,18 @@ def generate_launch_description(): base_dir = os.path.dirname(__file__) world_launch = IncludeLaunchDescription( - PythonLaunchDescriptionSource(os.path.join(base_dir, "world.launch.py")) + PythonLaunchDescriptionSource( + os.path.join(base_dir, "world.launch.py") + ) ) robot_launch = IncludeLaunchDescription( - PythonLaunchDescriptionSource(os.path.join(base_dir, "robot.launch.py")) + PythonLaunchDescriptionSource( + os.path.join(base_dir, "robot.launch.py") + ) ) - return LaunchDescription([world_launch, robot_launch]) \ No newline at end of file + return LaunchDescription([ + world_launch, + robot_launch + ]) \ No newline at end of file diff --git a/Launchers/machine_vision_harmonic/robot.launch.py b/Launchers/machine_vision_harmonic/robot.launch.py index fd675c6b2..c2c5b2391 100644 --- a/Launchers/machine_vision_harmonic/robot.launch.py +++ b/Launchers/machine_vision_harmonic/robot.launch.py @@ -1,18 +1,19 @@ """ -Machine Vision Harmonic - Robot Launcher +Machine Vision Harmonic - Robot Launcher (FIXED) """ import os import xacro from launch import LaunchDescription from launch_ros.actions import Node +from launch.actions import RegisterEventHandler +from launch.event_handlers import OnProcessExit from ament_index_python.packages import get_package_share_directory def generate_launch_description(): PACKAGE_NAME = "ros2srrc_ur5" - pkg_path = get_package_share_directory(PACKAGE_NAME + "_gazebo") xacro_file = os.path.join(pkg_path, "urdf", "ur5.urdf.xacro") @@ -44,7 +45,7 @@ def generate_launch_description(): "-x", "0", "-y", "0", "-z", "0", - "-R", "1.57", + "-R", "0", "-P", "0", "-Y", "0", ], @@ -63,10 +64,16 @@ def generate_launch_description(): arguments=["joint_trajectory_controller"], ) + delay_controllers = RegisterEventHandler( + OnProcessExit( + target_action=spawn_entity, + on_exit=[joint_state_broadcaster, joint_trajectory_controller], + ) + ) + return LaunchDescription([ robot_state_publisher, static_tf, spawn_entity, - joint_state_broadcaster, - joint_trajectory_controller, + delay_controllers, ]) \ No newline at end of file diff --git a/Launchers/machine_vision_harmonic/world.launch.py b/Launchers/machine_vision_harmonic/world.launch.py index 257d1ad6b..fc7ac44a6 100644 --- a/Launchers/machine_vision_harmonic/world.launch.py +++ b/Launchers/machine_vision_harmonic/world.launch.py @@ -1,32 +1,31 @@ """ -Machine Vision Harmonic - World Launcher +Machine Vision Harmonic - World Launcher (FIXED) """ import os from launch import LaunchDescription -from launch.actions import ExecuteProcess, SetEnvironmentVariable +from launch.actions import ExecuteProcess +from ament_index_python.packages import get_package_share_directory def generate_launch_description(): world_path = "/opt/jderobot/Worlds/machine_vision_harmonic.world" + ur5_pkg = get_package_share_directory("ur5_gripper_description") + robotiq_pkg = get_package_share_directory("robotiq_description") + + resource_path = ( + os.path.dirname(ur5_pkg) + ":" + + os.path.dirname(robotiq_pkg) + ) + gazebo = ExecuteProcess( cmd=["gz", "sim", "-r", "-v", "4", world_path], output="screen", + additional_env={ + "GZ_SIM_RESOURCE_PATH": resource_path + } ) - ld = LaunchDescription() - - SetEnvironmentVariable( - "GZ_SIM_RESOURCE_PATH", - os.path.join( - os.environ.get("GZ_SIM_RESOURCE_PATH", ""), - "/opt/ros/humble/share", - "/usr/share/gz", - ) - ) - - ld.add_action(gazebo) - - return ld \ No newline at end of file + return LaunchDescription([gazebo]) \ No newline at end of file From d8f94b2db23c73b7618ce563831e2d1f7a72ec82 Mon Sep 17 00:00:00 2001 From: jepear19 Date: Tue, 24 Mar 2026 20:04:22 +0100 Subject: [PATCH 18/32] Fix launchers --- .../machine_vision_harmonic.launch.py | 15 +--- .../machine_vision_harmonic/robot.launch.py | 77 ++++--------------- .../machine_vision_harmonic/world.launch.py | 71 ++++++++++++++--- 3 files changed, 77 insertions(+), 86 deletions(-) diff --git a/Launchers/machine_vision_harmonic/machine_vision_harmonic.launch.py b/Launchers/machine_vision_harmonic/machine_vision_harmonic.launch.py index b9a95e7ae..09c1e7f27 100644 --- a/Launchers/machine_vision_harmonic/machine_vision_harmonic.launch.py +++ b/Launchers/machine_vision_harmonic/machine_vision_harmonic.launch.py @@ -1,5 +1,5 @@ """ -Machine Vision Harmonic - Main Launcher (OK) +Pick Place Harmonic - Main Launcher """ import os @@ -13,18 +13,11 @@ def generate_launch_description(): base_dir = os.path.dirname(__file__) world_launch = IncludeLaunchDescription( - PythonLaunchDescriptionSource( - os.path.join(base_dir, "world.launch.py") - ) + PythonLaunchDescriptionSource(os.path.join(base_dir, "world.launch.py")) ) robot_launch = IncludeLaunchDescription( - PythonLaunchDescriptionSource( - os.path.join(base_dir, "robot.launch.py") - ) + PythonLaunchDescriptionSource(os.path.join(base_dir, "robot.launch.py")) ) - return LaunchDescription([ - world_launch, - robot_launch - ]) \ No newline at end of file + return LaunchDescription([world_launch, robot_launch]) diff --git a/Launchers/machine_vision_harmonic/robot.launch.py b/Launchers/machine_vision_harmonic/robot.launch.py index c2c5b2391..17d88dfff 100644 --- a/Launchers/machine_vision_harmonic/robot.launch.py +++ b/Launchers/machine_vision_harmonic/robot.launch.py @@ -1,79 +1,28 @@ """ -Machine Vision Harmonic - Robot Launcher (FIXED) +Machine Vision Harmonic - Robot Launcher +(USA EL MISMO QUE PICK&PLACE) """ import os -import xacro from launch import LaunchDescription -from launch_ros.actions import Node -from launch.actions import RegisterEventHandler -from launch.event_handlers import OnProcessExit +from launch.actions import IncludeLaunchDescription +from launch.launch_description_sources import PythonLaunchDescriptionSource from ament_index_python.packages import get_package_share_directory def generate_launch_description(): - PACKAGE_NAME = "ros2srrc_ur5" - pkg_path = get_package_share_directory(PACKAGE_NAME + "_gazebo") + ur5_gripper_pkg = get_package_share_directory("ur5_gripper_description") - xacro_file = os.path.join(pkg_path, "urdf", "ur5.urdf.xacro") - - doc = xacro.parse(open(xacro_file)) - xacro.process_doc(doc) - - robot_description = {"robot_description": doc.toxml()} - - robot_state_publisher = Node( - package="robot_state_publisher", - executable="robot_state_publisher", - output="screen", - parameters=[robot_description, {"use_sim_time": True}], - ) - - static_tf = Node( - package="tf2_ros", - executable="static_transform_publisher", - arguments=["0", "0", "0", "0", "0", "0", "world", "base_link"], - ) - - spawn_entity = Node( - package="ros_gz_sim", - executable="create", - arguments=[ - "-name", "ur5", - "-topic", "robot_description", - "-x", "0", - "-y", "0", - "-z", "0", - "-R", "0", - "-P", "0", - "-Y", "0", - ], - output="screen", - ) - - joint_state_broadcaster = Node( - package="controller_manager", - executable="spawner", - arguments=["joint_state_broadcaster"], - ) - - joint_trajectory_controller = Node( - package="controller_manager", - executable="spawner", - arguments=["joint_trajectory_controller"], + warehouse_launch_file = os.path.join( + ur5_gripper_pkg, "launch", "spawn_robot_warehouse.launch.py" ) - delay_controllers = RegisterEventHandler( - OnProcessExit( - target_action=spawn_entity, - on_exit=[joint_state_broadcaster, joint_trajectory_controller], - ) + warehouse_launch = IncludeLaunchDescription( + PythonLaunchDescriptionSource(warehouse_launch_file), + launch_arguments={ + "launch_rviz": "false", + }.items(), ) - return LaunchDescription([ - robot_state_publisher, - static_tf, - spawn_entity, - delay_controllers, - ]) \ No newline at end of file + return LaunchDescription([warehouse_launch]) \ No newline at end of file diff --git a/Launchers/machine_vision_harmonic/world.launch.py b/Launchers/machine_vision_harmonic/world.launch.py index fc7ac44a6..ddfa4bbdb 100644 --- a/Launchers/machine_vision_harmonic/world.launch.py +++ b/Launchers/machine_vision_harmonic/world.launch.py @@ -1,31 +1,80 @@ """ -Machine Vision Harmonic - World Launcher (FIXED) +Pick Place Harmonic - World Launcher +Configures Gazebo resource paths """ import os from launch import LaunchDescription -from launch.actions import ExecuteProcess +from launch.actions import ( + DeclareLaunchArgument, + ExecuteProcess, + IncludeLaunchDescription, + SetEnvironmentVariable, + AppendEnvironmentVariable, +) +from launch.launch_description_sources import PythonLaunchDescriptionSource from ament_index_python.packages import get_package_share_directory +from launch_ros.actions import Node def generate_launch_description(): + package_dir = get_package_share_directory("custom_robots") + robotiq_description_pkg = get_package_share_directory("robotiq_description") + ur5_gripper_pkg = get_package_share_directory("ur5_gripper_description") + robotiq_pkg_share_dir = get_package_share_directory("robotiq_description") + ros_gz_sim = get_package_share_directory("ros_gz_sim") + + warehouse_models_path = os.path.join(robotiq_pkg_share_dir, "world", "models") + ur5_share_parent = os.path.dirname(ur5_gripper_pkg) + robotiq_share_parent = os.path.dirname(robotiq_pkg_share_dir) + world_path = "/opt/jderobot/Worlds/machine_vision_harmonic.world" + gazebo_models_path = os.path.join(package_dir, "models") - ur5_pkg = get_package_share_directory("ur5_gripper_description") - robotiq_pkg = get_package_share_directory("robotiq_description") + gz_ros2_control_install = "/home/ws/install" + gz_lib_path = os.path.join(gz_ros2_control_install, "gz_ros2_control", "lib") resource_path = ( - os.path.dirname(ur5_pkg) + ":" + - os.path.dirname(robotiq_pkg) + ur5_share_parent + ":" + robotiq_share_parent + ":" + warehouse_models_path ) + gz_env = { + "GZ_SIM_RESOURCE_PATH": resource_path, + "GZ_SIM_SYSTEM_PLUGIN_PATH": gz_lib_path + ":/opt/ros/humble/lib", + "LD_LIBRARY_PATH": gz_lib_path + + ":/opt/ros/humble/lib:/usr/lib/x86_64-linux-gnu", + "DISPLAY": ":2", + } + gazebo = ExecuteProcess( - cmd=["gz", "sim", "-r", "-v", "4", world_path], + cmd=["gz", "sim", "-s", "-r", "-v", "4", world_path], + output="screen", + additional_env=gz_env, + shell=False, + ) + + world_entity_cmd = Node( + package="ros_gz_sim", + executable="create", + arguments=["-name", "world", "-file", world_path], output="screen", - additional_env={ - "GZ_SIM_RESOURCE_PATH": resource_path - } ) - return LaunchDescription([gazebo]) \ No newline at end of file + ld = LaunchDescription() + + ld.add_action( + SetEnvironmentVariable( + "GZ_SIM_RESOURCE_PATH", + gazebo_models_path + ":" + ur5_gripper_pkg + ":" + robotiq_description_pkg, + ) + ) + set_env_vars_resources = AppendEnvironmentVariable( + "GZ_SIM_RESOURCE_PATH", + gazebo_models_path + ":" + ur5_gripper_pkg + ":" + robotiq_description_pkg, + ) + ld.add_action(set_env_vars_resources) + ld.add_action(gazebo) + ld.add_action(world_entity_cmd) + + return ld From 936f18d265e3703f30239a9ffccefc5210dd0d70 Mon Sep 17 00:00:00 2001 From: jepear19 Date: Tue, 24 Mar 2026 20:20:12 +0100 Subject: [PATCH 19/32] Fix launchers --- .../machine_vision_harmonic/robot.launch.py | 214 ++++++++++++++++-- 1 file changed, 198 insertions(+), 16 deletions(-) diff --git a/Launchers/machine_vision_harmonic/robot.launch.py b/Launchers/machine_vision_harmonic/robot.launch.py index 17d88dfff..6fc3ffd00 100644 --- a/Launchers/machine_vision_harmonic/robot.launch.py +++ b/Launchers/machine_vision_harmonic/robot.launch.py @@ -1,28 +1,210 @@ -""" -Machine Vision Harmonic - Robot Launcher -(USA EL MISMO QUE PICK&PLACE) -""" - import os from launch import LaunchDescription -from launch.actions import IncludeLaunchDescription -from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.actions import ( + DeclareLaunchArgument, + ExecuteProcess, + RegisterEventHandler, + TimerAction, +) +from launch.conditions import IfCondition +from launch.event_handlers import OnProcessExit +from launch.substitutions import LaunchConfiguration +from launch_ros.actions import Node from ament_index_python.packages import get_package_share_directory +import xacro def generate_launch_description(): + pkg_share_dir = get_package_share_directory("ur5_gripper_description") + robotiq_pkg_share_dir = get_package_share_directory("robotiq_description") + + gz_ros2_control_install = "/home/ws/install" + gz_lib_path = os.path.join(gz_ros2_control_install, "gz_ros2_control", "lib") + + warehouse_models_path = os.path.join(robotiq_pkg_share_dir, "world", "models") + ur5_share_parent = os.path.dirname(pkg_share_dir) + robotiq_share_parent = os.path.dirname(robotiq_pkg_share_dir) + + resource_path = ( + ur5_share_parent + ":" + + robotiq_share_parent + ":" + + warehouse_models_path + ) + + gz_env = { + "GZ_SIM_RESOURCE_PATH": resource_path, + "GZ_SIM_SYSTEM_PLUGIN_PATH": ( + "/home/ws/install/gz_link_attacher/lib:" + + gz_lib_path + + ":/opt/ros/humble/lib" + ), + "LD_LIBRARY_PATH": "/home/ws/install/gz_link_attacher/lib:" + + gz_lib_path + + ":/opt/ros/humble/lib:/usr/lib/x86_64-linux-gnu:" + + os.environ.get("LD_LIBRARY_PATH", ""), + "DISPLAY": os.environ.get("DISPLAY", ":0"), + } + + declared_arguments = [ + DeclareLaunchArgument("ur_type", default_value="ur5"), + DeclareLaunchArgument("launch_rviz", default_value="true"), + ] + + + xacro_file = os.path.join(pkg_share_dir, "urdf", "ur5_robotiq85_gripper.urdf.xacro") + controllers_file = os.path.join(pkg_share_dir, "config", "ur5_controllers.yaml") + + robot_description_content = xacro.process_file( + xacro_file, + mappings={ + "ur_type": "ur5", + "name": "ur", + "prefix": "", + "use_fake_hardware": "false", + "sim_gazebo": "false", + "sim_gz": "true", + "simulation_controllers": controllers_file, + }, + ).toxml() + + robot_description = {"robot_description": robot_description_content} + + robot_state_publisher = Node( + package="robot_state_publisher", + executable="robot_state_publisher", + output="screen", + parameters=[robot_description, {"use_sim_time": True}], + ) + + + world_file = "/opt/jderobot/Worlds/machine_vision_harmonic.world" + + gazebo = ExecuteProcess( + cmd=["gz", "sim", "-r", "-v", "4", world_file], + output="screen", + additional_env=gz_env, + shell=False, + ) - ur5_gripper_pkg = get_package_share_directory("ur5_gripper_description") - warehouse_launch_file = os.path.join( - ur5_gripper_pkg, "launch", "spawn_robot_warehouse.launch.py" + spawn_entity = Node( + package="ros_gz_sim", + executable="create", + arguments=[ + "-topic", "robot_description", + "-name", "ur5_robotiq", + "-allow_renaming", "true", + "-x", "0.0", + "-y", "0.0", + "-z", "0.0", + "-R", "0.0", + "-P", "0.0", + "-Y", "0.0", + ], + output="screen", ) - warehouse_launch = IncludeLaunchDescription( - PythonLaunchDescriptionSource(warehouse_launch_file), - launch_arguments={ - "launch_rviz": "false", - }.items(), + static_tf = Node( + package="tf2_ros", + executable="static_transform_publisher", + arguments=["0", "0", "0", "0", "0", "0", "world", "base_link"], + output="screen", + parameters=[{"use_sim_time": True}], + ) + + gz_ros2_bridge_clock = Node( + package="ros_gz_bridge", + executable="parameter_bridge", + arguments=["/clock@rosgraph_msgs/msg/Clock[gz.msgs.Clock"], + output="screen", + parameters=[{"use_sim_time": True}], + ) + + + load_joint_state_broadcaster = Node( + package="controller_manager", + executable="spawner", + arguments=["joint_state_broadcaster", "--controller-manager", "/controller_manager"], + output="screen", ) - return LaunchDescription([warehouse_launch]) \ No newline at end of file + load_joint_trajectory_controller = Node( + package="controller_manager", + executable="spawner", + arguments=["joint_trajectory_controller", "-c", "/controller_manager"], + output="screen", + ) + + load_gripper_controller = Node( + package="controller_manager", + executable="spawner", + arguments=["gripper_controller", "-c", "/controller_manager"], + output="screen", + ) + + + moveit_config_package = "ur5_gripper_moveit_config" + moveit_pkg = get_package_share_directory(moveit_config_package) + + ompl_planning_yaml = os.path.join(moveit_pkg, "config", "ompl_planning.yaml") + kinematics_yaml = os.path.join(moveit_pkg, "config", "kinematics.yaml") + srdf_file = os.path.join(moveit_pkg, "srdf", "ur5_robotiq.srdf") + moveit_controllers = os.path.join(moveit_pkg, "config", "controllers.yaml") + + with open(srdf_file, "r") as f: + robot_description_semantic = {"robot_description_semantic": f.read()} + + move_group_node = Node( + package="moveit_ros_move_group", + executable="move_group", + output="screen", + parameters=[ + robot_description, + robot_description_semantic, + kinematics_yaml, + ompl_planning_yaml, + moveit_controllers, + {"use_sim_time": True}, + ], + condition=IfCondition(LaunchConfiguration("launch_rviz")), + ) + + rviz_node = Node( + package="rviz2", + executable="rviz2", + arguments=["-d", os.path.join(moveit_pkg, "rviz", "moveit.rviz")], + condition=IfCondition(LaunchConfiguration("launch_rviz")), + ) + + + delay_spawn = TimerAction(period=5.0, actions=[spawn_entity]) + + delay_jsb = RegisterEventHandler( + OnProcessExit(target_action=spawn_entity, on_exit=[load_joint_state_broadcaster]) + ) + + delay_jtc = RegisterEventHandler( + OnProcessExit(target_action=load_joint_state_broadcaster, on_exit=[load_joint_trajectory_controller]) + ) + + delay_gc = RegisterEventHandler( + OnProcessExit(target_action=load_joint_trajectory_controller, on_exit=[load_gripper_controller]) + ) + + delay_mg = TimerAction(period=10.0, actions=[move_group_node]) + delay_rviz = TimerAction(period=12.0, actions=[rviz_node]) + + return LaunchDescription( + declared_arguments + [ + gazebo, + robot_state_publisher, + static_tf, + gz_ros2_bridge_clock, + delay_spawn, + delay_jsb, + delay_jtc, + delay_gc, + delay_mg, + delay_rviz, + ] + ) \ No newline at end of file From 9c80e0a297d5547c55fba1f5da2ce1ae398f4730 Mon Sep 17 00:00:00 2001 From: jepear19 Date: Tue, 24 Mar 2026 20:37:30 +0100 Subject: [PATCH 20/32] Fix launchers --- .../machine_vision_harmonic/robot.launch.py | 102 +++++------------- 1 file changed, 29 insertions(+), 73 deletions(-) diff --git a/Launchers/machine_vision_harmonic/robot.launch.py b/Launchers/machine_vision_harmonic/robot.launch.py index 6fc3ffd00..04c9a16bb 100644 --- a/Launchers/machine_vision_harmonic/robot.launch.py +++ b/Launchers/machine_vision_harmonic/robot.launch.py @@ -15,21 +15,15 @@ def generate_launch_description(): + pkg_share_dir = get_package_share_directory("ur5_gripper_description") - robotiq_pkg_share_dir = get_package_share_directory("robotiq_description") gz_ros2_control_install = "/home/ws/install" gz_lib_path = os.path.join(gz_ros2_control_install, "gz_ros2_control", "lib") - warehouse_models_path = os.path.join(robotiq_pkg_share_dir, "world", "models") - ur5_share_parent = os.path.dirname(pkg_share_dir) - robotiq_share_parent = os.path.dirname(robotiq_pkg_share_dir) + machine_vision_models_path = "/opt/jderobot/Worlds/models" - resource_path = ( - ur5_share_parent + ":" + - robotiq_share_parent + ":" + - warehouse_models_path - ) + resource_path = machine_vision_models_path gz_env = { "GZ_SIM_RESOURCE_PATH": resource_path, @@ -42,15 +36,12 @@ def generate_launch_description(): + gz_lib_path + ":/opt/ros/humble/lib:/usr/lib/x86_64-linux-gnu:" + os.environ.get("LD_LIBRARY_PATH", ""), - "DISPLAY": os.environ.get("DISPLAY", ":0"), } declared_arguments = [ - DeclareLaunchArgument("ur_type", default_value="ur5"), DeclareLaunchArgument("launch_rviz", default_value="true"), ] - xacro_file = os.path.join(pkg_share_dir, "urdf", "ur5_robotiq85_gripper.urdf.xacro") controllers_file = os.path.join(pkg_share_dir, "config", "ur5_controllers.yaml") @@ -61,7 +52,6 @@ def generate_launch_description(): "name": "ur", "prefix": "", "use_fake_hardware": "false", - "sim_gazebo": "false", "sim_gz": "true", "simulation_controllers": controllers_file, }, @@ -72,34 +62,30 @@ def generate_launch_description(): robot_state_publisher = Node( package="robot_state_publisher", executable="robot_state_publisher", - output="screen", parameters=[robot_description, {"use_sim_time": True}], + output="screen", ) - world_file = "/opt/jderobot/Worlds/machine_vision_harmonic.world" + env = os.environ.copy() + env.update(gz_env) + gazebo = ExecuteProcess( cmd=["gz", "sim", "-r", "-v", "4", world_file], output="screen", - additional_env=gz_env, - shell=False, + additional_env=env, ) - spawn_entity = Node( package="ros_gz_sim", executable="create", arguments=[ "-topic", "robot_description", - "-name", "ur5_robotiq", - "-allow_renaming", "true", + "-name", "ur5", "-x", "0.0", "-y", "0.0", - "-z", "0.0", - "-R", "0.0", - "-P", "0.0", - "-Y", "0.0", + "-z", "0.75", ], output="screen", ) @@ -108,103 +94,73 @@ def generate_launch_description(): package="tf2_ros", executable="static_transform_publisher", arguments=["0", "0", "0", "0", "0", "0", "world", "base_link"], - output="screen", - parameters=[{"use_sim_time": True}], ) - gz_ros2_bridge_clock = Node( + bridge = Node( package="ros_gz_bridge", executable="parameter_bridge", arguments=["/clock@rosgraph_msgs/msg/Clock[gz.msgs.Clock"], - output="screen", - parameters=[{"use_sim_time": True}], ) - - load_joint_state_broadcaster = Node( + jsb = Node( package="controller_manager", executable="spawner", - arguments=["joint_state_broadcaster", "--controller-manager", "/controller_manager"], - output="screen", + arguments=["joint_state_broadcaster"], ) - load_joint_trajectory_controller = Node( + jtc = Node( package="controller_manager", executable="spawner", - arguments=["joint_trajectory_controller", "-c", "/controller_manager"], - output="screen", + arguments=["joint_trajectory_controller"], ) - load_gripper_controller = Node( + gripper = Node( package="controller_manager", executable="spawner", - arguments=["gripper_controller", "-c", "/controller_manager"], - output="screen", + arguments=["gripper_controller"], ) + moveit_pkg = get_package_share_directory("ur5_gripper_moveit_config") - moveit_config_package = "ur5_gripper_moveit_config" - moveit_pkg = get_package_share_directory(moveit_config_package) - - ompl_planning_yaml = os.path.join(moveit_pkg, "config", "ompl_planning.yaml") - kinematics_yaml = os.path.join(moveit_pkg, "config", "kinematics.yaml") - srdf_file = os.path.join(moveit_pkg, "srdf", "ur5_robotiq.srdf") - moveit_controllers = os.path.join(moveit_pkg, "config", "controllers.yaml") - - with open(srdf_file, "r") as f: - robot_description_semantic = {"robot_description_semantic": f.read()} - - move_group_node = Node( + move_group = Node( package="moveit_ros_move_group", executable="move_group", - output="screen", - parameters=[ - robot_description, - robot_description_semantic, - kinematics_yaml, - ompl_planning_yaml, - moveit_controllers, - {"use_sim_time": True}, - ], + parameters=[robot_description, {"use_sim_time": True}], condition=IfCondition(LaunchConfiguration("launch_rviz")), ) - rviz_node = Node( + rviz = Node( package="rviz2", executable="rviz2", arguments=["-d", os.path.join(moveit_pkg, "rviz", "moveit.rviz")], condition=IfCondition(LaunchConfiguration("launch_rviz")), ) - delay_spawn = TimerAction(period=5.0, actions=[spawn_entity]) delay_jsb = RegisterEventHandler( - OnProcessExit(target_action=spawn_entity, on_exit=[load_joint_state_broadcaster]) + OnProcessExit(target_action=spawn_entity, on_exit=[jsb]) ) delay_jtc = RegisterEventHandler( - OnProcessExit(target_action=load_joint_state_broadcaster, on_exit=[load_joint_trajectory_controller]) + OnProcessExit(target_action=jsb, on_exit=[jtc]) ) - delay_gc = RegisterEventHandler( - OnProcessExit(target_action=load_joint_trajectory_controller, on_exit=[load_gripper_controller]) + delay_gripper = RegisterEventHandler( + OnProcessExit(target_action=jtc, on_exit=[gripper]) ) - delay_mg = TimerAction(period=10.0, actions=[move_group_node]) - delay_rviz = TimerAction(period=12.0, actions=[rviz_node]) - return LaunchDescription( declared_arguments + [ gazebo, robot_state_publisher, static_tf, - gz_ros2_bridge_clock, + bridge, delay_spawn, delay_jsb, delay_jtc, - delay_gc, - delay_mg, - delay_rviz, + delay_gripper, + move_group, + rviz, ] ) \ No newline at end of file From bb6a47544cd2403917f0dccc7cdb860db1a66ae9 Mon Sep 17 00:00:00 2001 From: jepear19 Date: Tue, 24 Mar 2026 20:51:19 +0100 Subject: [PATCH 21/32] Fix robot.launch.py --- .../machine_vision_harmonic/robot.launch.py | 409 ++++++++++++------ 1 file changed, 273 insertions(+), 136 deletions(-) diff --git a/Launchers/machine_vision_harmonic/robot.launch.py b/Launchers/machine_vision_harmonic/robot.launch.py index 04c9a16bb..919b98941 100644 --- a/Launchers/machine_vision_harmonic/robot.launch.py +++ b/Launchers/machine_vision_harmonic/robot.launch.py @@ -1,166 +1,303 @@ -import os +#!/usr/bin/python3 + +# ===================================== COPYRIGHT ===================================== # +# # +# IFRA (Intelligent Flexible Robotics and Assembly) Group, CRANFIELD UNIVERSITY # +# Created on behalf of the IFRA Group at Cranfield University, United Kingdom # +# E-mail: IFRA@cranfield.ac.uk # +# # +# Licensed under the Apache-2.0 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. # +# # +# IFRA Group - Cranfield University # +# AUTHORS: Mikel Bueno Viso - Mikel.Bueno-Viso@cranfield.ac.uk # +# Dr. Seemal Asif - s.asif@cranfield.ac.uk # +# Prof. Phil Webb - p.f.webb@cranfield.ac.uk # +# # +# Date: June, 2024. # +# # +# ===================================== COPYRIGHT ===================================== # + +# ======= CITE OUR WORK ======= # +# You can cite our work with the following statement: +# IFRA-Cranfield (2023) ROS 2 Sim-to-Real Robot Control. URL: https://github.com/IFRA-Cranfield/ros2_SimRealRobotControl. + +# simulation.launch.py: +# Launch file for the ROBOT's GAZEBO SIMULATION in ROS2 Humble: + +# Import libraries: +import os, sys, xacro, yaml +from ament_index_python.packages import get_package_share_directory, PackageNotFoundError from launch import LaunchDescription -from launch.actions import ( - DeclareLaunchArgument, - ExecuteProcess, - RegisterEventHandler, - TimerAction, -) -from launch.conditions import IfCondition -from launch.event_handlers import OnProcessExit -from launch.substitutions import LaunchConfiguration from launch_ros.actions import Node -from ament_index_python.packages import get_package_share_directory -import xacro +from launch.actions import IncludeLaunchDescription, RegisterEventHandler +from launch.event_handlers import OnProcessExit +from launch.launch_description_sources import PythonLaunchDescriptionSource +# LOAD FILE: +def load_file(package_name, file_path): + package_path = get_package_share_directory(package_name) + absolute_file_path = os.path.join(package_path, file_path) + try: + with open(absolute_file_path, 'r') as file: + return file.read() + except EnvironmentError: + # parent of IOError, OSError *and* WindowsError where available. + return None +# LOAD YAML: +def load_yaml(package_name, file_path): + package_path = get_package_share_directory(package_name) + absolute_file_path = os.path.join(package_path, file_path) + try: + with open(absolute_file_path, 'r') as file: + return yaml.safe_load(file) + except EnvironmentError: + # parent of IOError, OSError *and* WindowsError where available. + return None -def generate_launch_description(): +# ===== REQUIRED TO GET THE ROBOT CONFIGURATION === # - pkg_share_dir = get_package_share_directory("ur5_gripper_description") - - gz_ros2_control_install = "/home/ws/install" - gz_lib_path = os.path.join(gz_ros2_control_install, "gz_ros2_control", "lib") - - machine_vision_models_path = "/opt/jderobot/Worlds/models" - - resource_path = machine_vision_models_path - - gz_env = { - "GZ_SIM_RESOURCE_PATH": resource_path, - "GZ_SIM_SYSTEM_PLUGIN_PATH": ( - "/home/ws/install/gz_link_attacher/lib:" - + gz_lib_path - + ":/opt/ros/humble/lib" - ), - "LD_LIBRARY_PATH": "/home/ws/install/gz_link_attacher/lib:" - + gz_lib_path - + ":/opt/ros/humble/lib:/usr/lib/x86_64-linux-gnu:" - + os.environ.get("LD_LIBRARY_PATH", ""), - } - - declared_arguments = [ - DeclareLaunchArgument("launch_rviz", default_value="true"), - ] - - xacro_file = os.path.join(pkg_share_dir, "urdf", "ur5_robotiq85_gripper.urdf.xacro") - controllers_file = os.path.join(pkg_share_dir, "config", "ur5_controllers.yaml") - - robot_description_content = xacro.process_file( - xacro_file, - mappings={ - "ur_type": "ur5", - "name": "ur", - "prefix": "", - "use_fake_hardware": "false", - "sim_gz": "true", - "simulation_controllers": controllers_file, - }, - ).toxml() - - robot_description = {"robot_description": robot_description_content} - - robot_state_publisher = Node( - package="robot_state_publisher", - executable="robot_state_publisher", - parameters=[robot_description, {"use_sim_time": True}], - output="screen", - ) +# EVALUATE INPUT ARGUMENTS: +def AssignArgument(ARGUMENT): + ARGUMENTS = sys.argv + for y in ARGUMENTS: + if (ARGUMENT + ":=") in y: + ARG = y.replace((ARGUMENT + ":="),"") + return(ARG) - world_file = "/opt/jderobot/Worlds/machine_vision_harmonic.world" +# GET CONFIGURATION from YAML: +def GetCONFIG(CONFIGURATION, PKG_PATH): + + RESULT = {"Success": False, "ID": "", "Name": "", "urdf": "", "ee": ""} + + YAML_PATH = PKG_PATH + "/config/configurations.yaml" + + if not os.path.exists(YAML_PATH): + return (RESULT) + + with open(YAML_PATH, 'r') as YAML: + cYAML = yaml.safe_load(YAML) - env = os.environ.copy() - env.update(gz_env) + for x in cYAML["Configurations"]: - gazebo = ExecuteProcess( - cmd=["gz", "sim", "-r", "-v", "4", world_file], - output="screen", - additional_env=env, - ) + if x["ID"] == CONFIGURATION: + RESULT["Success"] = True + RESULT["ID"] = x["ID"] + RESULT["Name"] = x["Name"] + RESULT["urdf"] = x["urdf"] + RESULT["rob"] = x["rob"] + RESULT["ee"] = x["ee"] - spawn_entity = Node( - package="ros_gz_sim", - executable="create", - arguments=[ - "-topic", "robot_description", - "-name", "ur5", - "-x", "0.0", - "-y", "0.0", - "-z", "0.75", - ], - output="screen", - ) + return(RESULT) - static_tf = Node( - package="tf2_ros", - executable="static_transform_publisher", - arguments=["0", "0", "0", "0", "0", "0", "world", "base_link"], - ) +# GET EE-Controllers LIST: +def GetEEctr(EEName): + + RESULT = [] - bridge = Node( - package="ros_gz_bridge", - executable="parameter_bridge", - arguments=["/clock@rosgraph_msgs/msg/Clock[gz.msgs.Clock"], - ) + PATH = os.path.join(os.path.expanduser('~'), 'dev_ws', 'src', 'IndustrialRobots', 'ros2_SimRealRobotControl', 'ros2srrc_endeffectors', EEName, 'config') + YAML_PATH = PATH + "/controller_moveit2.yaml" + + with open(YAML_PATH, 'r') as YAML: + cYAML = yaml.safe_load(YAML) - jsb = Node( - package="controller_manager", - executable="spawner", - arguments=["joint_state_broadcaster"], + for x in cYAML["controller_names"]: + RESULT.append(x) + + return(RESULT) + +# CHECK if CONTROLLER file exists for EE: +def EEctrlEXISTS(EEName): + + PATH = os.path.join(os.path.expanduser('~'), 'dev_ws', 'src', 'IndustrialRobots', 'ros2_SimRealRobotControl', 'ros2srrc_endeffectors', EEName, 'config') + YAML_PATH = PATH + "/controller.yaml" + + RES = os.path.exists(YAML_PATH) + return(RES) + +# ========== **GENERATE LAUNCH DESCRIPTION** ========== # +def generate_launch_description(): + + LD = LaunchDescription() + + # === INPUT ARGUMENT: ROS 2 PACKAGE === # + PACKAGE_NAME = AssignArgument("package") + if PACKAGE_NAME != None: + None + else: + print("") + print("ERROR: package INPUT ARGUMENT has not been defined. Please try again.") + print("Closing... BYE!") + exit() + + # CHECK if -> PACKAGE EXISTS, and GET PATH: + try: + PKG_PATH = get_package_share_directory(PACKAGE_NAME + "_gazebo") + except PackageNotFoundError: + print("") + print("ERROR: The defined ROS 2 Package was not found. Please try again.") + print("Closing... BYE!") + exit() + except ValueError: + print("") + print("ERROR: The defined ROS 2 Package name is not valid. Please try again.") + print("Closing... BYE!") + exit() + + # === INPUT ARGUMENT: CONFIGURATION === # + CONFIG = AssignArgument("config") + CONFIGURATION = GetCONFIG(CONFIG, PKG_PATH) + + if CONFIGURATION["Success"] == False: + print("") + print("ERROR: config INPUT ARGUMENT has not been correctly defined. Please try again.") + print("Closing... BYE!") + exit() + + # === INPUT ARGUMENT: HMI === # + HMI = AssignArgument("hmi") + if HMI == "True" or HMI == "true": + HMI = "true" + else: + HMI = "false" + + # ========== CELL INFORMATION ========== # + print("") + print("===== GAZEBO: Robot Simulation (" + PACKAGE_NAME + "_gazebo) =====") + print("Robot configuration:") + print(CONFIGURATION["ID"] + " -> " + CONFIGURATION["Name"]) + print("") + + # ***** GAZEBO ***** # + # DECLARE Gazebo WORLD file: + robot_gazebo = os.path.join( + get_package_share_directory(PACKAGE_NAME + '_gazebo'), + 'worlds', + PACKAGE_NAME + '_machie_vision'+'.world') + # DECLARE Gazebo LAUNCH file: + gazebo = IncludeLaunchDescription( + PythonLaunchDescriptionSource([os.path.join(get_package_share_directory('gazebo_ros'), 'launch'), '/gazebo.launch.py']), + launch_arguments={'world': robot_gazebo}.items(), + ) + + # ***** ROBOT DESCRIPTION ***** # + # Robot Description file package: + robot_description_path = os.path.join(get_package_share_directory(PACKAGE_NAME + '_gazebo')) + # ROBOT urdf file path: + xacro_file = os.path.join(robot_description_path,'urdf',CONFIGURATION["urdf"]) + # Generate ROBOT_DESCRIPTION variable: + doc = xacro.parse(open(xacro_file)) + + if CONFIGURATION["ee"] == "none": + EE = "false" + else: + EE = "true" + + xacro.process_doc(doc, mappings={ + "EE": EE, + "EE_name": CONFIGURATION["ee"], + "hmi": HMI, + }) + + # EE -> Controller file needed? + if EE == "true": + if EEctrlEXISTS(CONFIGURATION["ee"]) == False: + EE = "true-NOctr" + + robot_description_config = doc.toxml() + robot_description = {'robot_description': robot_description_config} + + # ROBOT STATE PUBLISHER NODE: + node_robot_state_publisher = Node( + package='robot_state_publisher', + executable='robot_state_publisher', + output='both', + parameters=[ + robot_description, + {"use_sim_time": True} + ] ) - jtc = Node( + # SPAWN ROBOT TO GAZEBO: + spawn_entity = Node(package='gazebo_ros', executable='spawn_entity.py', + arguments=['-topic', 'robot_description','-entity', CONFIGURATION["rob"]], + output='both') + + # ***** CONTROLLERS ***** # + # Joint STATE BROADCASTER: + joint_state_broadcaster_spawner = Node( package="controller_manager", executable="spawner", - arguments=["joint_trajectory_controller"], + arguments=["joint_state_broadcaster", "--controller-manager", "/controller_manager"], ) - - gripper = Node( + # Joint TRAJECTORY Controller: + joint_trajectory_controller_spawner = Node( package="controller_manager", executable="spawner", - arguments=["gripper_controller"], + arguments=["joint_trajectory_controller", "-c", "/controller_manager"], ) - moveit_pkg = get_package_share_directory("ur5_gripper_moveit_config") + # EE CONTROLLERS: + if EE == "true": + CONTROLLERS = GetEEctr(CONFIGURATION["ee"]) + CONTROLLER_NODES = [] - move_group = Node( - package="moveit_ros_move_group", - executable="move_group", - parameters=[robot_description, {"use_sim_time": True}], - condition=IfCondition(LaunchConfiguration("launch_rviz")), - ) + for x in CONTROLLERS: + CONTROLLER_NODES.append( + Node( + package="controller_manager", + executable="spawner", + arguments=[x, "-c", "/controller_manager"], + ) + ) - rviz = Node( - package="rviz2", - executable="rviz2", - arguments=["-d", os.path.join(moveit_pkg, "rviz", "moveit.rviz")], - condition=IfCondition(LaunchConfiguration("launch_rviz")), - ) + # =============================================== # + # ========== RETURN LAUNCH DESCRIPTION ========== # - delay_spawn = TimerAction(period=5.0, actions=[spawn_entity]) + # Add ROS 2 Nodes to LaunchDescription() element: + LD.add_action(gazebo) + LD.add_action(node_robot_state_publisher) + LD.add_action(spawn_entity) - delay_jsb = RegisterEventHandler( - OnProcessExit(target_action=spawn_entity, on_exit=[jsb]) + LD.add_action(RegisterEventHandler( + OnProcessExit( + target_action = spawn_entity, + on_exit = [ + joint_state_broadcaster_spawner, + ] + ) + ) ) - delay_jtc = RegisterEventHandler( - OnProcessExit(target_action=jsb, on_exit=[jtc]) + LD.add_action(RegisterEventHandler( + OnProcessExit( + target_action = spawn_entity, + on_exit = [ + joint_trajectory_controller_spawner, + ] + ) + ) ) - delay_gripper = RegisterEventHandler( - OnProcessExit(target_action=jtc, on_exit=[gripper]) - ) + if EE == "true": - return LaunchDescription( - declared_arguments + [ - gazebo, - robot_state_publisher, - static_tf, - bridge, - delay_spawn, - delay_jsb, - delay_jtc, - delay_gripper, - move_group, - rviz, - ] - ) \ No newline at end of file + for x in CONTROLLER_NODES: + + LD.add_action(RegisterEventHandler( + OnProcessExit( + target_action = joint_trajectory_controller_spawner, + on_exit = [ + x, + ] + ) + ) + ) + + # ***** RETURN ***** # + return(LD) \ No newline at end of file From d6ccad1dbb7bba67e2d62e6ba00e8ef607888db8 Mon Sep 17 00:00:00 2001 From: jepear19 Date: Tue, 24 Mar 2026 21:05:04 +0100 Subject: [PATCH 22/32] Restart robot.launch.py --- .../machine_vision_harmonic/robot.launch.py | 331 +++--------------- 1 file changed, 54 insertions(+), 277 deletions(-) diff --git a/Launchers/machine_vision_harmonic/robot.launch.py b/Launchers/machine_vision_harmonic/robot.launch.py index 919b98941..f0aedd455 100644 --- a/Launchers/machine_vision_harmonic/robot.launch.py +++ b/Launchers/machine_vision_harmonic/robot.launch.py @@ -1,303 +1,80 @@ -#!/usr/bin/python3 +""" +Machine Vision Harmonic - Robot Launcher (FIXED) +""" -# ===================================== COPYRIGHT ===================================== # -# # -# IFRA (Intelligent Flexible Robotics and Assembly) Group, CRANFIELD UNIVERSITY # -# Created on behalf of the IFRA Group at Cranfield University, United Kingdom # -# E-mail: IFRA@cranfield.ac.uk # -# # -# Licensed under the Apache-2.0 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. # -# # -# IFRA Group - Cranfield University # -# AUTHORS: Mikel Bueno Viso - Mikel.Bueno-Viso@cranfield.ac.uk # -# Dr. Seemal Asif - s.asif@cranfield.ac.uk # -# Prof. Phil Webb - p.f.webb@cranfield.ac.uk # -# # -# Date: June, 2024. # -# # -# ===================================== COPYRIGHT ===================================== # - -# ======= CITE OUR WORK ======= # -# You can cite our work with the following statement: -# IFRA-Cranfield (2023) ROS 2 Sim-to-Real Robot Control. URL: https://github.com/IFRA-Cranfield/ros2_SimRealRobotControl. - -# simulation.launch.py: -# Launch file for the ROBOT's GAZEBO SIMULATION in ROS2 Humble: - -# Import libraries: -import os, sys, xacro, yaml -from ament_index_python.packages import get_package_share_directory, PackageNotFoundError +import os +import xacro from launch import LaunchDescription from launch_ros.actions import Node -from launch.actions import IncludeLaunchDescription, RegisterEventHandler +from launch.actions import RegisterEventHandler from launch.event_handlers import OnProcessExit -from launch.launch_description_sources import PythonLaunchDescriptionSource - -# LOAD FILE: -def load_file(package_name, file_path): - package_path = get_package_share_directory(package_name) - absolute_file_path = os.path.join(package_path, file_path) - try: - with open(absolute_file_path, 'r') as file: - return file.read() - except EnvironmentError: - # parent of IOError, OSError *and* WindowsError where available. - return None -# LOAD YAML: -def load_yaml(package_name, file_path): - package_path = get_package_share_directory(package_name) - absolute_file_path = os.path.join(package_path, file_path) - try: - with open(absolute_file_path, 'r') as file: - return yaml.safe_load(file) - except EnvironmentError: - # parent of IOError, OSError *and* WindowsError where available. - return None - -# ===== REQUIRED TO GET THE ROBOT CONFIGURATION === # - -# EVALUATE INPUT ARGUMENTS: -def AssignArgument(ARGUMENT): - ARGUMENTS = sys.argv - for y in ARGUMENTS: - if (ARGUMENT + ":=") in y: - ARG = y.replace((ARGUMENT + ":="),"") - return(ARG) - -# GET CONFIGURATION from YAML: -def GetCONFIG(CONFIGURATION, PKG_PATH): - - RESULT = {"Success": False, "ID": "", "Name": "", "urdf": "", "ee": ""} - - YAML_PATH = PKG_PATH + "/config/configurations.yaml" - - if not os.path.exists(YAML_PATH): - return (RESULT) - - with open(YAML_PATH, 'r') as YAML: - cYAML = yaml.safe_load(YAML) - - for x in cYAML["Configurations"]: +from ament_index_python.packages import get_package_share_directory - if x["ID"] == CONFIGURATION: - RESULT["Success"] = True - RESULT["ID"] = x["ID"] - RESULT["Name"] = x["Name"] - RESULT["urdf"] = x["urdf"] - RESULT["rob"] = x["rob"] - RESULT["ee"] = x["ee"] - return(RESULT) - -# GET EE-Controllers LIST: -def GetEEctr(EEName): - - RESULT = [] - - PATH = os.path.join(os.path.expanduser('~'), 'dev_ws', 'src', 'IndustrialRobots', 'ros2_SimRealRobotControl', 'ros2srrc_endeffectors', EEName, 'config') - YAML_PATH = PATH + "/controller_moveit2.yaml" - - with open(YAML_PATH, 'r') as YAML: - cYAML = yaml.safe_load(YAML) - - for x in cYAML["controller_names"]: - RESULT.append(x) - - return(RESULT) - -# CHECK if CONTROLLER file exists for EE: -def EEctrlEXISTS(EEName): - - PATH = os.path.join(os.path.expanduser('~'), 'dev_ws', 'src', 'IndustrialRobots', 'ros2_SimRealRobotControl', 'ros2srrc_endeffectors', EEName, 'config') - YAML_PATH = PATH + "/controller.yaml" - - RES = os.path.exists(YAML_PATH) - return(RES) - -# ========== **GENERATE LAUNCH DESCRIPTION** ========== # def generate_launch_description(): - - LD = LaunchDescription() - - # === INPUT ARGUMENT: ROS 2 PACKAGE === # - PACKAGE_NAME = AssignArgument("package") - if PACKAGE_NAME != None: - None - else: - print("") - print("ERROR: package INPUT ARGUMENT has not been defined. Please try again.") - print("Closing... BYE!") - exit() - - # CHECK if -> PACKAGE EXISTS, and GET PATH: - try: - PKG_PATH = get_package_share_directory(PACKAGE_NAME + "_gazebo") - except PackageNotFoundError: - print("") - print("ERROR: The defined ROS 2 Package was not found. Please try again.") - print("Closing... BYE!") - exit() - except ValueError: - print("") - print("ERROR: The defined ROS 2 Package name is not valid. Please try again.") - print("Closing... BYE!") - exit() - - # === INPUT ARGUMENT: CONFIGURATION === # - CONFIG = AssignArgument("config") - CONFIGURATION = GetCONFIG(CONFIG, PKG_PATH) - if CONFIGURATION["Success"] == False: - print("") - print("ERROR: config INPUT ARGUMENT has not been correctly defined. Please try again.") - print("Closing... BYE!") - exit() + PACKAGE_NAME = "ros2srrc_ur5" + pkg_path = get_package_share_directory(PACKAGE_NAME + "_gazebo") - # === INPUT ARGUMENT: HMI === # - HMI = AssignArgument("hmi") - if HMI == "True" or HMI == "true": - HMI = "true" - else: - HMI = "false" + xacro_file = os.path.join(pkg_path, "urdf", "ur5.urdf.xacro") - # ========== CELL INFORMATION ========== # - print("") - print("===== GAZEBO: Robot Simulation (" + PACKAGE_NAME + "_gazebo) =====") - print("Robot configuration:") - print(CONFIGURATION["ID"] + " -> " + CONFIGURATION["Name"]) - print("") - - # ***** GAZEBO ***** # - # DECLARE Gazebo WORLD file: - robot_gazebo = os.path.join( - get_package_share_directory(PACKAGE_NAME + '_gazebo'), - 'worlds', - PACKAGE_NAME + '_machie_vision'+'.world') - # DECLARE Gazebo LAUNCH file: - gazebo = IncludeLaunchDescription( - PythonLaunchDescriptionSource([os.path.join(get_package_share_directory('gazebo_ros'), 'launch'), '/gazebo.launch.py']), - launch_arguments={'world': robot_gazebo}.items(), - ) - - # ***** ROBOT DESCRIPTION ***** # - # Robot Description file package: - robot_description_path = os.path.join(get_package_share_directory(PACKAGE_NAME + '_gazebo')) - # ROBOT urdf file path: - xacro_file = os.path.join(robot_description_path,'urdf',CONFIGURATION["urdf"]) - # Generate ROBOT_DESCRIPTION variable: doc = xacro.parse(open(xacro_file)) - - if CONFIGURATION["ee"] == "none": - EE = "false" - else: - EE = "true" - - xacro.process_doc(doc, mappings={ - "EE": EE, - "EE_name": CONFIGURATION["ee"], - "hmi": HMI, - }) - - # EE -> Controller file needed? - if EE == "true": - if EEctrlEXISTS(CONFIGURATION["ee"]) == False: - EE = "true-NOctr" - - robot_description_config = doc.toxml() - robot_description = {'robot_description': robot_description_config} + xacro.process_doc(doc) + + robot_description = {"robot_description": doc.toxml()} - # ROBOT STATE PUBLISHER NODE: - node_robot_state_publisher = Node( - package='robot_state_publisher', - executable='robot_state_publisher', - output='both', - parameters=[ - robot_description, - {"use_sim_time": True} - ] + robot_state_publisher = Node( + package="robot_state_publisher", + executable="robot_state_publisher", + output="screen", + parameters=[robot_description, {"use_sim_time": True}], ) - # SPAWN ROBOT TO GAZEBO: - spawn_entity = Node(package='gazebo_ros', executable='spawn_entity.py', - arguments=['-topic', 'robot_description','-entity', CONFIGURATION["rob"]], - output='both') + static_tf = Node( + package="tf2_ros", + executable="static_transform_publisher", + arguments=["0", "0", "0", "0", "0", "0", "world", "base_link"], + ) - # ***** CONTROLLERS ***** # - # Joint STATE BROADCASTER: - joint_state_broadcaster_spawner = Node( - package="controller_manager", - executable="spawner", - arguments=["joint_state_broadcaster", "--controller-manager", "/controller_manager"], + spawn_entity = Node( + package="ros_gz_sim", + executable="create", + arguments=[ + "-name", "ur5", + "-topic", "robot_description", + "-x", "0", + "-y", "0", + "-z", "0", + "-R", "0", + "-P", "0", + "-Y", "0", + ], + output="screen", ) - # Joint TRAJECTORY Controller: - joint_trajectory_controller_spawner = Node( + + # Controllers + joint_state_broadcaster = Node( package="controller_manager", executable="spawner", - arguments=["joint_trajectory_controller", "-c", "/controller_manager"], + arguments=["joint_state_broadcaster"], ) - # EE CONTROLLERS: - if EE == "true": - CONTROLLERS = GetEEctr(CONFIGURATION["ee"]) - CONTROLLER_NODES = [] - - for x in CONTROLLERS: - CONTROLLER_NODES.append( - Node( - package="controller_manager", - executable="spawner", - arguments=[x, "-c", "/controller_manager"], - ) - ) - - # =============================================== # - # ========== RETURN LAUNCH DESCRIPTION ========== # - - # Add ROS 2 Nodes to LaunchDescription() element: - LD.add_action(gazebo) - LD.add_action(node_robot_state_publisher) - LD.add_action(spawn_entity) - - LD.add_action(RegisterEventHandler( - OnProcessExit( - target_action = spawn_entity, - on_exit = [ - joint_state_broadcaster_spawner, - ] - ) - ) + joint_trajectory_controller = Node( + package="controller_manager", + executable="spawner", + arguments=["joint_trajectory_controller"], ) - LD.add_action(RegisterEventHandler( + delay_controllers = RegisterEventHandler( OnProcessExit( - target_action = spawn_entity, - on_exit = [ - joint_trajectory_controller_spawner, - ] - ) + target_action=spawn_entity, + on_exit=[joint_state_broadcaster, joint_trajectory_controller], ) ) - if EE == "true": - - for x in CONTROLLER_NODES: - - LD.add_action(RegisterEventHandler( - OnProcessExit( - target_action = joint_trajectory_controller_spawner, - on_exit = [ - x, - ] - ) - ) - ) - - # ***** RETURN ***** # - return(LD) \ No newline at end of file + return LaunchDescription([ + robot_state_publisher, + static_tf, + spawn_entity, + delay_controllers, + ]) \ No newline at end of file From c1c864239894b3474271be0d137a8bd856465acd Mon Sep 17 00:00:00 2001 From: jepear19 Date: Fri, 27 Mar 2026 00:01:00 +0100 Subject: [PATCH 23/32] Fix robot urdf with cameras --- .../ur5_robotiq_2f85_with_cams.urdf.xacro | 156 ++++++++++++++++++ .../machine_vision_harmonic.launch.py | 9 +- .../machine_vision_harmonic/robot.launch.py | 40 ++--- .../machine_vision_harmonic/world.launch.py | 77 ++------- 4 files changed, 198 insertions(+), 84 deletions(-) create mode 100644 Industrial/ur5_gripper_description/urdf/ur5_robotiq_2f85_with_cams.urdf.xacro diff --git a/Industrial/ur5_gripper_description/urdf/ur5_robotiq_2f85_with_cams.urdf.xacro b/Industrial/ur5_gripper_description/urdf/ur5_robotiq_2f85_with_cams.urdf.xacro new file mode 100644 index 000000000..90c0ca784 --- /dev/null +++ b/Industrial/ur5_gripper_description/urdf/ur5_robotiq_2f85_with_cams.urdf.xacro @@ -0,0 +1,156 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + gz_ros2_control/GazeboSimSystem + + $(find ros2srrc_robots)/ur5/config/controller.yaml + $(find ros2srrc_endeffectors)/${EE_name}/config/controller.yaml + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/Launchers/machine_vision_harmonic/machine_vision_harmonic.launch.py b/Launchers/machine_vision_harmonic/machine_vision_harmonic.launch.py index 09c1e7f27..d8a3b399e 100644 --- a/Launchers/machine_vision_harmonic/machine_vision_harmonic.launch.py +++ b/Launchers/machine_vision_harmonic/machine_vision_harmonic.launch.py @@ -1,7 +1,3 @@ -""" -Pick Place Harmonic - Main Launcher -""" - import os from launch import LaunchDescription from launch.actions import IncludeLaunchDescription @@ -20,4 +16,7 @@ def generate_launch_description(): PythonLaunchDescriptionSource(os.path.join(base_dir, "robot.launch.py")) ) - return LaunchDescription([world_launch, robot_launch]) + return LaunchDescription([ + world_launch, + robot_launch + ]) \ No newline at end of file diff --git a/Launchers/machine_vision_harmonic/robot.launch.py b/Launchers/machine_vision_harmonic/robot.launch.py index f0aedd455..6fde4903c 100644 --- a/Launchers/machine_vision_harmonic/robot.launch.py +++ b/Launchers/machine_vision_harmonic/robot.launch.py @@ -1,9 +1,6 @@ -""" -Machine Vision Harmonic - Robot Launcher (FIXED) -""" - import os import xacro + from launch import LaunchDescription from launch_ros.actions import Node from launch.actions import RegisterEventHandler @@ -13,15 +10,22 @@ def generate_launch_description(): - PACKAGE_NAME = "ros2srrc_ur5" - pkg_path = get_package_share_directory(PACKAGE_NAME + "_gazebo") + pkg_path = get_package_share_directory("ur5_gripper_description") - xacro_file = os.path.join(pkg_path, "urdf", "ur5.urdf.xacro") + xacro_file = os.path.join( + pkg_path, + "urdf", + "ur5_robotiq_2f85_with_cams.urdf.xacro" + ) doc = xacro.parse(open(xacro_file)) - xacro.process_doc(doc) + xacro.process_doc(doc, mappings={ + "hmi": "true" + }) - robot_description = {"robot_description": doc.toxml()} + robot_description = { + "robot_description": doc.toxml() + } robot_state_publisher = Node( package="robot_state_publisher", @@ -30,10 +34,12 @@ def generate_launch_description(): parameters=[robot_description, {"use_sim_time": True}], ) - static_tf = Node( - package="tf2_ros", - executable="static_transform_publisher", - arguments=["0", "0", "0", "0", "0", "0", "world", "base_link"], + clock_bridge = Node( + package="ros_gz_bridge", + executable="parameter_bridge", + arguments=["/clock@rosgraph_msgs/msg/Clock[gz.msgs.Clock"], + parameters=[{"use_sim_time": True}], + output="screen", ) spawn_entity = Node( @@ -44,15 +50,11 @@ def generate_launch_description(): "-topic", "robot_description", "-x", "0", "-y", "0", - "-z", "0", - "-R", "0", - "-P", "0", - "-Y", "0", + "-z", "0.01", ], output="screen", ) - # Controllers joint_state_broadcaster = Node( package="controller_manager", executable="spawner", @@ -74,7 +76,7 @@ def generate_launch_description(): return LaunchDescription([ robot_state_publisher, - static_tf, + clock_bridge, spawn_entity, delay_controllers, ]) \ No newline at end of file diff --git a/Launchers/machine_vision_harmonic/world.launch.py b/Launchers/machine_vision_harmonic/world.launch.py index ddfa4bbdb..0b95eb014 100644 --- a/Launchers/machine_vision_harmonic/world.launch.py +++ b/Launchers/machine_vision_harmonic/world.launch.py @@ -1,80 +1,37 @@ -""" -Pick Place Harmonic - World Launcher -Configures Gazebo resource paths -""" - import os from launch import LaunchDescription -from launch.actions import ( - DeclareLaunchArgument, - ExecuteProcess, - IncludeLaunchDescription, - SetEnvironmentVariable, - AppendEnvironmentVariable, -) -from launch.launch_description_sources import PythonLaunchDescriptionSource -from ament_index_python.packages import get_package_share_directory -from launch_ros.actions import Node +from launch.actions import ExecuteProcess def generate_launch_description(): - package_dir = get_package_share_directory("custom_robots") - robotiq_description_pkg = get_package_share_directory("robotiq_description") - ur5_gripper_pkg = get_package_share_directory("ur5_gripper_description") - robotiq_pkg_share_dir = get_package_share_directory("robotiq_description") - ros_gz_sim = get_package_share_directory("ros_gz_sim") - - warehouse_models_path = os.path.join(robotiq_pkg_share_dir, "world", "models") - ur5_share_parent = os.path.dirname(ur5_gripper_pkg) - robotiq_share_parent = os.path.dirname(robotiq_pkg_share_dir) - world_path = "/opt/jderobot/Worlds/machine_vision_harmonic.world" - gazebo_models_path = os.path.join(package_dir, "models") gz_ros2_control_install = "/home/ws/install" gz_lib_path = os.path.join(gz_ros2_control_install, "gz_ros2_control", "lib") - resource_path = ( - ur5_share_parent + ":" + robotiq_share_parent + ":" + warehouse_models_path - ) - gz_env = { - "GZ_SIM_RESOURCE_PATH": resource_path, - "GZ_SIM_SYSTEM_PLUGIN_PATH": gz_lib_path + ":/opt/ros/humble/lib", - "LD_LIBRARY_PATH": gz_lib_path - + ":/opt/ros/humble/lib:/usr/lib/x86_64-linux-gnu", - "DISPLAY": ":2", + "GZ_SIM_SYSTEM_PLUGIN_PATH": ( + "/home/ws/install/gz_link_attacher/lib:" + + gz_lib_path + + ":/opt/ros/humble/lib" + ), + "LD_LIBRARY_PATH": ( + "/home/ws/install/gz_link_attacher/lib:" + + gz_lib_path + + ":/opt/ros/humble/lib:/usr/lib/x86_64-linux-gnu:" + + os.environ.get("LD_LIBRARY_PATH", "") + ), + "DISPLAY": os.environ.get("DISPLAY", ":0"), } gazebo = ExecuteProcess( - cmd=["gz", "sim", "-s", "-r", "-v", "4", world_path], + cmd=["gz", "sim", "-r", "-v", "4", world_path], output="screen", additional_env=gz_env, shell=False, ) - world_entity_cmd = Node( - package="ros_gz_sim", - executable="create", - arguments=["-name", "world", "-file", world_path], - output="screen", - ) - - ld = LaunchDescription() - - ld.add_action( - SetEnvironmentVariable( - "GZ_SIM_RESOURCE_PATH", - gazebo_models_path + ":" + ur5_gripper_pkg + ":" + robotiq_description_pkg, - ) - ) - set_env_vars_resources = AppendEnvironmentVariable( - "GZ_SIM_RESOURCE_PATH", - gazebo_models_path + ":" + ur5_gripper_pkg + ":" + robotiq_description_pkg, - ) - ld.add_action(set_env_vars_resources) - ld.add_action(gazebo) - ld.add_action(world_entity_cmd) - - return ld + return LaunchDescription([ + gazebo + ]) \ No newline at end of file From bf3551a3a48d50a769b9c526c4d2569500a48846 Mon Sep 17 00:00:00 2001 From: jepear19 Date: Fri, 27 Mar 2026 00:37:21 +0100 Subject: [PATCH 24/32] Fix robot.launch.py --- Launchers/machine_vision_harmonic/robot.launch.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Launchers/machine_vision_harmonic/robot.launch.py b/Launchers/machine_vision_harmonic/robot.launch.py index 6fde4903c..5da751c75 100644 --- a/Launchers/machine_vision_harmonic/robot.launch.py +++ b/Launchers/machine_vision_harmonic/robot.launch.py @@ -20,7 +20,7 @@ def generate_launch_description(): doc = xacro.parse(open(xacro_file)) xacro.process_doc(doc, mappings={ - "hmi": "true" + "hmi": "false" }) robot_description = { From 2d4d22033641df8649f168e63000aabecd0abbb3 Mon Sep 17 00:00:00 2001 From: jepear19 Date: Fri, 27 Mar 2026 01:01:08 +0100 Subject: [PATCH 25/32] Fix robot model --- .../ur5_robotiq_2f85_with_cams.urdf.xacro | 155 +++++------------- 1 file changed, 39 insertions(+), 116 deletions(-) diff --git a/Industrial/ur5_gripper_description/urdf/ur5_robotiq_2f85_with_cams.urdf.xacro b/Industrial/ur5_gripper_description/urdf/ur5_robotiq_2f85_with_cams.urdf.xacro index 90c0ca784..0acba3a81 100644 --- a/Industrial/ur5_gripper_description/urdf/ur5_robotiq_2f85_with_cams.urdf.xacro +++ b/Industrial/ur5_gripper_description/urdf/ur5_robotiq_2f85_with_cams.urdf.xacro @@ -1,63 +1,10 @@ - - - - + - - - - - - - - - - - - - - - - - - - - - - - + + + @@ -68,89 +15,65 @@ + + + - - - - - - - - gz_ros2_control/GazeboSimSystem - - $(find ros2srrc_robots)/ur5/config/controller.yaml - $(find ros2srrc_endeffectors)/${EE_name}/config/controller.yaml - - - - - - - - - - - - - - - + + + - + - - - + + - + - + + + + + - - - - + + + - - - - + + + + + + gz_ros2_control/GazeboSimSystem - - - - - + $(find ros2srrc_robots)/ur5/config/controller.yaml + $(find ros2srrc_endeffectors)/${EE_name}/config/controller.yaml + + \ No newline at end of file From 751d5281b37b334dd45dd6e58098fb74f2e8f4bc Mon Sep 17 00:00:00 2001 From: jepear19 Date: Fri, 27 Mar 2026 01:23:29 +0100 Subject: [PATCH 26/32] Fix robot.launch.py --- Launchers/machine_vision_harmonic/robot.launch.py | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/Launchers/machine_vision_harmonic/robot.launch.py b/Launchers/machine_vision_harmonic/robot.launch.py index 5da751c75..e27638fbe 100644 --- a/Launchers/machine_vision_harmonic/robot.launch.py +++ b/Launchers/machine_vision_harmonic/robot.launch.py @@ -20,7 +20,9 @@ def generate_launch_description(): doc = xacro.parse(open(xacro_file)) xacro.process_doc(doc, mappings={ - "hmi": "false" + "hmi": "false", + "EE": "true", + "EE_name": "robotiq_2f85" }) robot_description = { From 3f2c8f8b60faaeafb9bb30dbd48c43ba04ca3e90 Mon Sep 17 00:00:00 2001 From: jepear19 Date: Fri, 27 Mar 2026 01:43:25 +0100 Subject: [PATCH 27/32] Fix robot model --- .../urdf/ur5_robotiq_2f85_with_cams.urdf.xacro | 13 ++++++++++--- 1 file changed, 10 insertions(+), 3 deletions(-) diff --git a/Industrial/ur5_gripper_description/urdf/ur5_robotiq_2f85_with_cams.urdf.xacro b/Industrial/ur5_gripper_description/urdf/ur5_robotiq_2f85_with_cams.urdf.xacro index 0acba3a81..08b498f2d 100644 --- a/Industrial/ur5_gripper_description/urdf/ur5_robotiq_2f85_with_cams.urdf.xacro +++ b/Industrial/ur5_gripper_description/urdf/ur5_robotiq_2f85_with_cams.urdf.xacro @@ -5,6 +5,10 @@ + + + + @@ -18,11 +22,12 @@ + + @@ -67,12 +73,13 @@ + gz_ros2_control/GazeboSimSystem $(find ros2srrc_robots)/ur5/config/controller.yaml - $(find ros2srrc_endeffectors)/${EE_name}/config/controller.yaml + $(find ros2srrc_endeffectors)/$(arg EE_name)/config/controller.yaml From 1cf06a2164ea14eb779ba07673cceae742b2e2ae Mon Sep 17 00:00:00 2001 From: jepear19 Date: Fri, 27 Mar 2026 02:06:30 +0100 Subject: [PATCH 28/32] Fix world.launch.py --- .../machine_vision_harmonic/world.launch.py | 67 ++++++++++++++++--- 1 file changed, 58 insertions(+), 9 deletions(-) diff --git a/Launchers/machine_vision_harmonic/world.launch.py b/Launchers/machine_vision_harmonic/world.launch.py index 0b95eb014..fad9933fc 100644 --- a/Launchers/machine_vision_harmonic/world.launch.py +++ b/Launchers/machine_vision_harmonic/world.launch.py @@ -1,37 +1,86 @@ import os from launch import LaunchDescription -from launch.actions import ExecuteProcess +from launch.actions import ( + ExecuteProcess, + SetEnvironmentVariable, + AppendEnvironmentVariable, +) +from ament_index_python.packages import get_package_share_directory +from launch_ros.actions import Node def generate_launch_description(): + package_dir = get_package_share_directory("custom_robots") + robotiq_description_pkg = get_package_share_directory("robotiq_description") + ur5_gripper_pkg = get_package_share_directory("ur5_gripper_description") + + robotiq_pkg_share_dir = robotiq_description_pkg + warehouse_models_path = os.path.join(robotiq_pkg_share_dir, "world", "models") + + ur5_share_parent = os.path.dirname(ur5_gripper_pkg) + robotiq_share_parent = os.path.dirname(robotiq_pkg_share_dir) + world_path = "/opt/jderobot/Worlds/machine_vision_harmonic.world" + gazebo_models_path = os.path.join(package_dir, "models") + gz_ros2_control_install = "/home/ws/install" gz_lib_path = os.path.join(gz_ros2_control_install, "gz_ros2_control", "lib") + resource_path = ( + ur5_share_parent + ":" + + robotiq_share_parent + ":" + + warehouse_models_path + ) + gz_env = { + "GZ_SIM_RESOURCE_PATH": resource_path, "GZ_SIM_SYSTEM_PLUGIN_PATH": ( - "/home/ws/install/gz_link_attacher/lib:" + "/usr/lib/x86_64-linux-gnu/gz-sim-8/plugins:" + "/usr/lib/x86_64-linux-gnu/gz-sim-8/systems:" + gz_lib_path + ":/opt/ros/humble/lib" ), "LD_LIBRARY_PATH": ( - "/home/ws/install/gz_link_attacher/lib:" + "/usr/lib/x86_64-linux-gnu:" + gz_lib_path + - ":/opt/ros/humble/lib:/usr/lib/x86_64-linux-gnu:" - + os.environ.get("LD_LIBRARY_PATH", "") + ":/opt/ros/humble/lib:/usr/lib/x86_64-linux-gnu" ), "DISPLAY": os.environ.get("DISPLAY", ":0"), } gazebo = ExecuteProcess( - cmd=["gz", "sim", "-r", "-v", "4", world_path], + cmd=["gz", "sim", "-s", "-r", "-v", "4", world_path], output="screen", additional_env=gz_env, shell=False, ) - return LaunchDescription([ - gazebo - ]) \ No newline at end of file + world_entity_cmd = Node( + package="ros_gz_sim", + executable="create", + arguments=["-name", "world", "-file", world_path], + output="screen", + ) + + ld = LaunchDescription() + + ld.add_action( + SetEnvironmentVariable( + "GZ_SIM_RESOURCE_PATH", + gazebo_models_path + ":" + ur5_gripper_pkg + ":" + robotiq_description_pkg, + ) + ) + + ld.add_action( + AppendEnvironmentVariable( + "GZ_SIM_RESOURCE_PATH", + gazebo_models_path + ":" + ur5_gripper_pkg + ":" + robotiq_description_pkg, + ) + ) + + ld.add_action(gazebo) + ld.add_action(world_entity_cmd) + + return ld \ No newline at end of file From f08d3ea90a191a6dc56319fafc88286fd8c1c101 Mon Sep 17 00:00:00 2001 From: jepear19 Date: Fri, 27 Mar 2026 02:41:19 +0100 Subject: [PATCH 29/32] Fix world.launch.py --- Launchers/machine_vision_harmonic/world.launch.py | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/Launchers/machine_vision_harmonic/world.launch.py b/Launchers/machine_vision_harmonic/world.launch.py index fad9933fc..3fc41fcac 100644 --- a/Launchers/machine_vision_harmonic/world.launch.py +++ b/Launchers/machine_vision_harmonic/world.launch.py @@ -28,10 +28,13 @@ def generate_launch_description(): gz_ros2_control_install = "/home/ws/install" gz_lib_path = os.path.join(gz_ros2_control_install, "gz_ros2_control", "lib") + custom_models_path = "/home/dev_ws/src/IndustrialRobots/ros2_SimRealRobotControl/packages/ur5/ros2srrc_ur5_gazebo/models" + resource_path = ( ur5_share_parent + ":" + robotiq_share_parent + ":" + - warehouse_models_path + warehouse_models_path + ":" + + custom_models_path ) gz_env = { From 4043d8f3a5d7d6308db7802b6086f094b47ce613 Mon Sep 17 00:00:00 2001 From: jepear19 Date: Sat, 28 Mar 2026 09:52:36 +0100 Subject: [PATCH 30/32] Fix launchers --- Launchers/machine_vision_harmonic/robot.launch.py | 2 +- Launchers/machine_vision_harmonic/world.launch.py | 3 ++- 2 files changed, 3 insertions(+), 2 deletions(-) diff --git a/Launchers/machine_vision_harmonic/robot.launch.py b/Launchers/machine_vision_harmonic/robot.launch.py index e27638fbe..ec2d4b316 100644 --- a/Launchers/machine_vision_harmonic/robot.launch.py +++ b/Launchers/machine_vision_harmonic/robot.launch.py @@ -52,7 +52,7 @@ def generate_launch_description(): "-topic", "robot_description", "-x", "0", "-y", "0", - "-z", "0.01", + "-z", "0.9", ], output="screen", ) diff --git a/Launchers/machine_vision_harmonic/world.launch.py b/Launchers/machine_vision_harmonic/world.launch.py index 3fc41fcac..7207bd0fc 100644 --- a/Launchers/machine_vision_harmonic/world.launch.py +++ b/Launchers/machine_vision_harmonic/world.launch.py @@ -34,7 +34,8 @@ def generate_launch_description(): ur5_share_parent + ":" + robotiq_share_parent + ":" + warehouse_models_path + ":" + - custom_models_path + custom_models_path + ":" + + get_package_share_directory("ros2srrc_ur5_gazebo") ) gz_env = { From 2386387a1c3c578b757609fa15f99bcc37fa302d Mon Sep 17 00:00:00 2001 From: jepear19 Date: Sat, 28 Mar 2026 10:35:22 +0100 Subject: [PATCH 31/32] Add meshes, fix renders and change arm position --- .../ur5_robotiq_2f85_with_cams.urdf.xacro | 137 ++++++++++++------ .../machine_vision_harmonic/robot.launch.py | 62 +++++--- .../machine_vision_harmonic/world.launch.py | 22 +-- Worlds/machine_vision_harmonic.world | 14 +- 4 files changed, 149 insertions(+), 86 deletions(-) diff --git a/Industrial/ur5_gripper_description/urdf/ur5_robotiq_2f85_with_cams.urdf.xacro b/Industrial/ur5_gripper_description/urdf/ur5_robotiq_2f85_with_cams.urdf.xacro index 08b498f2d..1c4079b16 100644 --- a/Industrial/ur5_gripper_description/urdf/ur5_robotiq_2f85_with_cams.urdf.xacro +++ b/Industrial/ur5_gripper_description/urdf/ur5_robotiq_2f85_with_cams.urdf.xacro @@ -1,46 +1,37 @@ - + - - - + - - + + + - - - - - - - - - - - - - - - - - - + + + + + + + + + + + + - + + + @@ -60,6 +51,8 @@ + + @@ -72,15 +65,77 @@ + + + + + + + 0 0 0 1 + + + + + + + + - - - gz_ros2_control/GazeboSimSystem + + + true + 30 + + 1.047 + + 640 + 480 + R8G8B8 + + + 0.1 + 100 + + + + - $(find ros2srrc_robots)/ur5/config/controller.yaml - $(find ros2srrc_endeffectors)/$(arg EE_name)/config/controller.yaml - + + + + + + + 0 0 0 1 + + + + + + + + + + + + + true + 30 + + 1.047 + + 640 + 480 + R8G8B8 + + + + + \ No newline at end of file diff --git a/Launchers/machine_vision_harmonic/robot.launch.py b/Launchers/machine_vision_harmonic/robot.launch.py index ec2d4b316..4909a208f 100644 --- a/Launchers/machine_vision_harmonic/robot.launch.py +++ b/Launchers/machine_vision_harmonic/robot.launch.py @@ -3,7 +3,7 @@ from launch import LaunchDescription from launch_ros.actions import Node -from launch.actions import RegisterEventHandler +from launch.actions import RegisterEventHandler, TimerAction from launch.event_handlers import OnProcessExit from ament_index_python.packages import get_package_share_directory @@ -15,19 +15,17 @@ def generate_launch_description(): xacro_file = os.path.join( pkg_path, "urdf", - "ur5_robotiq_2f85_with_cams.urdf.xacro" + "ur5_machine_vision.urdf.xacro" ) - doc = xacro.parse(open(xacro_file)) - xacro.process_doc(doc, mappings={ - "hmi": "false", - "EE": "true", - "EE_name": "robotiq_2f85" - }) + robot_description_content = xacro.process_file( + xacro_file, + mappings={ + "sim_gz": "true" + } + ).toxml() - robot_description = { - "robot_description": doc.toxml() - } + robot_description = {"robot_description": robot_description_content} robot_state_publisher = Node( package="robot_state_publisher", @@ -36,20 +34,12 @@ def generate_launch_description(): parameters=[robot_description, {"use_sim_time": True}], ) - clock_bridge = Node( - package="ros_gz_bridge", - executable="parameter_bridge", - arguments=["/clock@rosgraph_msgs/msg/Clock[gz.msgs.Clock"], - parameters=[{"use_sim_time": True}], - output="screen", - ) - spawn_entity = Node( package="ros_gz_sim", executable="create", arguments=[ - "-name", "ur5", "-topic", "robot_description", + "-name", "ur5", "-x", "0", "-y", "0", "-z", "0.9", @@ -57,6 +47,19 @@ def generate_launch_description(): output="screen", ) + static_tf = Node( + package="tf2_ros", + executable="static_transform_publisher", + arguments=["0", "0", "0.9", "0", "0", "0", "world", "base_link"], + ) + + clock_bridge = Node( + package="ros_gz_bridge", + executable="parameter_bridge", + arguments=["/clock@rosgraph_msgs/msg/Clock[gz.msgs.Clock"], + parameters=[{"use_sim_time": True}], + ) + joint_state_broadcaster = Node( package="controller_manager", executable="spawner", @@ -69,16 +72,27 @@ def generate_launch_description(): arguments=["joint_trajectory_controller"], ) - delay_controllers = RegisterEventHandler( + delay_spawn = TimerAction(period=3.0, actions=[spawn_entity]) + + delay_js = RegisterEventHandler( OnProcessExit( target_action=spawn_entity, - on_exit=[joint_state_broadcaster, joint_trajectory_controller], + on_exit=[joint_state_broadcaster], + ) + ) + + delay_traj = RegisterEventHandler( + OnProcessExit( + target_action=joint_state_broadcaster, + on_exit=[joint_trajectory_controller], ) ) return LaunchDescription([ robot_state_publisher, + static_tf, clock_bridge, - spawn_entity, - delay_controllers, + delay_spawn, + delay_js, + delay_traj, ]) \ No newline at end of file diff --git a/Launchers/machine_vision_harmonic/world.launch.py b/Launchers/machine_vision_harmonic/world.launch.py index 7207bd0fc..776c25461 100644 --- a/Launchers/machine_vision_harmonic/world.launch.py +++ b/Launchers/machine_vision_harmonic/world.launch.py @@ -16,11 +16,14 @@ def generate_launch_description(): ur5_gripper_pkg = get_package_share_directory("ur5_gripper_description") robotiq_pkg_share_dir = robotiq_description_pkg + warehouse_models_path = os.path.join(robotiq_pkg_share_dir, "world", "models") ur5_share_parent = os.path.dirname(ur5_gripper_pkg) robotiq_share_parent = os.path.dirname(robotiq_pkg_share_dir) + custom_models_path = "/home/dev_ws/src/IndustrialRobots/ros2_SimRealRobotControl/packages/ur5/ros2srrc_ur5_gazebo/models" + world_path = "/opt/jderobot/Worlds/machine_vision_harmonic.world" gazebo_models_path = os.path.join(package_dir, "models") @@ -28,29 +31,18 @@ def generate_launch_description(): gz_ros2_control_install = "/home/ws/install" gz_lib_path = os.path.join(gz_ros2_control_install, "gz_ros2_control", "lib") - custom_models_path = "/home/dev_ws/src/IndustrialRobots/ros2_SimRealRobotControl/packages/ur5/ros2srrc_ur5_gazebo/models" - resource_path = ( ur5_share_parent + ":" + robotiq_share_parent + ":" + warehouse_models_path + ":" + - custom_models_path + ":" + - get_package_share_directory("ros2srrc_ur5_gazebo") + custom_models_path ) gz_env = { "GZ_SIM_RESOURCE_PATH": resource_path, - "GZ_SIM_SYSTEM_PLUGIN_PATH": ( - "/usr/lib/x86_64-linux-gnu/gz-sim-8/plugins:" - "/usr/lib/x86_64-linux-gnu/gz-sim-8/systems:" - + gz_lib_path + - ":/opt/ros/humble/lib" - ), - "LD_LIBRARY_PATH": ( - "/usr/lib/x86_64-linux-gnu:" - + gz_lib_path + - ":/opt/ros/humble/lib:/usr/lib/x86_64-linux-gnu" - ), + "GZ_SIM_SYSTEM_PLUGIN_PATH": gz_lib_path + ":/opt/ros/humble/lib", + "LD_LIBRARY_PATH": gz_lib_path + + ":/opt/ros/humble/lib:/usr/lib/x86_64-linux-gnu", "DISPLAY": os.environ.get("DISPLAY", ":0"), } diff --git a/Worlds/machine_vision_harmonic.world b/Worlds/machine_vision_harmonic.world index b0acf5d2c..edd375ca4 100644 --- a/Worlds/machine_vision_harmonic.world +++ b/Worlds/machine_vision_harmonic.world @@ -1,5 +1,13 @@ + + + + + ogre2 + + + 1 0 0 10 0 -0 0 @@ -1497,12 +1505,6 @@ - - ogre2 - - - From d75aac2c6b7a280a18d5de7480f804540b9840fe Mon Sep 17 00:00:00 2001 From: jepear19 Date: Sat, 28 Mar 2026 11:13:58 +0100 Subject: [PATCH 32/32] Fix robot launcher --- .../machine_vision_harmonic/robot.launch.py | 90 +++++++++++++++++-- 1 file changed, 81 insertions(+), 9 deletions(-) diff --git a/Launchers/machine_vision_harmonic/robot.launch.py b/Launchers/machine_vision_harmonic/robot.launch.py index 4909a208f..586a7ff73 100644 --- a/Launchers/machine_vision_harmonic/robot.launch.py +++ b/Launchers/machine_vision_harmonic/robot.launch.py @@ -2,27 +2,65 @@ import xacro from launch import LaunchDescription -from launch_ros.actions import Node -from launch.actions import RegisterEventHandler, TimerAction +from launch.actions import ExecuteProcess, RegisterEventHandler, TimerAction from launch.event_handlers import OnProcessExit +from launch_ros.actions import Node from ament_index_python.packages import get_package_share_directory def generate_launch_description(): - pkg_path = get_package_share_directory("ur5_gripper_description") + pkg_share_dir = get_package_share_directory("ur5_gripper_description") + robotiq_pkg_share_dir = get_package_share_directory("robotiq_description") + + gz_ros2_control_install = "/home/ws/install" + gz_lib_path = os.path.join(gz_ros2_control_install, "gz_ros2_control", "lib") + + warehouse_models_path = os.path.join(robotiq_pkg_share_dir, "world", "models") + ur5_share_parent = os.path.dirname(pkg_share_dir) + robotiq_share_parent = os.path.dirname(robotiq_pkg_share_dir) + resource_path = ( + ur5_share_parent + ":" + + robotiq_share_parent + ":" + + warehouse_models_path + ) + + gz_env = { + "GZ_SIM_RESOURCE_PATH": resource_path, + "GZ_SIM_SYSTEM_PLUGIN_PATH": ( + "/home/ws/install/gz_link_attacher/lib:" + + gz_lib_path + + ":/opt/ros/humble/lib" + ), + "LD_LIBRARY_PATH": + "/home/ws/install/gz_link_attacher/lib:" + + gz_lib_path + + ":/opt/ros/humble/lib:/usr/lib/x86_64-linux-gnu:" + + os.environ.get("LD_LIBRARY_PATH", ""), + "DISPLAY": os.environ.get("DISPLAY", ":0"), + } + + print("DEBUG: GZ_SIM_RESOURCE_PATH =", resource_path) xacro_file = os.path.join( - pkg_path, + pkg_share_dir, "urdf", "ur5_machine_vision.urdf.xacro" ) + controllers_file = os.path.join(pkg_share_dir, "config", "ur5_controllers.yaml") + robot_description_content = xacro.process_file( xacro_file, mappings={ - "sim_gz": "true" - } + "ur_type": "ur5", + "name": "ur", + "prefix": "", + "use_fake_hardware": "false", + "sim_gazebo": "false", + "sim_gz": "true", + "simulation_controllers": controllers_file, + }, ).toxml() robot_description = {"robot_description": robot_description_content} @@ -40,17 +78,23 @@ def generate_launch_description(): arguments=[ "-topic", "robot_description", "-name", "ur5", + "-allow_renaming", "true", "-x", "0", "-y", "0", "-z", "0.9", + "-R", "0", + "-P", "0", + "-Y", "0", ], output="screen", + additional_env=gz_env, ) static_tf = Node( package="tf2_ros", executable="static_transform_publisher", arguments=["0", "0", "0.9", "0", "0", "0", "world", "base_link"], + parameters=[{"use_sim_time": True}], ) clock_bridge = Node( @@ -58,21 +102,41 @@ def generate_launch_description(): executable="parameter_bridge", arguments=["/clock@rosgraph_msgs/msg/Clock[gz.msgs.Clock"], parameters=[{"use_sim_time": True}], + output="screen", ) joint_state_broadcaster = Node( package="controller_manager", executable="spawner", - arguments=["joint_state_broadcaster"], + arguments=[ + "joint_state_broadcaster", + "--controller-manager", + "/controller_manager", + ], + parameters=[{"use_sim_time": True}], ) joint_trajectory_controller = Node( package="controller_manager", executable="spawner", - arguments=["joint_trajectory_controller"], + arguments=[ + "joint_trajectory_controller", + "-c", "/controller_manager", + ], + parameters=[{"use_sim_time": True}], + ) + + gripper_controller = Node( + package="controller_manager", + executable="spawner", + arguments=[ + "gripper_controller", + "-c", "/controller_manager", + ], + parameters=[{"use_sim_time": True}], ) - delay_spawn = TimerAction(period=3.0, actions=[spawn_entity]) + delay_spawn = TimerAction(period=5.0, actions=[spawn_entity]) delay_js = RegisterEventHandler( OnProcessExit( @@ -88,6 +152,13 @@ def generate_launch_description(): ) ) + delay_gripper = RegisterEventHandler( + OnProcessExit( + target_action=joint_trajectory_controller, + on_exit=[gripper_controller], + ) + ) + return LaunchDescription([ robot_state_publisher, static_tf, @@ -95,4 +166,5 @@ def generate_launch_description(): delay_spawn, delay_js, delay_traj, + delay_gripper, ]) \ No newline at end of file