Add Pick and Place Harmonic launchers and environment

CustomRobots/pick_place_harmonic/README.md

@@ -0,0 +1,23 @@
+# Pick Place Harmonic - CustomRobots Directory
+
+This directory contains resources for the Pick and Place Harmonic exercise using Gazebo Harmonic.
+
+## Structure
+
+- **launch/**: Launch files for the exercise
+- **models/**: Symbolic link to warehouse models from robotiq_description
+- **urdf/**: Symbolic link to URDF files from ur5_gripper_description
+
+## Main Launch File
+
+The main launcher is located in `/RoboticsInfrastructure/Launchers/pick_place_harmonic.launch.py`
+
+This launcher wraps the `spawn_robot_warehouse.launch.py` from the `ur5_gripper_description` package,
+which is part of the `pick_place_harmonic_exercise` in IndustrialRobots.
+
+## Package Dependencies
+
+The exercise depends on packages from:
+`/home/dev_ws/src/IndustrialRobots/pick_place_harmonic_exercise/`
+
+These must be built with colcon before the exercise can run.

CustomRobots/pick_place_harmonic/models_robotiq

@@ -0,0 +1 @@
+/home/anish/jde2/IndustrialRobots/pick_place_harmonic_exercise/robotiq_description/world

CustomRobots/pick_place_harmonic/urdf/ur5_gripper_urdf

@@ -0,0 +1 @@
+/home/anish/jde2/IndustrialRobots/pick_place_harmonic_exercise/ur5_gripper_description/urdf

Launchers/pick_place_harmonic.launch.py

@@ -0,0 +1,122 @@
+#!/usr/bin/env python3
+"""
+Pick Place Harmonic - Gazebo World Launcher
+Launches ONLY: Gazebo Harmonic + UR5 Robot + Controllers
+MoveIt and RViz are launched separately by the Academy
+"""
+
+import os
+from launch import LaunchDescription
+from launch.actions import ExecuteProcess, TimerAction
+from launch_ros.actions import Node
+from ament_index_python.packages import get_package_share_directory
+import xacro
+
+
+def generate_launch_description():
+    # Get package directories
+    pkg_share_dir = get_package_share_directory("ur5_gripper_description")
+    robotiq_pkg_share_dir = get_package_share_directory("robotiq_description")
+
+    # Workspace for gz_ros2_control
+    workspace_dir = "/home/dev_ws"
+    gz_lib_path = os.path.join(workspace_dir, 'install', 'gz_ros2_control', 'lib')
+
+    # Set environment variables for Gazebo Harmonic
+    gz_env = {
+        'GZ_SIM_SYSTEM_PLUGIN_PATH': f"{gz_lib_path}:{os.environ.get('GZ_SIM_SYSTEM_PLUGIN_PATH', '')}",
+        'GZ_SIM_RESOURCE_PATH': f"{pkg_share_dir}/share:{robotiq_pkg_share_dir}/share:{os.environ.get('GZ_SIM_RESOURCE_PATH', '')}",
+        'LD_LIBRARY_PATH': f"{gz_lib_path}:{os.environ.get('LD_LIBRARY_PATH', '')}"
+    }
+
+    # Process URDF with xacro
+    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
+    robot_state_publisher = Node(
+        package="robot_state_publisher",
+        executable="robot_state_publisher",
+        output="screen",
+        parameters=[robot_description, {"use_sim_time": True}],
+    )
+
+    # Static transform (world → base_link)
+    static_tf = Node(
+        package="tf2_ros",
+        executable="static_transform_publisher",
+        name="static_transform_publisher",
+        arguments=["0", "0", "0.9", "0", "0", "0", "world", "base_link"],
+        output="screen",
+        parameters=[{"use_sim_time": True}],
+    )
+
+    # Gazebo Sim
+    world_file = os.path.join(robotiq_pkg_share_dir, 'world', 'warehouse_arm_harmonic.world')
+    gz_cmd = (
+        f'export GZ_SIM_SYSTEM_PLUGIN_PATH="{gz_env["GZ_SIM_SYSTEM_PLUGIN_PATH"]}" && '
+        f'export GZ_SIM_RESOURCE_PATH="{gz_env["GZ_SIM_RESOURCE_PATH"]}" && '
+        f'export LD_LIBRARY_PATH="{gz_env["LD_LIBRARY_PATH"]}" && '
+        f'gz sim -r -v 4 "{world_file}"'
+    )
+
+    gazebo = ExecuteProcess(
+        cmd=['bash', '-c', gz_cmd],
+        output='screen',
+        shell=False
+    )
+
+    # Spawn robot
+    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.9",
+            "-R", "0.0", "-P", "0.0", "-Y", "0.0"
+        ],
+        output="screen",
+    )
+
+    # Clock bridge
+    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}],
+    )
+
+    # Spawn controllers via background script (more reliable than launch system)
+    controller_script = os.path.join(os.path.dirname(__file__), 'spawn_controllers.sh')
+    spawn_controllers = ExecuteProcess(
+        cmd=['bash', controller_script],
+        output='screen',
+        shell=False
+    )
+
+    return LaunchDescription([
+        gazebo,
+        robot_state_publisher,
+        spawn_entity,
+        static_tf,
+        gz_ros2_bridge_clock,
+        spawn_controllers,  # This will run in background and spawn controllers with delays
+    ])

Launchers/pick_place_harmonic_rviz.launch.py

@@ -0,0 +1,132 @@
+#!/usr/bin/env python3
+"""
+Pick Place Harmonic - RViz + MoveIt Launcher
+Launches ONLY: MoveIt move_group + RViz with motion planning
+Assumes Gazebo and robot are already running
+"""
+
+import os
+from launch import LaunchDescription
+from launch_ros.actions import Node
+from launch.actions import TimerAction
+from ament_index_python.packages import get_package_share_directory
+
+
+def generate_launch_description():
+    # Get package directories
+    pkg_share_dir = get_package_share_directory("ur5_gripper_description")
+    moveit_config_package = "ur5_gripper_moveit_config"
+    moveit_pkg_share = get_package_share_directory(moveit_config_package)
+
+    # Robot description (must match what's in Gazebo)
+    import xacro
+    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}
+
+    # SRDF
+    srdf_file = os.path.join(moveit_pkg_share, "srdf", "ur5_robotiq.srdf")
+    with open(srdf_file, 'r') as file:
+        robot_description_semantic = {"robot_description_semantic": file.read()}
+
+    # Kinematics
+    kinematics_yaml = os.path.join(moveit_pkg_share, "config", "kinematics.yaml")
+
+    # OMPL planning
+    ompl_planning_yaml = os.path.join(moveit_pkg_share, "config", "ompl_planning.yaml")
+
+    # MoveIt controllers
+    moveit_controllers = os.path.join(moveit_pkg_share, "config", "controllers.yaml")
+
+    # Planning pipeline
+    planning_pipelines_config = {
+        "planning_pipelines": ["ompl"],
+        "default_planning_pipeline": "ompl",
+        "ompl": {
+            "planning_plugin": "ompl_interface/OMPLPlanner",
+            "request_adapters": "default_planner_request_adapters/AddTimeOptimalParameterization default_planner_request_adapters/ResolveConstraintFrames default_planner_request_adapters/FixWorkspaceBounds default_planner_request_adapters/FixStartStateBounds default_planner_request_adapters/FixStartStateCollision default_planner_request_adapters/FixStartStatePathConstraints",
+            "start_state_max_bounds_error": 0.1,
+        }
+    }
+
+    # Trajectory execution
+    trajectory_execution = {
+        "moveit_manage_controllers": True,
+        "trajectory_execution.allowed_execution_duration_scaling": 1.2,
+        "trajectory_execution.allowed_goal_duration_margin": 0.5,
+        "trajectory_execution.allowed_start_tolerance": 0.01,
+    }
+
+    planning_scene_monitor_parameters = {
+        "publish_planning_scene": True,
+        "publish_geometry_updates": True,
+        "publish_state_updates": True,
+        "publish_transforms_updates": True,
+    }
+
+    # MoveIt move_group node
+    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,
+            planning_pipelines_config,
+            trajectory_execution,
+            moveit_controllers,
+            planning_scene_monitor_parameters,
+            {"use_sim_time": True},
+        ],
+    )
+
+    # RViz with MoveIt configuration
+    rviz_config_file = os.path.join(moveit_pkg_share, "rviz", "moveit.rviz")
+
+    rviz_node = Node(
+        package="rviz2",
+        executable="rviz2",
+        name="rviz2",
+        output="log",
+        arguments=["-d", rviz_config_file],
+        parameters=[
+            robot_description,
+            robot_description_semantic,
+            ompl_planning_yaml,
+            kinematics_yaml,
+            {"use_sim_time": True},
+        ],
+    )
+
+    # Delay MoveIt slightly to let controllers stabilize
+    delay_move_group = TimerAction(
+        period=2.0,
+        actions=[move_group_node],
+    )
+
+    # Delay RViz after MoveIt
+    delay_rviz = TimerAction(
+        period=3.0,
+        actions=[rviz_node],
+    )
+
+    return LaunchDescription([
+        delay_move_group,
+        delay_rviz,
+    ])

database/universes.sql

@@ -178,6 +178,7 @@ COPY public.universes (id, name, world_id, robot_id) FROM stdin;
 51 	Montmelo Circuit Classic	51	0
 52 	Montreal Circuit Classic	52	0
 53 	Nurburgring Circuit Classic	53	0
+55	Pick And Place Harmonic World	55	0
 54 	Vacuums House Roof Classic	54	0
 \.
 
@@ -242,6 +243,7 @@ COPY public.worlds (id, name, launch_file_path, tools_config, ros_version, type,
 51	Montmelo Circuit Classic	/opt/jderobot/Launchers/montmelo_circuit_classic.launch.py	None	ROS2	gazebo	{0.0,0.0,0.0,0.0,0.0,0.0}
 52	Montreal Circuit Classic	/opt/jderobot/Launchers/montreal_circuit_classic.launch.py	None	ROS2	gazebo	{0.0,0.0,0.0,0.0,0.0,0.0}
 53	Nurburgring Circuit Classic	/opt/jderobot/Launchers/nurburgring_circuit_classic.launch.py	None	ROS2	gazebo	{0.0,0.0,0.0,0.0,0.0,0.0}
+55	Pick And Place Harmonic	/opt/jderobot/Launchers/pick_place_harmonic.launch.py	{"rviz":"/opt/jderobot/Launchers/pick_place_harmonic_rviz.launch.py"}	ROS2	gz	{0.0,0.0,0.0,0.0,0.0,0.0}
 54	Vacuums House Roof Classic	/opt/jderobot/Launchers/montecarlo_visual_loc_classic.launch.py	None	ROS2	gazebo	{0.0,0.0,0.0,0.0,0.0,0.0}
 \.
 

scripts/.env

@@ -23,3 +23,17 @@ export LD_LIBRARY_PATH=$LD_LIBRARY_PATH:/usr/lib/x86_64-linux-gnu/gazebo-11/plug
 
 # Default rendering display
 export DISPLAY=:0
+
+# === Pick Place Harmonic - Gazebo Harmonic Environment === #
+export GZ_VERSION=harmonic
+
+# gz_ros2_control plugin paths - CRITICAL for controller loading
+export GZ_SIM_SYSTEM_PLUGIN_PATH="/home/dev_ws/install/gz_ros2_control/lib:/home/dev_ws/install/ign_ros2_control/lib:${GZ_SIM_SYSTEM_PLUGIN_PATH}"
+
+# Gazebo Harmonic resource paths for robot models
+# CRITICAL: Use BOTH install/share (parent of package name) AND source directories
+# Source directories are needed because that's where meshes actually are during development
+export GZ_SIM_RESOURCE_PATH="/home/dev_ws/install/ur5_gripper_description/share:/home/dev_ws/install/robotiq_description/share:/home/dev_ws/src/IndustrialRobots/pick_place_harmonic_exercise/ur5_gripper_description:/home/dev_ws/src/IndustrialRobots/pick_place_harmonic_exercise/robotiq_description:${GZ_SIM_RESOURCE_PATH}"
+
+# Library paths for gz_ros2_control
+export LD_LIBRARY_PATH="/home/dev_ws/install/gz_ros2_control/lib:/home/dev_ws/install/ign_ros2_control/lib:${LD_LIBRARY_PATH}"