Skip to content

Commit fb1db7b

Browse files
Jepear19Jepear19
committed
Try only 1 launcher
1 parent dfa3a1d commit fb1db7b

4 files changed

Lines changed: 212 additions & 160 deletions

File tree

‎Industrial/ros2_SimRealRobotControl_gz/packages/ur5/ros2srrc_ur5_moveit2/config/ur5robotiq_2f85.srdf‎

Lines changed: 4 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -34,8 +34,8 @@
3434

3535
<robot name="ur5">
3636

37-
<group name="ur5_arm">
38-
<chain base_link="base_link" tip_link="EE_robotiq_2f85"/>
37+
<group name="ur5_manipulator">
38+
<chain base_link="base_link" tip_link="tool0"/>
3939
</group>
4040

4141
<group name="robotiq_2f85">
@@ -57,7 +57,7 @@
5757

5858
</group>
5959

60-
<group_state group="ur5_arm" name="home">
60+
<group_state group="ur5_manipulator" name="home">
6161
<joint name="elbow_joint" value="0" />
6262
<joint name="shoulder_lift_joint" value="-1.5707" />
6363
<joint name="shoulder_pan_joint" value="0" />
@@ -66,7 +66,7 @@
6666
<joint name="wrist_3_joint" value="0" />
6767
</group_state>
6868

69-
<group_state group="ur5_arm" name="up">
69+
<group_state group="ur5_manipulator" name="up">
7070
<joint name="elbow_joint" value="0" />
7171
<joint name="shoulder_lift_joint" value="-1.5707" />
7272
<joint name="shoulder_pan_joint" value="0" />

‎Industrial/ur5_gripper_moveit_config/srdf/ur5_robotiq.srdf‎

Lines changed: 5 additions & 5 deletions
Original file line numberDiff line numberDiff line change
@@ -1,7 +1,7 @@
11
<?xml version="1.0" ?>
22
<robot name="ur5">
33
<!--GROUPS-->
4-
<group name="ur5_manipulator">
4+
<group name="ur5_arm">
55
<chain base_link="base_link" tip_link="tool0"/>
66
</group>
77

@@ -10,7 +10,7 @@
1010
</group>
1111

1212
<!--GROUP STATES-->
13-
<group_state group="ur5_manipulator" name="home">
13+
<group_state group="ur5_arm" name="home">
1414
<joint name="elbow_joint" value="0"/>
1515
<joint name="shoulder_lift_joint" value="-1.5707"/>
1616
<joint name="shoulder_pan_joint" value="0"/>
@@ -19,7 +19,7 @@
1919
<joint name="wrist_3_joint" value="0"/>
2020
</group_state>
2121

22-
<group_state group="ur5_manipulator" name="up">
22+
<group_state group="ur5_arm" name="up">
2323
<joint name="elbow_joint" value="0"/>
2424
<joint name="shoulder_lift_joint" value="-1.5707"/>
2525
<joint name="shoulder_pan_joint" value="0"/>
@@ -28,7 +28,7 @@
2828
<joint name="wrist_3_joint" value="0"/>
2929
</group_state>
3030

31-
<group_state group="ur5_manipulator" name="test_configuration">
31+
<group_state group="ur5_arm" name="test_configuration">
3232
<joint name="elbow_joint" value="1.4"/>
3333
<joint name="shoulder_lift_joint" value="-1.62"/>
3434
<joint name="shoulder_pan_joint" value="1.54"/>
@@ -46,7 +46,7 @@
4646
</group_state>
4747

4848
<!--END EFFECTOR-->
49-
<end_effector group="gripper" name="gripper" parent_group="ur5_manipulator" parent_link="tool0"/>
49+
<end_effector group="gripper" name="gripper" parent_group="ur5_arm" parent_link="tool0"/>
5050

5151
<!--PASSIVE JOINTS-->
5252
<!-- Declare mimic joints as passive so MoveIt doesn't try to control them directly -->

‎Launchers/pick_place_harmonic/pick_place_harmonic.launch.py‎

Lines changed: 192 additions & 37 deletions
Original file line numberDiff line numberDiff line change
@@ -1,11 +1,12 @@
11
#!/usr/bin/env python3
22

