Skip to content

Commit 22e7b7e

Browse files
Jepear19Jepear19
committed
Fix xacro and controller.yaml and change launcher
1 parent 87be9b9 commit 22e7b7e

3 files changed

Lines changed: 41 additions & 111 deletions

File tree

‎Industrial/ros2_SimRealRobotControl_gz/packages/ur5/ros2srrc_ur5_gazebo/urdf/ur5_robotiq_2f85.urdf.xacro‎

Lines changed: 8 additions & 6 deletions
Original file line numberDiff line numberDiff line change
@@ -79,21 +79,23 @@
7979
<xacro:unless value="${bringup}">
8080

8181
<gazebo>
82-
<plugin filename="libgz_ros2_control-system.so" name="gz_ros2_control">
83-
82+
<plugin filename="libgz_ros2_control-system.so" name="gz_ros2_control::GazeboSimROS2ControlPlugin">
83+
84+
<ros2_control name="GazeboSystem" type="system">
85+
<hardware>
86+
<plugin>gz_ros2_control/GazeboSystem</plugin>
87+
</hardware>
88+
</ros2_control>
89+
8490
<parameters>/home/ws/src/Industrial/ros2_SimRealRobotControl_gz/ros2srrc_robots/ur5/config/controller.yaml</parameters>
8591

8692
<parameters>/home/ws/src/Industrial/ros2_SimRealRobotControl_gz/ros2srrc_endeffectors/${EE_name}/config/controller.yaml</parameters>
8793

8894
</plugin>
89-
</gazebo>
9095

91-
<gazebo>
9296
<plugin filename="libgz_mimic_joint.so" name="gz_mimic_joint">
93-
9497
<parent_joint>robotiq_85_left_knuckle_joint</parent_joint>
9598
<mimic_joint>robotiq_85_right_knuckle_joint</mimic_joint>
96-
9799
</plugin>
98100
</gazebo>
99101

‎Industrial/ros2_SimRealRobotControl_gz/ros2srrc_endeffectors/robotiq_2f85/config/controller.yaml‎

Lines changed: 4 additions & 30 deletions
Original file line numberDiff line numberDiff line change
@@ -1,36 +1,10 @@
11
controller_manager:
22
ros__parameters:
3+
update_rate: 250
34

4-
update_rate: 250 #Hz
5-
6-
robotiq_controller_LKJ:
7-
type: position_controllers/GripperActionController
8-
robotiq_controller_RKJ:
9-
type: position_controllers/GripperActionController
10-
robotiq_controller_LIKJ:
11-
type: position_controllers/GripperActionController
12-
robotiq_controller_RIKJ:
13-
type: position_controllers/GripperActionController
14-
robotiq_controller_LFTJ:
15-
type: position_controllers/GripperActionController
16-
robotiq_controller_RFTJ:
5+
gripper_controller:
176
type: position_controllers/GripperActionController
187

19-
robotiq_controller_LKJ:
20-
ros__parameters:
21-
joint: robotiq_85_left_knuckle_joint
22-
robotiq_controller_RKJ:
23-
ros__parameters:
24-
joint: robotiq_85_right_knuckle_joint
25-
robotiq_controller_LIKJ:
26-
ros__parameters:
27-
joint: robotiq_85_left_inner_knuckle_joint
28-
robotiq_controller_RIKJ:
29-
ros__parameters:
30-
joint: robotiq_85_right_inner_knuckle_joint
31-
robotiq_controller_LFTJ:
32-
ros__parameters:
33-
joint: robotiq_85_left_finger_tip_joint
34-
robotiq_controller_RFTJ:
8+
gripper_controller:
359
ros__parameters:
36-
joint: robotiq_85_right_finger_tip_joint
10+
joint: robotiq_85_left_knuckle_joint

‎Launchers/pick_place_harmonic/pick_place_harmonic.launch.py‎

Lines changed: 29 additions & 75 deletions
Original file line numberDiff line numberDiff line change
@@ -26,7 +26,7 @@ def load_yaml(package_name, file_path):
2626
def generate_launch_description():
2727

2828
# =========================
29-
# WORLD (HARMONIC)
29+
# WORLD
3030
# =========================
3131

3232
world_path = os.path.join(
@@ -36,12 +36,12 @@ def generate_launch_description():
3636
)
3737

3838
gz = ExecuteProcess(
39-
cmd=["gz", "sim", "-r", "-v", "4", world_path],
39+
cmd=["gz", "sim", "-r", "-s", "-v", "4", world_path], # 👈 FIX GUI
4040
output="both"
4141
)
4242

4343
# =========================
44-
# ROBOT DESCRIPTION (CLAVE)
44+
# ROBOT DESCRIPTION
4545
# =========================
4646

4747
xacro_file = "/home/ws/src/Industrial/ros2_SimRealRobotControl_gz/packages/ur5/ros2srrc_ur5_gazebo/urdf/ur5_robotiq_2f85.urdf.xacro"
@@ -59,13 +59,18 @@ def generate_launch_description():
5959
"sim_gazebo": "false",
6060
"sim_gz": "true",
6161
"simulation_controllers": controllers_file,
62+
63+
"EE": "true",
64+
"EE_name": "robotiq_2f85",
6265
},
6366
).toxml()
6467

68+
print("ROBOT DESCRIPTION LENGTH:", len(robot_description_content))
69+
6570
robot_description = {"robot_description": robot_description_content}
6671

6772
# =========================
68-
# SRDF + KINEMATICS
73+
# MOVEIT CONFIG
6974
# =========================
7075

7176
robot_description_semantic = {
@@ -82,33 +87,24 @@ def generate_launch_description():
8287
)
8388
}
8489

