Skip to content

Commit 5407484

Browse files
Jepear19Jepear19
committed
Restart rviz launcher
1 parent df715b3 commit 5407484

1 file changed

Lines changed: 75 additions & 243 deletions

File tree

‎Launchers/rviz/pick_place_harmonic.launch.py‎

Lines changed: 75 additions & 243 deletions
Original file line numberDiff line numberDiff line change
@@ -1,52 +1,25 @@
1-
#!/usr/bin/env python3
1+
"""
2+
Pick Place Harmonic - RViz + MoveIt Launcher
3+
Launches ONLY: MoveIt move_group + RViz with motion planning
4+
Assumes Gazebo and robot are already running
5+
"""
26

37
import os
4-
import xacro
5-
import yaml
6-
78
from launch import LaunchDescription
8-
from launch.actions import ExecuteProcess, RegisterEventHandler, TimerAction
9-
from launch.event_handlers import OnProcessExit
109
from launch_ros.actions import Node
10+
from launch.actions import TimerAction
1111
from ament_index_python.packages import get_package_share_directory
12-
13-
14-
def load_file(package_name, file_path):
15-
pkg_path = get_package_share_directory(package_name)
16-
with open(os.path.join(pkg_path, file_path), 'r') as f:
17-
return f.read()
18-
19-
20-
def load_yaml(package_name, file_path):
21-
pkg_path = get_package_share_directory(package_name)
22-
with open(os.path.join(pkg_path, file_path), 'r') as f:
23-
return yaml.safe_load(f)
12+
import xacro
2413

2514

2615
def generate_launch_description():
27-
28-
# =========================
29-
# WORLD
30-
# =========================
31-
32-
world_path = os.path.join(
33-
get_package_share_directory("robotiq_description"),
34-
"world",
35-
"warehouse_arm_harmonic.world"
36-
)
37-
38-
gz = ExecuteProcess(
39-
cmd=["gz", "sim", "-r", "-s", "-v", "4", world_path],
40-
output="both"
41-
)
42-
43-
# =========================
44-
# ROBOT DESCRIPTION
45-
# =========================
46-
47-
xacro_file = "/home/ws/src/Industrial/ros2_SimRealRobotControl_gz/packages/ur5/ros2srrc_ur5_gazebo/urdf/ur5_robotiq_2f85.urdf.xacro"
48-
16+
# Get package directories
4917
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+
xacro_file = os.path.join(pkg_share_dir, "urdf", "ur5_robotiq85_gripper.urdf.xacro")
5023
controllers_file = os.path.join(pkg_share_dir, "config", "ur5_controllers.yaml")
5124

5225
robot_description_content = xacro.process_file(
@@ -59,243 +32,102 @@ def generate_launch_description():
5932
"sim_gazebo": "false",
6033
"sim_gz": "true",
6134
"simulation_controllers": controllers_file,
62-
63-
"EE": "true",
64-
"EE_name": "robotiq_2f85",
6535
},
6636
).toxml()
6737

68-
print("ROBOT DESCRIPTION LENGTH:", len(robot_description_content))
69-
7038
robot_description = {"robot_description": robot_description_content}
7139

72-
# =========================
73-
# MOVEIT CONFIG
74-
# =========================
40+
# SRDF
41+
srdf_file = os.path.join(moveit_pkg_share, "srdf", "ur5_robotiq.srdf")
42+
with open(srdf_file, "r") as file:
43+
robot_description_semantic = {"robot_description_semantic": file.read()}
7544

76-
robot_description_semantic = {
77-
"robot_description_semantic": load_file(
78-
"ros2srrc_ur5_moveit2",
79-
"config/ur5robotiq_2f85.srdf"
80-
)
81-
}
82-
83-
raw_kinematics = load_yaml(
84-
"ur5_gripper_moveit_config",
85-
"config/kinematics.yaml"
86-
)
45+
# Kinematics
46+
kinematics_yaml = os.path.join(moveit_pkg_share, "config", "kinematics.yaml")
8747

