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..08b498f2d --- /dev/null +++ b/Industrial/ur5_gripper_description/urdf/ur5_robotiq_2f85_with_cams.urdf.xacro @@ -0,0 +1,86 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + gz_ros2_control/GazeboSimSystem + + $(find ros2srrc_robots)/ur5/config/controller.yaml + $(find ros2srrc_endeffectors)/$(arg 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 new file mode 100644 index 000000000..d8a3b399e --- /dev/null +++ b/Launchers/machine_vision_harmonic/machine_vision_harmonic.launch.py @@ -0,0 +1,22 @@ +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..e27638fbe --- /dev/null +++ b/Launchers/machine_vision_harmonic/robot.launch.py @@ -0,0 +1,84 @@ +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(): + + pkg_path = get_package_share_directory("ur5_gripper_description") + + 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, mappings={ + "hmi": "false", + "EE": "true", + "EE_name": "robotiq_2f85" + }) + + 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}], + ) + + 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", + "-x", "0", + "-y", "0", + "-z", "0.01", + ], + 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"], + ) + + delay_controllers = RegisterEventHandler( + OnProcessExit( + target_action=spawn_entity, + on_exit=[joint_state_broadcaster, joint_trajectory_controller], + ) + ) + + return LaunchDescription([ + robot_state_publisher, + 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 new file mode 100644 index 000000000..3fc41fcac --- /dev/null +++ b/Launchers/machine_vision_harmonic/world.launch.py @@ -0,0 +1,89 @@ +import os +from launch import LaunchDescription +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") + + 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 + ) + + 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" + ), + "DISPLAY": os.environ.get("DISPLAY", ":0"), + } + + gazebo = ExecuteProcess( + 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", + ) + + 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 diff --git a/Worlds/machine_vision_harmonic.world b/Worlds/machine_vision_harmonic.world new file mode 100644 index 000000000..b0acf5d2c --- /dev/null +++ b/Worlds/machine_vision_harmonic.world @@ -0,0 +1,1514 @@ + + + + 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 + + + 1 + + + + + 0 0 1 + 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 + + + + 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 + + + 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 + + + + + + 1 + 1 + 0.6 0 0 0 0 1.57 + + + 1 + + + 0 0 0.001 0 -0 0 + + + 1 1 1 + model://ground_logo_big/meshes/ground_logo.dae + + + + 0 + 0 + 0 + + -0.5 0 0 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 + + + + + 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 + -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 + + + + + 0.4 0.04 0.5 + + + 10 + + + + + + + + + + + + + + + + + 0.4 0.04 0.5 + + + + + + + 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.08 + 0.4 + + + 10 + + + + + + + + + + + + + + + + + 0.08 + 0.04 + + + + + + + 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 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 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 + + + + + + + + + + + + + + + 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.02 + 0.06 + + + + + + + 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.03 + 0.07 + + + + + + + 0 + 0 + 0 + + 0.65 -0.25 1.01 0 -0 0 + + + + + ogre2 + + + + + + + + + + + \ No newline at end of file diff --git a/database/universes.sql b/database/universes.sql index 821cd3f35..981862017 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 0 \. -- @@ -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} \. --