Skip to content

Commit 846f94a

Browse files
vimiorvimior
committed
[feat] (xarm_planner) add dual xarm example
1 parent e368cce commit 846f94a

File tree

4 files changed

+492
-0
lines changed

4 files changed

+492
-0
lines changed

‎xarm_planner/CMakeLists.txt‎

Lines changed: 10 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -74,6 +74,15 @@ target_link_libraries(test_xarm_planner_api_joint
7474
${ament_LIBRARIES}
7575
)
7676

77+
add_executable(test_dual_xarm_planner_api_joint test/test_dual_xarm_planner_api_joint.cpp)
78+
ament_target_dependencies(test_dual_xarm_planner_api_joint
79+
${dependencies}
80+
)
81+
target_link_libraries(test_dual_xarm_planner_api_joint
82+
xarm_planner
83+
${ament_LIBRARIES}
84+
)
85+
7786
add_executable(test_xarm_planner_api_pose test/test_xarm_planner_api_pose.cpp)
7887
ament_target_dependencies(test_xarm_planner_api_pose
7988
${dependencies}
@@ -134,6 +143,7 @@ install(
134143
xarm_planner_node
135144
xarm_gripper_planner_node
136145
test_xarm_planner_api_joint
146+
test_dual_xarm_planner_api_joint
137147
test_xarm_planner_api_pose
138148
test_xarm_gripper_planner_api_joint
139149
test_xarm_planner_client_joint

‎xarm_planner/launch/_dual_robot_planner.launch.py‎

Lines changed: 271 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,271 @@
1+
#!/usr/bin/env python3
2+
# Software License Agreement (BSD License)
3+
#
4+
# Copyright (c) 2021, UFACTORY, Inc.
5+
# All rights reserved.
6+
#
7+
# Author: Vinman <[email protected]> <[email protected]>
8+
9+
import os
10+
import json
11+
from ament_index_python import get_package_share_directory
12+
from launch.frontend import expose
13+
from launch.launch_description_sources import load_python_launch_file_as_module
14+
from launch import LaunchDescription
15+
from launch.actions import OpaqueFunction
16+
from launch.substitutions import LaunchConfiguration, PathJoinSubstitution
17+
from launch_ros.actions import Node
18+
from launch_ros.substitutions import FindPackageShare
19+
20+
21+
def launch_setup(context, *args, **kwargs):
22+
prefix_1 = LaunchConfiguration('prefix_1', default='L_')
23+
prefix_2 = LaunchConfiguration('prefix_2', default='R_')
24+
dof = LaunchConfiguration('dof', default=7)
25+
dof_1 = LaunchConfiguration('dof_1', default=dof)
26+
dof_2 = LaunchConfiguration('dof_2', default=dof)
27+
robot_type = LaunchConfiguration('robot_type', default='xarm')
28+
robot_type_1 = LaunchConfiguration('robot_type_1', default=robot_type)
29+
robot_type_2 = LaunchConfiguration('robot_type_2', default=robot_type)
30+
add_gripper = LaunchConfiguration('add_gripper', default=False)
31+
add_gripper_1 = LaunchConfiguration('add_gripper_1', default=add_gripper)
32+
add_gripper_2 = LaunchConfiguration('add_gripper_2', default=add_gripper)
33+
add_vacuum_gripper = LaunchConfiguration('add_vacuum_gripper', default=False)
34+
add_vacuum_gripper_1 = LaunchConfiguration('add_vacuum_gripper_1', default=add_vacuum_gripper)
35+
add_vacuum_gripper_2 = LaunchConfiguration('add_vacuum_gripper_2', default=add_vacuum_gripper)
36+
hw_ns = LaunchConfiguration('hw_ns', default='xarm')
37+
limited = LaunchConfiguration('limited', default=True)
38+
effort_control = LaunchConfiguration('effort_control', default=False)
39+
velocity_control = LaunchConfiguration('velocity_control', default=False)
40+
ros2_control_plugin = LaunchConfiguration('ros2_control_plugin', default='uf_robot_hardware/UFRobotFakeSystemHardware')
41+
node_executable = LaunchConfiguration('node_executable', default='xarm_planner_node')
42+
node_name = LaunchConfiguration('node_name', default=node_executable)
43+
node_parameters = LaunchConfiguration('node_parameters', default={})
44+
use_gripper_node = LaunchConfiguration('use_gripper_node', default=add_gripper)
45+
46+
add_realsense_d435i = LaunchConfiguration('add_realsense_d435i', default=False)
47+
add_realsense_d435i_1 = LaunchConfiguration('add_realsense_d435i_1', default=add_realsense_d435i)
48+
add_realsense_d435i_2 = LaunchConfiguration('add_realsense_d435i_2', default=add_realsense_d435i)
49+
50+
add_d435i_links = LaunchConfiguration('add_d435i_links', default=True)
51+
add_d435i_links_1 = LaunchConfiguration('add_d435i_links_1', default=add_d435i_links)
52+
add_d435i_links_2 = LaunchConfiguration('add_d435i_links_2', default=add_d435i_links)
53+
54+
model1300 = LaunchConfiguration('model1300', default=False)
55+
model1300_1 = LaunchConfiguration('model1300_1', default=model1300)
56+
model1300_2 = LaunchConfiguration('model1300_2', default=model1300)
57+
58+
robot_sn = LaunchConfiguration('robot_sn', default='')
59+
robot_sn_1 = LaunchConfiguration('robot_sn_1', default=robot_sn)
60+
robot_sn_2 = LaunchConfiguration('robot_sn_2', default=robot_sn)
61+
62+
add_other_geometry = LaunchConfiguration('add_other_geometry', default=False)
63+
add_other_geometry_1 = LaunchConfiguration('add_other_geometry_1', default=add_other_geometry)
64+
add_other_geometry_2 = LaunchConfiguration('add_other_geometry_2', default=add_other_geometry)
65+
66+
geometry_type = LaunchConfiguration('geometry_type', default='box')
67+
geometry_type_1 = LaunchConfiguration('geometry_type_1', default=geometry_type)
68+
geometry_type_2 = LaunchConfiguration('geometry_type_2', default=geometry_type)
69+
70+
geometry_mass = LaunchConfiguration('geometry_mass', default=0.1)
71+
geometry_mass_1 = LaunchConfiguration('geometry_mass_1', default=geometry_mass)
72+
geometry_mass_2 = LaunchConfiguration('geometry_mass_2', default=geometry_mass)
73+
74+
geometry_height = LaunchConfiguration('geometry_height', default=0.1)
75+
geometry_height_1 = LaunchConfiguration('geometry_height_1', default=geometry_height)
76+
geometry_height_2 = LaunchConfiguration('geometry_height_2', default=geometry_height)
77+
78+
geometry_radius = LaunchConfiguration('geometry_radius', default=0.1)
79+
geometry_radius_1 = LaunchConfiguration('geometry_radius_1', default=geometry_radius)
80+
geometry_radius_2 = LaunchConfiguration('geometry_radius_2', default=geometry_radius)
81+
82+
geometry_length = LaunchConfiguration('geometry_length', default=0.1)
83+
geometry_length_1 = LaunchConfiguration('geometry_length_1', default=geometry_length)
84+
geometry_length_2 = LaunchConfiguration('geometry_length_2', default=geometry_length)
85+
86+
geometry_width = LaunchConfiguration('geometry_width', default=0.1)
87+
geometry_width_1 = LaunchConfiguration('geometry_width_1', default=geometry_width)
88+
geometry_width_2 = LaunchConfiguration('geometry_width_2', default=geometry_width)
89+
90+
geometry_mesh_filename = LaunchConfiguration('geometry_mesh_filename', default='')
91+
geometry_mesh_filename_1 = LaunchConfiguration('geometry_mesh_filename_1', default=geometry_mesh_filename)
92+
geometry_mesh_filename_2 = LaunchConfiguration('geometry_mesh_filename_2', default=geometry_mesh_filename)
93+
94+
geometry_mesh_origin_xyz = LaunchConfiguration('geometry_mesh_origin_xyz', default='"0 0 0"')
95+
geometry_mesh_origin_xyz_1 = LaunchConfiguration('geometry_mesh_origin_xyz_1', default=geometry_mesh_origin_xyz)
96+
geometry_mesh_origin_xyz_2 = LaunchConfiguration('geometry_mesh_origin_xyz_2', default=geometry_mesh_origin_xyz)
97+
98+
geometry_mesh_origin_rpy = LaunchConfiguration('geometry_mesh_origin_rpy', default='"0 0 0"')
99+
geometry_mesh_origin_rpy_1 = LaunchConfiguration('geometry_mesh_origin_rpy_1', default=geometry_mesh_origin_rpy)
100+
geometry_mesh_origin_rpy_2 = LaunchConfiguration('geometry_mesh_origin_rpy_2', default=geometry_mesh_origin_rpy)
101+
102+
geometry_mesh_tcp_xyz = LaunchConfiguration('geometry_mesh_tcp_xyz', default='"0 0 0"')
103+
geometry_mesh_tcp_xyz_1 = LaunchConfiguration('geometry_mesh_tcp_xyz_1', default=geometry_mesh_tcp_xyz)
104+
geometry_mesh_tcp_xyz_2 = LaunchConfiguration('geometry_mesh_tcp_xyz_2', default=geometry_mesh_tcp_xyz)
105+
106+
geometry_mesh_tcp_rpy = LaunchConfiguration('geometry_mesh_tcp_rpy', default='"0 0 0"')
107+
geometry_mesh_tcp_rpy_1 = LaunchConfiguration('geometry_mesh_tcp_rpy_1', default=geometry_mesh_tcp_rpy)
108+
geometry_mesh_tcp_rpy_2 = LaunchConfiguration('geometry_mesh_tcp_rpy_2', default=geometry_mesh_tcp_rpy)
109+
110+
kinematics_suffix = LaunchConfiguration('kinematics_suffix', default='')
111+
kinematics_suffix_1 = LaunchConfiguration('kinematics_suffix_1', default=kinematics_suffix)
112+
kinematics_suffix_2 = LaunchConfiguration('kinematics_suffix_2', default=kinematics_suffix)
113+
114+
moveit_config_package_name = 'xarm_moveit_config'
115+
xarm_type = '{}{}'.format(robot_type.perform(context), '' if robot_type.perform(context) == 'uf850' else dof.perform(context))
116+
117+
# robot_description_parameters
118+
# xarm_moveit_config/launch/lib/robot_moveit_config_lib.py
119+
mod = load_python_launch_file_as_module(os.path.join(get_package_share_directory(moveit_config_package_name), 'launch', 'lib', 'robot_moveit_config_lib.py'))
120+
get_xarm_robot_description_parameters = getattr(mod, 'get_xarm_robot_description_parameters')
121+
robot_description_parameters = get_xarm_robot_description_parameters(
122+
xacro_urdf_file=PathJoinSubstitution([FindPackageShare('xarm_description'), 'urdf', 'dual_xarm_device.urdf.xacro']),
123+
xacro_srdf_file=PathJoinSubstitution([FindPackageShare(moveit_config_package_name), 'srdf', 'dual_xarm.srdf.xacro']),
124+
urdf_arguments={
125+
'prefix_1': prefix_1,
126+
'prefix_2': prefix_2,
127+
'dof_1': dof_1,
128+
'dof_2': dof_2,
129+
'robot_type_1': robot_type_1,
130+
'robot_type_2': robot_type_2,
131+
'add_gripper_1': add_gripper_1,
132+
'add_gripper_2': add_gripper_2,
133+
'add_vacuum_gripper_1': add_vacuum_gripper_1,
134+
'add_vacuum_gripper_2': add_vacuum_gripper_2,
135+
'hw_ns': hw_ns.perform(context).strip('/'),
136+
'limited': limited,
137+
'effort_control': effort_control,
138+
'velocity_control': velocity_control,
139+
'ros2_control_plugin': ros2_control_plugin,
140+
'add_realsense_d435i_1': add_realsense_d435i_1,
141+
'add_realsense_d435i_2': add_realsense_d435i_2,
142+
'add_d435i_links_1': add_d435i_links_1,
143+
'add_d435i_links_2': add_d435i_links_2,
144+
'model1300_1': model1300_1,
145+
'model1300_2': model1300_2,
146+
'robot_sn_1': robot_sn_1,
147+
'robot_sn_2': robot_sn_2,
148+
'add_other_geometry_1': add_other_geometry_1,
149+
'add_other_geometry_2': add_other_geometry_2,
150+
'geometry_type_1': geometry_type_1,
151+
'geometry_type_2': geometry_type_2,
152+
'geometry_mass_1': geometry_mass_1,
153+
'geometry_mass_2': geometry_mass_2,
154+
'geometry_height_1': geometry_height_1,
155+
'geometry_height_2': geometry_height_2,
156+
'geometry_radius_1': geometry_radius_1,
157+
'geometry_radius_2': geometry_radius_2,
158+
'geometry_length_1': geometry_length_1,
159+
'geometry_length_2': geometry_length_2,
160+
'geometry_width_1': geometry_width_1,
161+
'geometry_width_2': geometry_width_2,
162+
'geometry_mesh_filename_1': geometry_mesh_filename_1,
163+
'geometry_mesh_filename_2': geometry_mesh_filename_2,
164+
'geometry_mesh_origin_xyz_1': geometry_mesh_origin_xyz_1,
165+
'geometry_mesh_origin_xyz_2': geometry_mesh_origin_xyz_2,
166+
'geometry_mesh_origin_rpy_1': geometry_mesh_origin_rpy_1,
167+
'geometry_mesh_origin_rpy_2': geometry_mesh_origin_rpy_2,
168+
'geometry_mesh_tcp_xyz_1': geometry_mesh_tcp_xyz_1,
169+
'geometry_mesh_tcp_xyz_2': geometry_mesh_tcp_xyz_2,
170+
'geometry_mesh_tcp_rpy_1': geometry_mesh_tcp_rpy_1,
171+
'geometry_mesh_tcp_rpy_2': geometry_mesh_tcp_rpy_2,
172+
'kinematics_suffix_1': kinematics_suffix_1,
173+
'kinematics_suffix_2': kinematics_suffix_2,
174+
},
175+
srdf_arguments={
176+
'prefix_1': prefix_1,
177+
'prefix_2': prefix_2,
178+
'dof_1': dof_1,
179+
'dof_2': dof_2,
180+
'robot_type_1': robot_type_1,
181+
'robot_type_2': robot_type_2,
182+
'add_gripper_1': add_gripper_1,
183+
'add_gripper_2': add_gripper_2,
184+
'add_vacuum_gripper_1': add_vacuum_gripper_1,
185+
'add_vacuum_gripper_2': add_vacuum_gripper_2,
186+
'add_other_geometry_1': add_other_geometry_1,
187+
'add_other_geometry_2': add_other_geometry_2,
188+
},
189+
arguments={
190+
'context': context,
191+
'xarm_type': xarm_type
192+
}
193+
)
194+
195+
load_yaml = getattr(mod, 'load_yaml')
196+
197+
kinematics_yaml_1 = load_yaml(moveit_config_package_name, 'config', xarm_type, 'kinematics.yaml')
198+
joint_limits_yaml_1 = load_yaml(moveit_config_package_name, 'config', xarm_type, 'joint_limits.yaml')
199+
200+
xarm_type = '{}{}'.format(robot_type_2.perform(context), '' if robot_type_2.perform(context) == 'uf850' else dof_2.perform(context))
201+
kinematics_yaml_2 = load_yaml(moveit_config_package_name, 'config', xarm_type, 'kinematics.yaml')
202+
joint_limits_yaml_2 = load_yaml(moveit_config_package_name, 'config', xarm_type, 'joint_limits.yaml')
203+
204+
if add_gripper_1.perform(context) in ('True', 'true'):
205+
gripper_joint_limits_yaml_1 = load_yaml(moveit_config_package_name, 'config', '{}_gripper'.format(robot_type_1.perform(context)), 'joint_limits.yaml')
206+
if joint_limits_yaml_1 and gripper_joint_limits_yaml_1:
207+
joint_limits_yaml_1['joint_limits'].update(gripper_joint_limits_yaml_1['joint_limits'])
208+
if add_gripper_2.perform(context) in ('True', 'true'):
209+
gripper_joint_limits_yaml_2 = load_yaml(moveit_config_package_name, 'config', '{}_gripper'.format(robot_type_2.perform(context)), 'joint_limits.yaml')
210+
if joint_limits_yaml_2 and gripper_joint_limits_yaml_2:
211+
joint_limits_yaml_2['joint_limits'].update(gripper_joint_limits_yaml_2['joint_limits'])
212+
213+
214+
add_prefix_to_moveit_params = getattr(mod, 'add_prefix_to_moveit_params')
215+
add_prefix_to_moveit_params(kinematics_yaml=kinematics_yaml_1, joint_limits_yaml=joint_limits_yaml_1, prefix=prefix_1.perform(context))
216+
add_prefix_to_moveit_params(kinematics_yaml=kinematics_yaml_2, joint_limits_yaml=joint_limits_yaml_2, prefix=prefix_2.perform(context))
217+
kinematics_yaml = {}
218+
kinematics_yaml.update(kinematics_yaml_1)
219+
kinematics_yaml.update(kinematics_yaml_2)
220+
joint_limits_yaml = {'joint_limits': {}}
221+
joint_limits_yaml.update(joint_limits_yaml_1['joint_limits'])
222+
joint_limits_yaml.update(joint_limits_yaml_2['joint_limits'])
223+
robot_description_parameters['robot_description_kinematics'] = kinematics_yaml
224+
robot_description_parameters['robot_description_planning'] = joint_limits_yaml
225+
226+
try:
227+
xarm_planner_parameters = json.loads(node_parameters.perform(context))
228+
except:
229+
xarm_planner_parameters = {}
230+
231+
xarm_planner_node = Node(
232+
name=node_name,
233+
package='xarm_planner',
234+
executable=node_executable,
235+
output='screen',
236+
parameters=[
237+
robot_description_parameters,
238+
{
239+
'robot_type_1': robot_type_1,
240+
'robot_type_2': robot_type_2,
241+
'dof_1': dof_1,
242+
'dof_2': dof_2,
243+
'prefix_1': prefix_1,
244+
'prefix_2': prefix_2,
245+
},
246+
xarm_planner_parameters,
247+
],
248+
)
249+
250+
nodes = [
251+
xarm_planner_node
252+
]
253+
if add_gripper.perform(context) in ('True', 'true') and use_gripper_node.perform(context) in ('True', 'true'):
254+
xarm_gripper_planner_node = Node(
255+
name=node_name,
256+
package='xarm_planner',
257+
executable='xarm_gripper_planner_node',
258+
output='screen',
259+
parameters=[
260+
robot_description_parameters,
261+
{'PLANNING_GROUP': 'xarm_gripper'},
262+
],
263+
)
264+
nodes.append(xarm_gripper_planner_node)
265+
return nodes
266+
267+
268+
def generate_launch_description():
269+
return LaunchDescription([
270+
OpaqueFunction(function=launch_setup)
271+
])

‎xarm_planner/launch/test_dual_xarm_planner_api_joint.launch.py‎

Lines changed: 61 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,61 @@
1+
#!/usr/bin/env python3
2+
# Software License Agreement (BSD License)
3+
#
4+
# Copyright (c) 2021, UFACTORY, Inc.
5+
# All rights reserved.
6+
#
7+
# Author: Vinman <[email protected]> <[email protected]>
8+
9+
import os
10+
import json
11+
from ament_index_python import get_package_share_directory
12+
from launch.launch_description_sources import load_python_launch_file_as_module
13+
from launch import LaunchDescription
14+
from launch.actions import OpaqueFunction
15+
from launch.actions import IncludeLaunchDescription
16+
from launch.launch_description_sources import PythonLaunchDescriptionSource
17+
from launch.substitutions import LaunchConfiguration, PathJoinSubstitution
18+
from launch_ros.actions import Node
19+
from launch_ros.substitutions import FindPackageShare
20+
21+
22+
def launch_setup(context, *args, **kwargs):
23+
hw_ns = LaunchConfiguration('hw_ns', default='xarm')
24+
limited = LaunchConfiguration('limited', default=False)
25+
effort_control = LaunchConfiguration('effort_control', default=False)
26+
velocity_control = LaunchConfiguration('velocity_control', default=False)
27+
add_gripper = LaunchConfiguration('add_gripper', default=False)
28+
add_vacuum_gripper = LaunchConfiguration('add_vacuum_gripper', default=False)
29+
dof = LaunchConfiguration('dof')
30+
robot_type = LaunchConfiguration('robot_type', default='xarm')
31+
32+
node_executable = 'test_dual_xarm_planner_api_joint'
33+
node_parameters = {}
34+
35+
# robot planner launch
36+
# xarm_planner/launch/_robot_planner.launch.py
37+
robot_planner_node_launch = IncludeLaunchDescription(
38+
PythonLaunchDescriptionSource(PathJoinSubstitution([FindPackageShare('xarm_planner'), 'launch', '_dual_robot_planner.launch.py'])),
39+
launch_arguments={
40+
'hw_ns': hw_ns,
41+
'limited': limited,
42+
'effort_control': effort_control,
43+
'velocity_control': velocity_control,
44+
'add_gripper': add_gripper,
45+
'add_vacuum_gripper': add_vacuum_gripper,
46+
'dof': dof,
47+
'robot_type': robot_type,
48+
'node_executable': node_executable,
49+
'node_parameters': json.dumps(node_parameters)
50+
}.items(),
51+
)
52+
53+
return [
54+
robot_planner_node_launch
55+
]
56+
57+
58+
def generate_launch_description():
59+
return LaunchDescription([
60+
OpaqueFunction(function=launch_setup)
61+
])

0 commit comments

Comments
 (0)