Skip to content

Commit dfa3a1d

Browse files
Jepear19Jepear19
committed
Change launchers
1 parent b9478c0 commit dfa3a1d

2 files changed

Lines changed: 189 additions & 86 deletions

File tree

‎Launchers/pick_place_harmonic/pick_place_harmonic.launch.py‎

Lines changed: 28 additions & 65 deletions
Original file line numberDiff line numberDiff line change
@@ -1,31 +1,24 @@
11
#!/usr/bin/env python3
22

3-
"""
4-
Pick Place Harmonic - CLASSIC MODE (NO MoveIt)
5-
"""
6-
73
import os
84
import yaml
9-
import subprocess
105

116
from launch import LaunchDescription
12-
from launch.actions import IncludeLaunchDescription, TimerAction
7+
from launch.actions import IncludeLaunchDescription
138
from launch.launch_description_sources import PythonLaunchDescriptionSource
149
from launch_ros.actions import Node
1510
from ament_index_python.packages import get_package_share_directory
1611

1712

1813
def load_file(package_name, file_path):
19-
package_path = get_package_share_directory(package_name)
20-
absolute_path = os.path.join(package_path, file_path)
21-
with open(absolute_path, 'r') as f:
14+
pkg_path = get_package_share_directory(package_name)
15+
with open(os.path.join(pkg_path, file_path), 'r') as f:
2216
return f.read()
2317

2418

2519
def load_yaml(package_name, file_path):
26-
package_path = get_package_share_directory(package_name)
27-
absolute_path = os.path.join(package_path, file_path)
28-
with open(absolute_path, 'r') as f:
20+
pkg_path = get_package_share_directory(package_name)
21+
with open(os.path.join(pkg_path, file_path), 'r') as f:
2922
return yaml.safe_load(f)
3023

3124

@@ -50,94 +43,64 @@ def generate_launch_description():
5043
)
5144

5245
# =========================
53-
# XACRO → URDF
54-
# =========================
55-
56-
xacro_file = "/home/ws/src/Industrial/ros2_SimRealRobotControl_gz/packages/ur5/ros2srrc_ur5_gazebo/urdf/ur5_robotiq_2f85.urdf.xacro"
57-
58-
robot_description_content = subprocess.check_output(
59-
["xacro", xacro_file],
60-
stderr=subprocess.STDOUT
61-
).decode("utf-8")
62-
63-
robot_description = {
64-
"robot_description": robot_description_content
65-
}
66-
67-
# =========================
68-
# CONFIG (necesario para execution)
46+
# MOVEIT PARAMS
6947
# =========================
7048

7149
robot_description_semantic = {
7250
"robot_description_semantic": load_file(
73-
"ros2srrc_ur5_moveit2",
74-
"config/ur5robotiq_2f85.srdf"
51+
"ur5_gripper_moveit_config",
52+
"srdf/ur5_robotiq.srdf"
7553
)
7654
}
7755

7856
kinematics_yaml = {
7957
"robot_description_kinematics": load_yaml(
80-
"ros2srrc_robots",
81-
"ur5/config/kinematics.yaml"
58+
"ur5_gripper_moveit_config",
59+
"config/kinematics.yaml"
8260
)
8361
}
8462

63+
common_params = {}
64+
common_params.update(robot_description_semantic)
65+
common_params.update(kinematics_yaml)
66+
67+
common_params.update({
68+
"use_sim_time": True,
69+
"ROB_PARAM": "ur5",
70+
"EE_PARAM": "robotiq_2f85",
71+
"ENV_PARAM": "gazebo",
72+
})
73+
8574
# =========================
86-
# EXECUTION NODES (CLAVE)
75+
# NODES
8776
# =========================
8877

8978
robmove_node = Node(
9079
package="ros2srrc_execution",
9180
executable="robmove",
9281
output="screen",
93-
parameters=[
94-
robot_description,
95-
robot_description_semantic,
96-
kinematics_yaml,
97-
{"use_sim_time": True},
98-
{"ROB_PARAM": "ur5"},
99-
],
82+
parameters=[common_params],
10083
)
10184

