Skip to content

Commit 46cf160

Browse files
anishk85anishk85
committed
Add Pick and Place Harmonic launchers and database entriesFiles added:- Launchers/pick_place_harmonic.launch.py - Gazebo Harmonic world launcher- Launchers/pick_place_harmonic_rviz.launch.py - MoveIt + RViz launcher- database/universes.sql - Universe (ID 54) and world (ID 54) entries- CustomRobots/pick_place_harmonic/
1 parent b72d9c5 commit 46cf160

File tree

5 files changed

+293
-0
lines changed

5 files changed

+293
-0
lines changed

‎CustomRobots/pick_place_harmonic/README.md‎

Lines changed: 23 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,23 @@
1+
# Pick Place Harmonic - CustomRobots Directory
2+
3+
This directory contains resources for the Pick and Place Harmonic exercise using Gazebo Harmonic.
4+
5+
## Structure
6+
7+
- **launch/**: Launch files for the exercise
8+
- **models/**: Symbolic link to warehouse models from robotiq_description
9+
- **urdf/**: Symbolic link to URDF files from ur5_gripper_description
10+
11+
## Main Launch File
12+
13+
The main launcher is located in `/RoboticsInfrastructure/Launchers/pick_place_harmonic.launch.py`
14+
15+
This launcher wraps the `spawn_robot_warehouse.launch.py` from the `ur5_gripper_description` package,
16+
which is part of the `pick_place_harmonic_exercise` in IndustrialRobots.
17+
18+
## Package Dependencies
19+
20+
The exercise depends on packages from:
21+
`/home/dev_ws/src/IndustrialRobots/pick_place_harmonic_exercise/`
22+
23+
These must be built with colcon before the exercise can run.

‎Launchers/pick_place_harmonic.launch.py‎

Lines changed: 122 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,122 @@
1+
#!/usr/bin/env python3
2+
"""
3+
Pick Place Harmonic - Gazebo World Launcher
4+
Launches ONLY: Gazebo Harmonic + UR5 Robot + Controllers
5+
MoveIt and RViz are launched separately by the Academy
6+
"""
7+
8+
import os
9+
from launch import LaunchDescription
10+
from launch.actions import ExecuteProcess, TimerAction
11+
from launch_ros.actions import Node
12+
from ament_index_python.packages import get_package_share_directory
13+
import xacro
14+
15+
16+
def generate_launch_description():
17+
# Get package directories
18+
pkg_share_dir = get_package_share_directory("ur5_gripper_description")
19+
robotiq_pkg_share_dir = get_package_share_directory("robotiq_description")
20+
21+
22+
# Use native gz_ros2_control installation from ROS 2 Humble
23+
gz_lib_path = "/opt/ros/humble/lib"
24+
25+
# Set environment variables for Gazebo Harmonic
26+
gz_env = {
27+
'GZ_SIM_SYSTEM_PLUGIN_PATH': f"{gz_lib_path}:{os.environ.get('GZ_SIM_SYSTEM_PLUGIN_PATH', '')}",
28+
'GZ_SIM_RESOURCE_PATH': f"{pkg_share_dir}/share:{robotiq_pkg_share_dir}/share:{os.environ.get('GZ_SIM_RESOURCE_PATH', '')}",
29+
'LD_LIBRARY_PATH': f"{gz_lib_path}:{os.environ.get('LD_LIBRARY_PATH', '')}"
30+
}
31+
32+
# Process URDF with xacro
33+
xacro_file = os.path.join(pkg_share_dir, "urdf", "ur5_robotiq85_gripper.urdf.xacro")
34+
controllers_file = os.path.join(pkg_share_dir, "config", "ur5_controllers.yaml")
35+
36+
robot_description_content = xacro.process_file(
37+
xacro_file,
38+
mappings={
39+
"ur_type": "ur5",
40+
"name": "ur",
41+
"prefix": "",
42+
"use_fake_hardware": "false",
43+
"sim_gazebo": "false",
44+
"sim_gz": "true",
45+
"simulation_controllers": controllers_file,
46+
}
47+
).toxml()
48+
49+
robot_description = {"robot_description": robot_description_content}
50+
51+
# Robot state publisher
52+
robot_state_publisher = Node(
53+
package="robot_state_publisher",
54+
executable="robot_state_publisher",
55+
output="screen",
56+
parameters=[robot_description, {"use_sim_time": True}],
57+
)
58+
59+
# Static transform (world → base_link)
60+
static_tf = Node(
61+
package="tf2_ros",
62+
executable="static_transform_publisher",
63+
name="static_transform_publisher",
64+
arguments=["0", "0", "0.9", "0", "0", "0", "world", "base_link"],
65+
output="screen",
66+
parameters=[{"use_sim_time": True}],
67+
)
68+
69+
# Gazebo Sim
70+
world_file = os.path.join(robotiq_pkg_share_dir, 'world', 'warehouse_arm_harmonic.world')
71+
gz_cmd = (
72+
f'export GZ_SIM_SYSTEM_PLUGIN_PATH="{gz_env["GZ_SIM_SYSTEM_PLUGIN_PATH"]}" && '
73+
f'export GZ_SIM_RESOURCE_PATH="{gz_env["GZ_SIM_RESOURCE_PATH"]}" && '
74+
f'export LD_LIBRARY_PATH="{gz_env["LD_LIBRARY_PATH"]}" && '
75+
f'gz sim -r -v 4 "{world_file}"'
76+
)
77+
78+
gazebo = ExecuteProcess(
79+
cmd=['bash', '-c', gz_cmd],
80+
output='screen',
81+
shell=False
82+
)
83+
84+
# Spawn robot
85+
spawn_entity = Node(
86+
package="ros_gz_sim",
87+
executable="create",
88+
arguments=[
89+
"-topic", "robot_description",
90+
"-name", "ur5_robotiq",
91+
"-allow_renaming", "true",
92+
"-x", "0.0", "-y", "0.0", "-z", "0.9",
93+
"-R", "0.0", "-P", "0.0", "-Y", "0.0"
94+
],
95+
output="screen",
96+
)
97+
98+
# Clock bridge
99+
gz_ros2_bridge_clock = Node(
100+
package="ros_gz_bridge",
101+
executable="parameter_bridge",
102+
arguments=["/clock@rosgraph_msgs/msg/Clock[gz.msgs.Clock"],
103+
output="screen",
104+
parameters=[{"use_sim_time": True}],
105+
)
106+
107+
# Spawn controllers via background script (more reliable than launch system)
108+
controller_script = os.path.join(os.path.dirname(__file__), 'spawn_controllers.sh')
109+
spawn_controllers = ExecuteProcess(
110+
cmd=['bash', controller_script],
111+
output='screen',
112+
shell=False
113+
)
114+
115+
return LaunchDescription([
116+
gazebo,
117+
robot_state_publisher,
118+
spawn_entity,
119+
static_tf,
120+
gz_ros2_bridge_clock,
121+
spawn_controllers, # This will run in background and spawn controllers with delays
122+
])

‎Launchers/pick_place_harmonic_rviz.launch.py‎

Lines changed: 132 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,132 @@
1+
#!/usr/bin/env python3
2+
"""
3+
Pick Place Harmonic - RViz + MoveIt Launcher
4+
Launches ONLY: MoveIt move_group + RViz with motion planning
5+
Assumes Gazebo and robot are already running
6+
"""
7+
8+
import os
9+
from launch import LaunchDescription
10+
from launch_ros.actions import Node
11+
from launch.actions import TimerAction
12+
from ament_index_python.packages import get_package_share_directory
13+
14+
15+
def generate_launch_description():
16+
# Get package directories
17+
pkg_share_dir = get_package_share_directory("ur5_gripper_description")
18+
moveit_config_package = "ur5_gripper_moveit_config"
19+
moveit_pkg_share = get_package_share_directory(moveit_config_package)
20+
21+
# Robot description (must match what's in Gazebo)
22+
import xacro
23+
xacro_file = os.path.join(pkg_share_dir, "urdf", "ur5_robotiq85_gripper.urdf.xacro")
24+
controllers_file = os.path.join(pkg_share_dir, "config", "ur5_controllers.yaml")
25+
26+
robot_description_content = xacro.process_file(
27+
xacro_file,
28+
mappings={
29+
"ur_type": "ur5",
30+
"name": "ur",
31+
"prefix": "",
32+
"use_fake_hardware": "false",
33+
"sim_gazebo": "false",
34+
"sim_gz": "true",
35+
"simulation_controllers": controllers_file,
36+
}
37+
).toxml()
38+
39+
robot_description = {"robot_description": robot_description_content}
40+
41+
# SRDF
42+
srdf_file = os.path.join(moveit_pkg_share, "srdf", "ur5_robotiq.srdf")
43+
with open(srdf_file, 'r') as file:
44+
robot_description_semantic = {"robot_description_semantic": file.read()}
45+
46+
# Kinematics
47+
kinematics_yaml = os.path.join(moveit_pkg_share, "config", "kinematics.yaml")
48+
49+
# OMPL planning
50+
ompl_planning_yaml = os.path.join(moveit_pkg_share, "config", "ompl_planning.yaml")
51+
52+
# MoveIt controllers
53+
moveit_controllers = os.path.join(moveit_pkg_share, "config", "controllers.yaml")
54+
55+
# Planning pipeline
56+
planning_pipelines_config = {
57+
"planning_pipelines": ["ompl"],
58+
"default_planning_pipeline": "ompl",
59+
"ompl": {
60+
"planning_plugin": "ompl_interface/OMPLPlanner",
61+
"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",
62+
"start_state_max_bounds_error": 0.1,
63+
}
64+
}
65+
66+
# Trajectory execution
67+
trajectory_execution = {
68+
"moveit_manage_controllers": True,
69+
"trajectory_execution.allowed_execution_duration_scaling": 1.2,
70+
"trajectory_execution.allowed_goal_duration_margin": 0.5,
71+
"trajectory_execution.allowed_start_tolerance": 0.01,
72+
}
73+
74+
planning_scene_monitor_parameters = {
75+
"publish_planning_scene": True,
76+
"publish_geometry_updates": True,
77+
"publish_state_updates": True,
78+
"publish_transforms_updates": True,
79+
}
80+
81+
# MoveIt move_group node
82+
move_group_node = Node(
83+
package="moveit_ros_move_group",
84+
executable="move_group",
85+
output="screen",
86+
parameters=[
87+
robot_description,
88+
robot_description_semantic,
89+
kinematics_yaml,
90+
ompl_planning_yaml,
91+
planning_pipelines_config,
92+
trajectory_execution,
93+
moveit_controllers,
94+
planning_scene_monitor_parameters,
95+
{"use_sim_time": True},
96+
],
97+
)
98+
99+
# RViz with MoveIt configuration
100+
rviz_config_file = os.path.join(moveit_pkg_share, "rviz", "moveit.rviz")
101+
102+
rviz_node = Node(
103+
package="rviz2",
104+
executable="rviz2",
105+
name="rviz2",
106+
output="log",
107+
arguments=["-d", rviz_config_file],
108+
parameters=[
109+
robot_description,
110+
robot_description_semantic,
111+
ompl_planning_yaml,
112+
kinematics_yaml,
113+
{"use_sim_time": True},
114+
],
115+
)
116+
117+
# Delay MoveIt slightly to let controllers stabilize
118+
delay_move_group = TimerAction(
119+
period=2.0,
120+
actions=[move_group_node],
121+
)
122+
123+
# Delay RViz after MoveIt
124+
delay_rviz = TimerAction(
125+
period=3.0,
126+
actions=[rviz_node],
127+
)
128+
129+
return LaunchDescription([
130+
delay_move_group,
131+
delay_rviz,
132+
])

‎database/universes.sql‎

Lines changed: 2 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -179,6 +179,7 @@ COPY public.universes (id, name, world_id, robot_id) FROM stdin;
179179
52 Montreal Circuit Classic 52 0
180180
53 Nurburgring Circuit Classic 53 0
181181
54 Vacuums House Roof Classic 54 0
182+
55 Pick And Place Harmonic World 55 0
182183
\.
183184

184185

@@ -243,6 +244,7 @@ COPY public.worlds (id, name, launch_file_path, tools_config, ros_version, type,
243244
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}
244245
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}
245246
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}
247+
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}
246248
\.
247249

248250
--

‎scripts/.env‎

Lines changed: 14 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -23,3 +23,17 @@ export LD_LIBRARY_PATH=$LD_LIBRARY_PATH:/usr/lib/x86_64-linux-gnu/gazebo-11/plug
2323

2424
# Default rendering display
2525
export DISPLAY=:0
26+
27+
# === Pick Place Harmonic - Gazebo Harmonic Environment === #
28+
export GZ_VERSION=harmonic
29+
30+
# gz_ros2_control plugin paths - CRITICAL for controller loading
31+
export GZ_SIM_SYSTEM_PLUGIN_PATH="/opt/ros/humble/lib:${GZ_SIM_SYSTEM_PLUGIN_PATH}"
32+
33+
# Gazebo Harmonic resource paths for robot models
34+
# CRITICAL: Use BOTH install/share (parent of package name) AND source directories
35+
# Source directories are needed because that's where meshes actually are during development
36+
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}"
37+
38+
# Library paths for gz_ros2_control
39+
export LD_LIBRARY_PATH="/opt/ros/humble/lib:${LD_LIBRARY_PATH}"

0 commit comments

Comments
 (0)