Skip to content

Commit d0a6cb5

Browse files
committed
Add simple universal launch file.
1 parent 3426106 commit d0a6cb5

File tree

1 file changed

+85
-0
lines changed

1 file changed

+85
-0
lines changed
Lines changed: 85 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,85 @@
1+
import os
2+
import xacro
3+
import yaml
4+
from ament_index_python.packages import get_package_share_directory
5+
from launch import LaunchDescription
6+
from launch_ros.actions import Node
7+
8+
def load_file(package_name, file_path):
9+
package_path = get_package_share_directory(package_name)
10+
absolute_file_path = os.path.join(package_path, file_path)
11+
12+
try:
13+
with open(absolute_file_path, 'r') as file:
14+
return file.read()
15+
except EnvironmentError: # parent of IOError, OSError *and* WindowsError where available
16+
return None
17+
18+
19+
def generate_launch_description():
20+
21+
# set ur robot
22+
robot_name = 'ur5e'
23+
24+
# <robot_name> parameters files
25+
joint_limits_params = os.path.join(get_package_share_directory('ur_description'), 'config/' +
26+
robot_name, 'joint_limits.yaml')
27+
kinematics_params = os.path.join(get_package_share_directory('ur_description'), 'config/' +
28+
robot_name, 'default_kinematics.yaml')
29+
physical_params = os.path.join(get_package_share_directory('ur_description'), 'config/' +
30+
robot_name, 'physical_parameters.yaml')
31+
visual_params = os.path.join(get_package_share_directory('ur_description'), 'config/' +
32+
robot_name, 'visual_parameters.yaml')
33+
34+
# common parameters
35+
# If True, enable the safety limits controller
36+
safety_limits = False
37+
# The lower/upper limits in the safety controller
38+
safety_pos_margin = 0.15
39+
# Used to set k position in the safety controller
40+
safety_k_position = 20
41+
42+
# Get URDF via xacro
43+
robot_description_path = os.path.join(get_package_share_directory('ur_description'), 'urdf', 'ur.xacro')
44+
45+
robot_description_config = xacro.process_file(robot_description_path,
46+
mappings={'joint_limit_params': joint_limits_params,
47+
'kinematics_params': kinematics_params,
48+
'physical_params': physical_params,
49+
'visual_params': visual_params,
50+
'safety_limits': str(safety_limits).lower(),
51+
'safety_pos_margin': str(safety_pos_margin),
52+
'safety_k_position': str(safety_k_position)}
53+
)
54+
55+
robot_description = {'robot_description': robot_description_config.toxml()}
56+
57+
robot_description_semantic_config = load_file(robot_name + '_moveit_config', 'config/' + robot_name + '.srdf')
58+
robot_description_semantic = {'robot_description_semantic' : robot_description_semantic_config}
59+
60+
# We do not have a robot connected, so publish fake joint states
61+
joint_state_pub_node = Node(
62+
package='joint_state_publisher',
63+
executable='joint_state_publisher'
64+
)
65+
66+
# Publishes tf's for the robot
67+
robot_state_pub_node = Node(
68+
package='robot_state_publisher',
69+
executable='robot_state_publisher',
70+
output='screen',
71+
parameters=[robot_description]
72+
)
73+
74+
# RViz
75+
rviz_config_file = get_package_share_directory(
76+
'ur_description') + "/cfg/view_robot.rviz"
77+
rviz_node = Node(package='rviz2',
78+
executable='rviz2',
79+
name='rviz2',
80+
output='log',
81+
arguments=['-d', rviz_config_file],
82+
parameters=[robot_description, robot_description_semantic]
83+
)
84+
85+
return LaunchDescription([rviz_node, robot_state_pub_node, joint_state_pub_node])

0 commit comments

Comments
 (0)