10285
robpose_node = Node(
10386
package="ros2srrc_execution",
10487
executable="robpose",
10588
output="screen",
106-
parameters=[
107-
robot_description,
108-
robot_description_semantic,
109-
kinematics_yaml,
110-
{"use_sim_time": True},
111-
{"ROB_PARAM": "ur5"},
112-
],
89+
parameters=[common_params],
11390
)
11491

11592
move_node = Node(
11693
package="ros2srrc_execution",
11794
executable="move",
11895
output="screen",
119-
parameters=[
120-
robot_description,
121-
robot_description_semantic,
122-
kinematics_yaml,
123-
{"use_sim_time": True},
124-
{"ROB_PARAM": "ur5"},
125-
{"EE_PARAM": "robotiq_2f85"},
126-
{"ENV_PARAM": "gazebo"},
127-
],
96+
parameters=[common_params],
12897
)
12998

130-
# =========================
131-
# LAUNCH
132-
# =========================
133-
13499
return LaunchDescription([
135100
world_launch,
136101
robot_launch,
137102

138-
# Espera corta para que Gazebo arranque
139-
TimerAction(
140-
period=2.0,
141-
actions=[robmove_node, robpose_node, move_node]
142-
)
103+
robmove_node,
104+
robpose_node,
105+
move_node
143106
])

‎Launchers/pick_place_harmonic/robot.launch.py‎

Lines changed: 161 additions & 21 deletions
Original file line numberDiff line numberDiff line change
@@ -1,35 +1,175 @@
1-
"""
2-
Pick Place Harmonic - Robot Launcher
3-
Wraps spawn_robot_warehouse.launch.py
4-
"""
1+
#!/usr/bin/env python3
52

63
import os
4+
import xacro
5+
76
from launch import LaunchDescription
8-
from launch.actions import IncludeLaunchDescription
9-
from launch.launch_description_sources import PythonLaunchDescriptionSource
7+
from launch.actions import TimerAction, RegisterEventHandler
8+
from launch.event_handlers import OnProcessExit
9+
from launch_ros.actions import Node
1010
from ament_index_python.packages import get_package_share_directory
1111

1212

1313
def generate_launch_description():
1414

15-
try:
16-
ur5_gripper_pkg = get_package_share_directory("ur5_gripper_description")
17-
except Exception as e:
18-
print(f"ERROR: Cannot find ur5_gripper_description package: {e}")
19-
print("Make sure packages are built in /home/ws")
20-
raise
15+
pkg_share_dir = get_package_share_directory("ur5_gripper_description")
16+
17+
# =========================
18+
# URDF (XACRO)
19+
# =========================
20+
21+
xacro_file = os.path.join(
22+
pkg_share_dir, "urdf", "ur5_robotiq85_gripper.urdf.xacro"
23+
)
24+
25+
controllers_file = os.path.join(
26+
pkg_share_dir, "config", "ur5_controllers.yaml"
27+
)
28+
29+
robot_description_content = xacro.process_file(
30+
xacro_file,
31+
mappings={
32+
"ur_type": "ur5",
33+
"name": "ur",
34+
"prefix": "",
35+
"use_fake_hardware": "false",
36+
"sim_gazebo": "false",
37+
"sim_gz": "true",
38+
"simulation_controllers": controllers_file,
39+
},
40+
).toxml()
41+
42+
robot_description = {"robot_description": robot_description_content}
2143

