forked from Kinovarobotics/ros_kortex
-
Notifications
You must be signed in to change notification settings - Fork 0
/
Copy pathkortex_driver.launch
169 lines (150 loc) · 9.66 KB
/
kortex_driver.launch
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
<?xml version="1.0"?>
<launch>
<!-- Arm configuration -->
<arg name="arm" default="gen3"/>
<arg name="dof" default="7" if="$(eval arg('arm') == 'gen3')"/> <!-- Number of degrees of freedom of the arm -->
<arg name="dof" default="6" if="$(eval arg('arm') == 'gen3_lite')"/> <!-- Number of degrees of freedom of the arm -->
<arg name="vision" default="true"/> <!-- True if the arm has a Vision module -->
<arg name="use_hard_limits" default="false"/> <!-- Set soft limits as hard limits (Gen3 only) -->
<!-- Gripper configuration -->
<!-- Default gripper for Gen3 is none, default gripper for Gen3 lite is gen3_lite_2f -->
<arg name="gripper" default="" if="$(eval arg('arm') == 'gen3')"/>
<arg name="gripper" default="gen3_lite_2f" if="$(eval arg('arm') == 'gen3_lite')"/>
<!-- Namespace -->
<arg name="robot_name" default="my_$(arg arm)"/>
<arg name="prefix" default=""/>
<!-- Kortex API options -->
<arg name="ip_address" default="192.168.1.10"/>
<arg name="username" default="admin"/>
<arg name="password" default="admin"/>
<arg name="cyclic_data_publish_rate" default="40"/> <!--Hz-->
<arg name="api_rpc_timeout_ms" default="2000"/> <!--milliseconds-->
<arg name="api_session_inactivity_timeout_ms" default="35000"/> <!--milliseconds-->
<arg name="api_connection_inactivity_timeout_ms" default="20000"/> <!--milliseconds-->
<!-- Automatically start other modules -->
<arg name="start_rviz" default="true"/>
<arg name="start_moveit" default="true"/>
<!-- Action server params -->
<arg name="default_goal_time_tolerance" default="0.5"/> <!--seconds-->
<arg name="default_goal_tolerance" default="0.005"/> <!--radians-->
<group ns="$(arg robot_name)">
<group if="$(eval not arg('prefix'))">
<!-- Load the description for the robot -->
<!-- Without gripper -->
<param name="robot_description" command="$(find xacro)/xacro --inorder $(find kortex_description)/robots/$(arg arm).xacro dof:=$(arg dof) vision:=$(arg vision) sim:=false"
if="$(eval not arg('gripper'))"/>
<!-- With gripper -->
<param name="robot_description" command="$(find xacro)/xacro --inorder $(find kortex_description)/robots/$(arg arm)_$(arg gripper).xacro dof:=$(arg dof) vision:=$(arg vision) sim:=false"
unless="$(eval not arg('gripper'))"/>
</group>
<group unless="$(eval not arg('prefix'))">
<!-- Load the description for the robot -->
<!-- Without gripper -->
<param name="robot_description" command="$(find xacro)/xacro --inorder $(find kortex_description)/robots/$(arg arm).xacro dof:=$(arg dof) vision:=$(arg vision) sim:=false prefix:=$(arg prefix)"
if="$(eval not arg('gripper'))"/>
<!-- With gripper -->
<param name="robot_description" command="$(find xacro)/xacro --inorder $(find kortex_description)/robots/$(arg arm)_$(arg gripper).xacro dof:=$(arg dof) vision:=$(arg vision) sim:=false prefix:=$(arg prefix)"
unless="$(eval not arg('gripper'))"/>
</group>
<!-- Start the kortex_driver node -->
<node name="$(arg robot_name)_driver" pkg="kortex_driver" type="kortex_arm_driver" output="screen"> <!--launch-prefix="gdb -ex run args"-->
<param name="sim" value="false"/>
<param name="ip_address" value="$(arg ip_address)"/>
<param name="username" value="$(arg username)"/>
<param name="password" value="$(arg password)"/>
<param name="cyclic_data_publish_rate" value="$(arg cyclic_data_publish_rate)"/>
<param name="api_rpc_timeout_ms" value="$(arg api_rpc_timeout_ms)"/>
<param name="api_session_inactivity_timeout_ms" value="$(arg api_session_inactivity_timeout_ms)"/>
<param name="api_connection_inactivity_timeout_ms" value="$(arg api_connection_inactivity_timeout_ms)"/>
<param name="default_goal_time_tolerance" value="$(arg default_goal_time_tolerance)"/>
<param name="default_goal_tolerance" value="$(arg default_goal_tolerance)"/>
<param name="arm" value="$(arg arm)"/>
<param name="gripper" value="$(arg gripper)"/>
<param name="dof" value="$(arg dof)"/>
<param name="use_hard_limits" value="$(arg use_hard_limits)"/>
<param name="robot_name" value="$(arg robot_name)"/>
<param name="prefix" value="$(arg prefix)"/>
<rosparam command="load" file="$(find kortex_description)/arms/$(arg arm)/$(arg dof)dof/config/joint_limits.yaml" subst_value="true"/>
<!-- If there is a gripper, load the active joint names for it -->
<rosparam command="load" file="$(find kortex_description)/grippers/$(arg gripper)/config/joint_limits.yaml" unless="$(eval not arg('gripper'))" subst_value="true"/>
</node>
<!-- Start MoveIt! main executable -->
<group if="$(arg start_moveit)">
<!-- TODO Find cleaner way to do that and that will work with other arms -->
<!-- Without gripper -->
<include file="$(find kortex_description)/../kortex_move_it_config/$(arg arm)_move_it_config/launch/move_group.launch" if="$(eval not arg('gripper'))">
<arg name="dof" value="$(arg dof)"/>
<arg name="use_hard_limits" value="$(arg use_hard_limits)"/>
<arg name="prefix" value="$(arg prefix)"/>
</include>
<!-- With gripper -->
<include file="$(find kortex_description)/../kortex_move_it_config/$(arg arm)_$(arg gripper)_move_it_config/launch/move_group.launch" unless="$(eval not arg('gripper'))">
<arg name="dof" value="$(arg dof)"/>
<arg name="use_hard_limits" value="$(arg use_hard_limits)"/>
<arg name="prefix" value="$(arg prefix)"/>
</include>
</group>
<!-- Start joint and robot state publisher -->
<node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher">
<rosparam param="source_list">[base_feedback/joint_state]</rosparam>
<param name="rate" value="$(arg cyclic_data_publish_rate)" />
</node>
<node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" />
<!-- Start RViz -->
<node name="rviz" pkg="rviz" type="rviz" output="log" args="-f base_link" if="$(arg start_rviz)"/>
<!-- Tests -->
<!-- Initialization test and custom functional tests -->
<test test-name="kortex_driver_tests" pkg="kortex_driver" type="kortex_arm_driver_func_tests">
<param name="ip_address" value="$(arg ip_address)"/>
<param name="cyclic_data_publish_rate" value="$(arg cyclic_data_publish_rate)"/>
<param name="api_rpc_timeout_ms" value="$(arg api_rpc_timeout_ms)"/>
<param name="api_session_inactivity_timeout_ms" value="$(arg api_session_inactivity_timeout_ms)"/>
<param name="api_connection_inactivity_timeout_ms" value="$(arg api_connection_inactivity_timeout_ms)"/>
<param name="default_goal_time_tolerance" value="$(arg default_goal_time_tolerance)"/>
<param name="default_goal_tolerance" value="$(arg default_goal_tolerance)"/>
<param name="arm" value="$(arg arm)"/>
<param name="gripper" value="$(arg gripper)"/>
<param name="dof" value="$(arg dof)"/>
<rosparam command="load" file="$(find kortex_description)/arms/$(arg arm)/$(arg dof)dof/config/joint_limits.yaml" subst_value="true"/>
<!-- If there is a gripper, load the active joint names for it -->
<rosparam command="load" file="$(find kortex_description)/grippers/$(arg gripper)/config/joint_limits.yaml" unless="$(eval not arg('gripper'))" subst_value="true"/>
</test>
<!-- Test feedback and joint state publishing -->
<test test-name="publish_test_kortex_driver" pkg="rostest" type="publishtest">
<rosparam subst_value="true">
topics:
- name: /$(arg robot_name)/base_feedback
timeout: 10
negative: False
- name: /$(arg robot_name)/base_feedback/joint_state
timeout: 10
negative: False
- name: /$(arg robot_name)/joint_states
timeout: 10
negative: False
</rosparam>
</test>
<!-- Test publishers rate -->
<test test-name="hztest_test_base_feedback" pkg="rostest" type="hztest">
<param name="topic" value="base_feedback" />
<param name="hz" value="$(arg cyclic_data_publish_rate)" />
<param name="hzerror" value="5.0" />
<param name="test_duration" value="30.0" />
<param name="wait_time" value="10.0" />
</test>
<test test-name="hztest_test_driver_joint_state" pkg="rostest" type="hztest">
<param name="topic" value="base_feedback/joint_state" />
<param name="hz" value="$(arg cyclic_data_publish_rate)" />
<param name="hzerror" value="5.0" />
<param name="test_duration" value="30.0" />
<param name="wait_time" value="10.0" />
</test>
<test test-name="hztest_test_joint_state_publisher_joint_states" pkg="rostest" type="hztest">
<param name="topic" value="joint_states" />
<param name="hz" value="$(arg cyclic_data_publish_rate)" />
<param name="hzerror" value="5.0" />
<param name="test_duration" value="30.0" />
<param name="wait_time" value="10.0" />
</test>
</group>
</launch>