Migrate machine vision to gazebo harmonic

Industrial/ur5_gripper_description/urdf/ur5_robotiq_2f85_with_cams.urdf.xacro

@@ -0,0 +1,86 @@
+<?xml version="1.0"?>
+<robot name="ur5" xmlns:xacro="http://ros.org/wiki/xacro">
+
+  <xacro:arg name="hmi" default="false"/>
+  <xacro:arg name="EE" default="true"/>
+  <xacro:arg name="EE_name" default="robotiq_2f85"/>
+
+  <xacro:property name="EE" value="$(arg EE)"/>
+  <xacro:property name="EE_name" value="$(arg EE_name)"/>
+
+
+  <xacro:include filename="$(find ros2srrc_robots)/ur5/urdf/ur5_macro.urdf.xacro"/>
+
+  <xacro:include filename="$(find ros2srrc_ur5_gazebo)/urdf/common_properties.xacro"/>	
+
+  <xacro:include filename="$(find ros2srrc_ur5_gazebo)/urdf/cam_on_hand_params.xacro"/>	
+  <xacro:include filename="$(find ros2srrc_ur5_gazebo)/urdf/cam_on_hand.xacro"/>	
+
+  <xacro:include filename="$(find ros2srrc_ur5_gazebo)/urdf/cam_on_base_params.xacro"/>	
+  <xacro:include filename="$(find ros2srrc_ur5_gazebo)/urdf/cam_on_base.xacro"/>	
+
+  <xacro:include filename="$(find ros2srrc_endeffectors)/robotiq_2f85/urdf/robotiq_2f85_macro.urdf.xacro"/>	
+
+
+
+  <xacro:ur5 
+    bringup="false"
+    robot_ip="0.0.0.0"
+    EE="$(arg EE)"
+    EE_name="$(arg EE_name)"
+    script_filename="none"
+    input_recipe_filename="none"
+    output_recipe_filename="none"
+  />
+
+  <xacro:robotiq_2f85 
+    bringup="false"
+    parent_link="tool0"
+  />
+
+
+
+  <link name="world"/>
+
+  <link name="robot_stand">
+    <visual>
+      <geometry>
+        <box size="0.2 0.2 1.0"/>
+      </geometry>
+    </visual>
+    <collision>
+      <geometry>
+        <box size="0.2 0.2 1.0"/>
+      </geometry>
+    </collision>
+    <inertial>
+      <mass value="100"/>
+      <inertia ixx="0.01" iyy="0.01" izz="0.01"
+               ixy="0" ixz="0" iyz="0"/>
+    </inertial>
+  </link>
+
+  <joint name="world_joint" type="fixed">
+    <parent link="world"/>
+    <child link="robot_stand"/>
+    <origin xyz="0 0 0.5"/>
+  </joint>
+
+  <joint name="stand_joint" type="fixed">
+    <parent link="robot_stand"/>
+    <child link="base_link"/>
+    <origin xyz="0 0 0.52"/>
+  </joint>
+
+
+
+  <gazebo>
+    <plugin filename="gz_ros2_control-system" name="gz_ros2_control">
+      <robot_sim_type>gz_ros2_control/GazeboSimSystem</robot_sim_type>
+
+      <parameters>$(find ros2srrc_robots)/ur5/config/controller.yaml</parameters>
+      <parameters>$(find ros2srrc_endeffectors)/$(arg EE_name)/config/controller.yaml</parameters>
+    </plugin>
+  </gazebo>
+
+</robot>

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
+    ])

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,
+    ])

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