33
import os
4+
import xacro
45
import yaml
56

67
from launch import LaunchDescription
7-
from launch.actions import IncludeLaunchDescription
8-
from launch.launch_description_sources import PythonLaunchDescriptionSource
8+
from launch.actions import ExecuteProcess, RegisterEventHandler, TimerAction
9+
from launch.event_handlers import OnProcessExit
910
from launch_ros.actions import Node
1011
from ament_index_python.packages import get_package_share_directory
1112

@@ -24,26 +25,47 @@ def load_yaml(package_name, file_path):
2425

2526
def generate_launch_description():
2627

27-
base_dir = os.path.dirname(__file__)
28-
2928
# =========================
30-
# WORLD + ROBOT
29+
# WORLD (HARMONIC)
3130
# =========================
3231

33-
world_launch = IncludeLaunchDescription(
34-
PythonLaunchDescriptionSource(
35-
os.path.join(base_dir, "world.launch.py")
36-
)
32+
world_path = os.path.join(
33+
get_package_share_directory("robotiq_description"),
34+
"world",
35+
"warehouse_arm_harmonic.world"
3736
)
3837

39-
robot_launch = IncludeLaunchDescription(
40-
PythonLaunchDescriptionSource(
41-
os.path.join(base_dir, "robot.launch.py")
42-
)
38+
gz = ExecuteProcess(
39+
cmd=["gz", "sim", "-r", "-v", "4", world_path],
40+
output="screen"
4341
)
4442

4543
# =========================
46-
# MOVEIT PARAMS
44+
# ROBOT DESCRIPTION (CLAVE)
45+
# =========================
46+
47+
xacro_file = "/home/ws/src/Industrial/ros2_SimRealRobotControl_gz/packages/ur5/ros2srrc_ur5_gazebo/urdf/ur5_robotiq_2f85.urdf.xacro"
48+
49+
pkg_share_dir = get_package_share_directory("ur5_gripper_description")
50+
controllers_file = os.path.join(pkg_share_dir, "config", "ur5_controllers.yaml")
51+
52+
robot_description_content = xacro.process_file(
53+
xacro_file,
54+
mappings={
55+
"ur_type": "ur5",
56+
"name": "ur",
57+
"prefix": "",
58+
"use_fake_hardware": "false",
59+
"sim_gazebo": "false",
60+
"sim_gz": "true",
61+
"simulation_controllers": controllers_file,
62+
},
63+
).toxml()
64+
65+
robot_description = {"robot_description": robot_description_content}
66+
67+
# =========================
68+
# SRDF + KINEMATICS
4769
# =========================
4870

4971
robot_description_semantic = {
@@ -60,47 +82,180 @@ def generate_launch_description():
6082
)
6183
}
6284

63-
common_params = {}
64-
common_params.update(robot_description_semantic)
65-
common_params.update(kinematics_yaml)
85+
# =========================
86+
# ROBOT STATE PUBLISHER
87+
# =========================
88+
89+
robot_state_publisher = Node(
90+
package="robot_state_publisher",
91+
executable="robot_state_publisher",
92+
output="screen",
93+
parameters=[robot_description, {"use_sim_time": True}],
94+
)
95+
96+
static_tf = Node(
97+
package="tf2_ros",
98+
executable="static_transform_publisher",
99+
arguments=["0", "0", "0.9", "0", "0", "0", "world", "base_link"],
100+
parameters=[{"use_sim_time": True}],
101+
)
102+
103+
# =========================
104+
# SPAWN ROBOT (HARMONIC)
105+
# =========================
106+
107+
spawn_robot = Node(
108+
package="ros_gz_sim",
109+
executable="create",
110+
arguments=[
111+
"-topic", "robot_description",
112+
"-name", "ur5",
113+
"-x", "0.0",
114+
"-y", "0.0",
115+
"-z", "0.9",
116+
],
117+
output="screen",
118+
)
119+
120+
# =========================
121+
# CLOCK BRIDGE
122+
# =========================
123+
124+
clock_bridge = Node(
125+
package="ros_gz_bridge",
126+
executable="parameter_bridge",
127+
arguments=["/clock@rosgraph_msgs/msg/Clock[gz.msgs.Clock"],
128+
parameters=[{"use_sim_time": True}],
129+
)
130+
131+
# =========================
132+
# CONTROLLERS
133+
# =========================
134+
135+
joint_state_broadcaster = Node(
136+
package="controller_manager",
137+
executable="spawner",
138+
arguments=["joint_state_broadcaster"],
139+
)
140+
141+
joint_trajectory_controller = Node(
142+
package="controller_manager",
143+
executable="spawner",
144+
arguments=["joint_trajectory_controller"],
145+
)
146+
147+
gripper_controller = Node(
148+
package="controller_manager",
149+
executable="spawner",
150+
arguments=["gripper_controller"],
151+
)
152+
153+
# =========================
154+
# MOVE GROUP (IGUAL QUE CLASSIC)
155+
# =========================
66156

67-
common_params.update({
68-
"use_sim_time": True,
69-
"ROB_PARAM": "ur5",
70-
"EE_PARAM": "robotiq_2f85",
71-
"ENV_PARAM": "gazebo",
72-
})
157+
move_group = Node(
158+
package="moveit_ros_move_group",
159+
executable="move_group",
160+
output="screen",
161+
parameters=[
162+
robot_description,
163+
robot_description_semantic,
164+
kinematics_yaml,
165+
{"use_sim_time": True},
166+
],
167+
)
73168

74169
# =========================
75-
# NODES
170+
# INTERFACES (CLÁSICO)
76171
# =========================
77172

173+
common_params = [
174+
robot_description,
175+
robot_description_semantic,
176+
kinematics_yaml,
177+
{"use_sim_time": True},
178+
{"ROB_PARAM": "ur5"},
179+
{"EE_PARAM": "robotiq_2f85"},
180+
{"ENV_PARAM": "gazebo"},
181+
]
182+
183+
move_node = Node(
184+
package="ros2srrc_execution",
185+
executable="move",
186+
output="screen",
187+
parameters=common_params,
188+
)
189+
78190
robmove_node = Node(
79191
package="ros2srrc_execution",
80192
executable="robmove",
81193
output="screen",
82-
parameters=[common_params],
194+
parameters=common_params,
83195
)
84196

85197
robpose_node = Node(
86198
package="ros2srrc_execution",
87199
executable="robpose",
88200
output="screen",
89-
parameters=[common_params],
201+
parameters=common_params,
90202
)
91203

92-
move_node = Node(
93-
package="ros2srrc_execution",
94-
executable="move",
95-
output="screen",
96-
parameters=[common_params],
97-
)
204+
# =========================
205+
# SECUENCIA (CLAVE)
206+
# =========================
98207

99208
return LaunchDescription([
100-
world_launch,
101-
robot_launch,
102209

103-
robmove_node,
104-
robpose_node,
105-
move_node
210+
gz,
211+
robot_state_publisher,
212+
static_tf,
213+
clock_bridge,
214+
215+
spawn_robot,
216+
217+
RegisterEventHandler(
218+
OnProcessExit(
219+
target_action=spawn_robot,
220+
on_exit=[joint_state_broadcaster],
221+
)
222+
),
223+
224+
RegisterEventHandler(
225+
OnProcessExit(
226+
target_action=joint_state_broadcaster,
227+
on_exit=[joint_trajectory_controller],
228+
)
229+
),
230+
231+
RegisterEventHandler(
232+
OnProcessExit(
233+
target_action=joint_trajectory_controller,
234+
on_exit=[gripper_controller],
235+
)
236+
),
237+
238+
RegisterEventHandler(
239+
OnProcessExit(
240+
target_action=spawn_robot,
241+
on_exit=[
242+
TimerAction(
243+
period=2.0,
244+
actions=[move_group],
245+
)
246+
],
247+
)
248+
),
249+
250+
RegisterEventHandler(
251+
OnProcessExit(
252+
target_action=spawn_robot,
253+
on_exit=[
254+
TimerAction(
255+
period=4.0,
256+
actions=[move_node, robmove_node, robpose_node],
257+
)
258+
],
259+
)
260+
),
106261
])

0 commit comments

Comments
 (0)