88-
kinematics_yaml = raw_kinematics["/**"]["ros__parameters"]
89-
90-
raw_controllers = load_yaml(
91-
"ur5_gripper_moveit_config",
92-
"config/controllers.yaml"
93-
)
94-
95-
moveit_controllers = raw_controllers["/**"]["ros__parameters"]
96-
97-
# =========================
98-
# CORE NODES
99-
# =========================
100-
101-
robot_state_publisher = Node(
102-
package="robot_state_publisher",
103-
executable="robot_state_publisher",
104-
output="both",
105-
parameters=[robot_description, {"use_sim_time": True}],
106-
)
107-
108-
spawn_robot = Node(
109-
package="ros_gz_sim",
110-
executable="create",
111-
arguments=[
112-
"-param", "robot_description",
113-
"-name", "ur5",
114-
"-x", "0.0",
115-
"-y", "0.0",
116-
"-z", "0.9",
117-
],
118-
parameters=[robot_description],
119-
output="both",
120-
)
121-
122-
clock_bridge = Node(
123-
package="ros_gz_bridge",
124-
executable="parameter_bridge",
125-
arguments=["/clock@rosgraph_msgs/msg/Clock[gz.msgs.Clock"],
126-
parameters=[{"use_sim_time": True}],
127-
)
128-
129-
# =========================
130-
# CONTROLLERS
131-
# =========================
132-
133-
joint_state_broadcaster = Node(
134-
package="controller_manager",
135-
executable="spawner",
136-
arguments=[
137-
"joint_state_broadcaster",
138-
"--controller-manager",
139-
"/controller_manager",
140-
],
141-
)
48+
# OMPL planning
49+
ompl_planning_yaml = os.path.join(moveit_pkg_share, "config", "ompl_planning.yaml")
14250

143-
joint_trajectory_controller = Node(
144-
package="controller_manager",
145-
executable="spawner",
146-
arguments=[
147-
"joint_trajectory_controller",
148-
"--controller-manager",
149-
"/controller_manager",
150-
],
151-
)
51+
# MoveIt controllers
52+
moveit_controllers = os.path.join(moveit_pkg_share, "config", "controllers.yaml")
15253

153-
gripper_controller = Node(
154-
package="controller_manager",
155-
executable="spawner",
156-
arguments=[
157-
"gripper_controller",
158-
"--controller-manager",
159-
"/controller_manager",
160-
],
161-
)
54+
# Planning pipeline
55+
planning_pipelines_config = {
56+
"planning_pipelines": ["ompl"],
57+
"default_planning_pipeline": "ompl",
58+
"ompl": {
59+
"planning_plugin": "ompl_interface/OMPLPlanner",
60+
"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",
61+
"start_state_max_bounds_error": 0.1,
62+
},
63+
}
16264

163-
delayed_joint_state_broadcaster = TimerAction(
164-
period=8.0,
165-
actions=[joint_state_broadcaster],
166-
)
65+
# Trajectory execution
66+
trajectory_execution = {
67+
"moveit_manage_controllers": True,
68+
"trajectory_execution.allowed_execution_duration_scaling": 1.2,
69+
"trajectory_execution.allowed_goal_duration_margin": 0.5,
70+
"trajectory_execution.allowed_start_tolerance": 0.01,
71+
}
16772

168-
# =========================
169-
# MOVEIT
170-
# =========================
73+
planning_scene_monitor_parameters = {
74+
"publish_planning_scene": True,
75+
"publish_geometry_updates": True,
76+
"publish_state_updates": True,
77+
"publish_transforms_updates": True,
78+
}
17179