22-
warehouse_launch_file = os.path.join(
23-
ur5_gripper_pkg, "launch", "spawn_robot_warehouse.launch.py"
44+
# =========================
45+
# ROBOT STATE PUBLISHER
46+
# =========================
47+
48+
robot_state_publisher = Node(
49+
package="robot_state_publisher",
50+
executable="robot_state_publisher",
51+
name="robot_state_publisher", # evita duplicados raros
52+
output="screen",
53+
parameters=[robot_description, {"use_sim_time": True}],
54+
)
55+
56+
# =========================
57+
# SPAWN ENTITY
58+
# =========================
59+
60+
spawn_entity = Node(
61+
package="ros_gz_sim",
62+
executable="create",
63+
arguments=[
64+
"-topic", "robot_description",
65+
"-name", "ur5_robotiq",
66+
"-allow_renaming", "true",
67+
"-x", "0.0",
68+
"-y", "0.0",
69+
"-z", "0.9",
70+
],
71+
output="screen",
2472
)
2573

26-
print(f"Including launch file: {warehouse_launch_file}")
74+
# =========================
75+
# TF
76+
# =========================
2777

28-
warehouse_launch = IncludeLaunchDescription(
29-
PythonLaunchDescriptionSource(warehouse_launch_file),
30-
launch_arguments={
31-
"launch_rviz": "false",
32-
}.items(),
78+
static_tf = Node(
79+
package="tf2_ros",
80+
executable="static_transform_publisher",
81+
arguments=["0", "0", "0.9", "0", "0", "0", "world", "base_link"],
82+
output="screen",
83+
parameters=[{"use_sim_time": True}],
3384
)
3485

35-
return LaunchDescription([warehouse_launch])
86+
# =========================
87+
# CLOCK
88+
# =========================
89+
90+
gz_ros2_bridge_clock = Node(
91+
package="ros_gz_bridge",
92+
executable="parameter_bridge",
93+
arguments=["/clock@rosgraph_msgs/msg/Clock[gz.msgs.Clock"],
94+
output="screen",
95+
parameters=[{"use_sim_time": True}],
96+
)
97+
98+
# =========================
99+
# CONTROLLERS
100+
# =========================
101+
102+
load_joint_state_broadcaster = Node(
103+
package="controller_manager",
104+
executable="spawner",
105+
arguments=[
106+
"joint_state_broadcaster",
107+
"--controller-manager",
108+
"/controller_manager",
109+
],
110+
output="screen",
111+
)
112+
113+
load_joint_trajectory_controller = Node(
114+
package="controller_manager",
115+
executable="spawner",
116+
arguments=[
117+
"joint_trajectory_controller",
118+
"--param-file",
119+
controllers_file,
120+
"-c",
121+
"/controller_manager",
122+
],
123+
output="screen",
124+
)
125+
126+
load_gripper_controller = Node(
127+
package="controller_manager",
128+
executable="spawner",
129+
arguments=[
130+
"gripper_controller",
131+
"--param-file",
132+
controllers_file,
133+
"-c",
134+
"/controller_manager",
135+
],
136+
output="screen",
137+
)
138+
139+
# =========================
140+
# DELAYS
141+
# =========================
142+
143+
delay_spawn = TimerAction(period=3.0, actions=[spawn_entity])
144+
145+
delay_jsb = RegisterEventHandler(
146+
OnProcessExit(
147+
target_action=spawn_entity,
148+
on_exit=[load_joint_state_broadcaster],
149+
)
150+
)
151+
152+
delay_jtc = RegisterEventHandler(
153+
OnProcessExit(
154+
target_action=load_joint_state_broadcaster,
155+
on_exit=[load_joint_trajectory_controller],
156+
)
157+
)
158+
159+
delay_gc = RegisterEventHandler(
160+
OnProcessExit(
161+
target_action=load_joint_trajectory_controller,
162+
on_exit=[load_gripper_controller],
163+
)
164+
)
165+
166+
return LaunchDescription([
167+
robot_state_publisher,
168+
static_tf,
169+
gz_ros2_bridge_clock,
170+
171+
delay_spawn,
172+
delay_jsb,
173+
delay_jtc,
174+
delay_gc,
175+
])

0 commit comments

Comments
 (0)