90+
moveit_controllers = {
91+
"moveit_simple_controller_manager": load_yaml(
92+
"ur5_gripper_moveit_config",
93+
"config/moveit_controllers.yaml"
94+
)
95+
}
96+
8597
# =========================
86-
# ROBOT STATE PUBLISHER
98+
# CORE NODES
8799
# =========================
88100

89101
robot_state_publisher = Node(
90102
package="robot_state_publisher",
91103
executable="robot_state_publisher",
92104
output="both",
93-
arguments=["--ros-args", "--log-level", "debug"],
94105
parameters=[robot_description, {"use_sim_time": True}],
95106
)
96107

97-
"""static_tf = Node(
98-
package="tf2_ros",
99-
executable="static_transform_publisher",
100-
arguments=[
101-
"0", "0", "0.9", "0", "0", "0", "world", "base_link",
102-
"--ros-args", "--log-level", "debug",
103-
],
104-
output="both",
105-
parameters=[{"use_sim_time": True}],
106-
)"""
107-
108-
# =========================
109-
# SPAWN ROBOT (HARMONIC)
110-
# =========================
111-
112108
spawn_robot = Node(
113109
package="ros_gz_sim",
114110
executable="create",
@@ -123,10 +119,6 @@ def generate_launch_description():
123119
output="both",
124120
)
125121

126-
# =========================
127-
# CLOCK BRIDGE
128-
# =========================
129-
130122
clock_bridge = Node(
131123
package="ros_gz_bridge",
132124
executable="parameter_bridge",
@@ -157,28 +149,13 @@ def generate_launch_description():
157149
)
158150

159151
# =========================
160-
# MOVE GROUP (IGUAL QUE CLASSIC)
152+
# MOVEIT
161153
# =========================
162-
163-
164-
moveit_controllers = {
165-
"moveit_simple_controller_manager": load_yaml(
166-
"ur5_gripper_moveit_config",
167-
"config/moveit_controllers.yaml"
168-
)
169-
}
170-
171-
print("===== DEBUG MOVEIT =====")
172-
print(robot_description.keys())
173-
print(robot_description_semantic.keys())
174-
print(moveit_controllers)
175-
print("========================")
176154

177155
move_group = Node(
178156
package="moveit_ros_move_group",
179157
executable="move_group",
180158
output="both",
181-
arguments=["--ros-args", "--log-level", "debug"],
182159
parameters=[
183160
robot_description,
184161
robot_description_semantic,
@@ -190,110 +167,87 @@ def generate_launch_description():
190167
)
191168

192169
# =========================
193-
# INTERFACES (CLÁSICO)
170+
# EXECUTION NODES
194171
# =========================
195172

196173
common_params = [
197174
robot_description,
198175
robot_description_semantic,
199176
kinematics_yaml,
200177
{"use_sim_time": True},
201-
{"ROB_PARAM": "ur5"},
202-
{"EE_PARAM": "robotiq_2f85"},
203-
{"ENV_PARAM": "gazebo"},
204178
]
205179

206180
move_node = Node(
207181
package="ros2srrc_execution",
208182
executable="move",
209183
output="both",
210-
arguments=["--ros-args", "--log-level", "debug"],
211184
parameters=common_params,
212185
)
213186

214187
robmove_node = Node(
215188
package="ros2srrc_execution",
216189
executable="robmove",
217190
output="both",
218-
arguments=["--ros-args", "--log-level", "debug"],
219191
parameters=common_params,
220192
)
221193

222194
robpose_node = Node(
223195
package="ros2srrc_execution",
224196
executable="robpose",
225197
output="both",
226-
arguments=["--ros-args", "--log-level", "debug"],
227198
parameters=common_params,
228199
)
229200

230-
# =========================
231-
# SECUENCIA (CLAVE)
232-
# =========================
233201

234202
return LaunchDescription([
235203

236204
gz,
237205
robot_state_publisher,
238-
# static_tf,
239206
clock_bridge,
240-
241207
spawn_robot,
242208

209+
# Controllers chain
243210
RegisterEventHandler(
244211
OnProcessExit(
245212
target_action=spawn_robot,
246-
on_exit=[
247-
TimerAction(
248-
period=3.0,
249-
actions=[joint_state_broadcaster],
250-
)
251-
],
213+
on_exit=[joint_state_broadcaster],
252214
)
253215
),
254216

255217
RegisterEventHandler(
256218
OnProcessExit(
257219
target_action=joint_state_broadcaster,
258-
on_exit=[
259-
TimerAction(
260-
period=2.0,
261-
actions=[joint_trajectory_controller],
262-
)
263-
],
220+
on_exit=[joint_trajectory_controller],
264221
)
265222
),
266223

267224
RegisterEventHandler(
268225
OnProcessExit(
269226
target_action=joint_trajectory_controller,
270-
on_exit=[
271-
TimerAction(
272-
period=2.0,
273-
actions=[gripper_controller],
274-
)
275-
],
227+
on_exit=[gripper_controller],
276228
)
277229
),
278230

231+
# MoveIt after controllers
279232
RegisterEventHandler(
280233
OnProcessExit(
281-
target_action=spawn_robot,
234+
target_action=gripper_controller,
282235
on_exit=[
283236
TimerAction(
284-
period=5.0,
237+
period=2.0,
285238
actions=[move_group],
286239
)
287240
],
288241
)
289242
),
290243

244+
# Action servers AFTER MoveIt
291245
RegisterEventHandler(
292246
OnProcessExit(
293-
target_action=move_group,
247+
target_action=gripper_controller,
294248
on_exit=[
295249
TimerAction(
296-
period=5.0,
250+
period=4.0,
297251
actions=[move_node, robmove_node, robpose_node],
298252
)
299253
],

0 commit comments

Comments
 (0)