172-
"""move_group = Node(
80+
# MoveIt move_group node
81+
move_group_node = Node(
17382
package="moveit_ros_move_group",
17483
executable="move_group",
175-
output="both",
176-
parameters=[
177-
robot_description,
178-
robot_description_semantic,
179-
kinematics_yaml,
180-
moveit_controllers,
181-
{"moveit_controller_manager": "moveit_simple_controller_manager/MoveItSimpleControllerManager"},
182-
{"use_sim_time": True},
183-
],
184-
)"""
185-
186-
# =========================
187-
# EXECUTION NODES
188-
# =========================
189-
190-
common_params = [
191-
robot_description,
192-
robot_description_semantic,
193-
kinematics_yaml,
194-
{"use_sim_time": True},
195-
{"ROB_PARAM": "ur5"},
196-
{"EE_PARAM": "robotiq_2f85"},
197-
]
198-
199-
move_node = Node(
200-
name="move",
201-
package="ros2srrc_execution",
202-
executable="move",
20384
output="screen",
204-
arguments=['--ros-args', '--log-level', 'debug'],
20585
parameters=[
20686
robot_description,
20787
robot_description_semantic,
20888
kinematics_yaml,
89+
ompl_planning_yaml,
90+
planning_pipelines_config,
91+
trajectory_execution,
20992
moveit_controllers,
93+
planning_scene_monitor_parameters,
21094
{"use_sim_time": True},
211-
{"ROB_PARAM": "ur5"},
212-
{"EE_PARAM": "robotiq_2f85"},
21395
],
21496
)
21597

216-
robmove_node = Node(
217-
name="robmove",
218-
package="ros2srrc_execution",
219-
executable="robmove",
220-
output="both",
221-
parameters=[
222-
robot_description,
223-
robot_description_semantic,
224-
kinematics_yaml,
225-
moveit_controllers,
226-
{"use_sim_time": True},
227-
{"ROB_PARAM": "ur5"},
228-
{"EE_PARAM": "robotiq_2f85"},
229-
{"ENV_PARAM": "gazebo"},
230-
],
231-
)
98+
# RViz with MoveIt configuration
99+
rviz_config_file = os.path.join(moveit_pkg_share, "rviz", "moveit.rviz")
232100

233-
robpose_node = Node(
234-
name="robpose",
235-
package="ros2srrc_execution",
236-
executable="robpose",
237-
output="both",
101+
rviz_node = Node(
102+
package="rviz2",
103+
executable="rviz2",
104+
name="rviz2",
105+
output="log",
106+
arguments=["-d", rviz_config_file],
238107
parameters=[
239108
robot_description,
240109
robot_description_semantic,
110+
ompl_planning_yaml,
241111
kinematics_yaml,
242-
moveit_controllers,
243112
{"use_sim_time": True},
244-
{"ROB_PARAM": "ur5"},
245-
{"EE_PARAM": "robotiq_2f85"},
246-
{"ENV_PARAM": "gazebo"},
247113
],
248114
)
249115

250-
delayed_spawn = TimerAction(
251-
period=5.0,
252-
actions=[spawn_robot],
116+
# Delay MoveIt slightly to let controllers stabilize
117+
delay_move_group = TimerAction(
118+
period=2.0,
119+
actions=[move_group_node],
253120
)
254121

255-
delayed_execution_nodes = TimerAction(
256-
period=10.0,
257-
actions=[
258-
ExecuteProcess(cmd=["echo", ">>> LAUNCH: intentando lanzar move_node"]),
259-
move_node,
260-
robmove_node,
261-
robpose_node
262-
],
122+
# Delay RViz after MoveIt
123+
delay_rviz = TimerAction(
124+
period=3.0,
125+
actions=[rviz_node],
263126
)
264127

265-
print(">>> LAUNCH: move_node configurado")
266-
267-
return LaunchDescription([
268-
269-
gz,
270-
robot_state_publisher,
271-
clock_bridge,
272-
delayed_spawn,
273-
delayed_joint_state_broadcaster,
274-
delayed_execution_nodes,
275-
276-
RegisterEventHandler(
277-
OnProcessExit(
278-
target_action=joint_state_broadcaster,
279-
on_exit=[joint_trajectory_controller],
280-
)
281-
),
282-
283-
RegisterEventHandler(
284-
OnProcessExit(
285-
target_action=joint_trajectory_controller,
286-
on_exit=[gripper_controller],
287-
)
288-
),
289-
290-
"""RegisterEventHandler(
291-
OnProcessExit(
292-
target_action=gripper_controller,
293-
on_exit=[
294-
TimerAction(
295-
period=2.0,
296-
actions=[move_group],
297-
)
298-
],
299-
)
300-
),"""
301-
])
128+
return LaunchDescription(
129+
[
130+
delay_move_group,
131+
delay_rviz,
132+
]
133+
)

0 commit comments

Comments
 (0)