forked from Kinovarobotics/ros_kortex
-
Notifications
You must be signed in to change notification settings - Fork 0
/
Copy pathkortex_dual_driver.launch
129 lines (111 loc) · 6.62 KB
/
kortex_dual_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
<?xml version="1.0"?>
<launch>
<!-- Namespace -->
<!-- Automatically start other modules -->
<arg name="start_rviz" default="true"/>
<arg name="start_moveit" default="true"/>
<arg name="cyclic_data_publish_rate" default="100"/> <!--Hz-->
<!-- LEFT ARM -->
<arg name="left_robot_name" default="my_left_arm"/>
<arg name="left_prefix" default="left_"/>
<arg name="left_arm" default="gen3"/>
<arg name="left_dof" default="7" if="$(eval arg('left_arm') == 'gen3')"/> <!-- Number of degrees of freedom of the arm -->
<arg name="left_dof" default="6" if="$(eval arg('left_arm') == 'gen3_lite')"/> <!-- Number of degrees of freedom of the arm -->
<arg name="left_vision" default="true"/> <!-- True if the arm has a Vision module -->
<!-- Gripper configuration -->
<arg name="left_gripper" default=""/>
<!-- Kortex API options -->
<arg name="left_ip_address" default="192.168.1.10"/>
<arg name="left_username" default="admin"/>
<arg name="left_password" default="admin"/>
<arg name="left_cyclic_data_publish_rate" default="$(arg cyclic_data_publish_rate)"/> <!--Hz-->
<arg name="left_api_rpc_timeout_ms" default="2000"/> <!--milliseconds-->
<arg name="left_api_session_inactivity_timeout_ms" default="35000"/> <!--milliseconds-->
<arg name="left_api_connection_inactivity_timeout_ms" default="20000"/> <!--milliseconds-->
<!-- Action server params -->
<arg name="left_default_goal_time_tolerance" default="0.5"/> <!--seconds-->
<arg name="left_default_goal_tolerance" default="0.005"/> <!--radians-->
<!-- RIGHT ARM -->
<arg name="right_robot_name" default="my_right_arm"/>
<arg name="right_prefix" default="right_"/>
<arg name="right_arm" default="gen3"/>
<arg name="right_dof" default="7" if="$(eval arg('right_arm') == 'gen3')"/> <!-- Number of degrees of freedom of the arm -->
<arg name="right_dof" default="6" if="$(eval arg('right_arm') == 'gen3_lite')"/> <!-- Number of degrees of freedom of the arm -->
<arg name="right_vision" default="true"/> <!-- True if the arm has a Vision module -->
<!-- Gripper configuration -->
<arg name="right_gripper" default=""/>
<!-- Kortex API options -->
<arg name="right_ip_address" default="192.168.1.10"/>
<arg name="right_username" default="admin"/>
<arg name="right_password" default="admin"/>
<arg name="right_cyclic_data_publish_rate" default="$(arg cyclic_data_publish_rate)"/> <!--Hz-->
<arg name="right_api_rpc_timeout_ms" default="2000"/> <!--milliseconds-->
<arg name="right_api_session_inactivity_timeout_ms" default="35000"/> <!--milliseconds-->
<arg name="right_api_connection_inactivity_timeout_ms" default="20000"/> <!--milliseconds-->
<!-- Action server params -->
<arg name="right_default_goal_time_tolerance" default="0.5"/> <!--seconds-->
<arg name="right_default_goal_tolerance" default="0.005"/> <!--radians-->
<!-- Load arms descriptions -->
<param
name="robot_description"
command="$(find xacro)/xacro --inorder $(find kortex_description)/multiple_robots/kortex_dual_robots.xacro
left_arm:=$(arg left_arm)
left_gripper:=$(arg left_gripper)
left_dof:=$(arg left_dof)
left_vision:=$(arg left_vision)
left_sim:=false
left_prefix:=$(arg left_prefix)
right_arm:=$(arg right_arm)
right_gripper:=$(arg right_gripper)
right_dof:=$(arg right_dof)
right_vision:=$(arg right_vision)
right_sim:=false
right_prefix:=$(arg right_prefix)"
/>
<!-- Start the left arm kortex_driver node -->
<include file="$(find kortex_driver)/launch/kortex_arm_driver.launch">
<arg name="sim" value="false"/>
<arg name="ip_address" value="$(arg left_ip_address)"/>
<arg name="username" value="$(arg left_username)"/>
<arg name="password" value="$(arg left_password)"/>
<arg name="cyclic_data_publish_rate" value="$(arg left_cyclic_data_publish_rate)"/>
<arg name="api_rpc_timeout_ms" value="$(arg left_api_rpc_timeout_ms)"/>
<arg name="api_session_inactivity_timeout_ms" value="$(arg left_api_session_inactivity_timeout_ms)"/>
<arg name="api_connection_inactivity_timeout_ms" value="$(arg left_api_connection_inactivity_timeout_ms)"/>
<arg name="default_goal_time_tolerance" value="$(arg left_default_goal_time_tolerance)"/>
<arg name="default_goal_tolerance" value="$(arg left_default_goal_tolerance)"/>
<arg name="arm" value="$(arg left_arm)"/>
<arg name="gripper" value="$(arg left_gripper)"/>
<arg name="dof" value="$(arg left_dof)"/>
<arg name="prefix" value="$(arg left_prefix)"/>
<arg name="robot_name" value="$(arg left_robot_name)"/>
</include>
<!-- Start the right arm kortex_driver node -->
<include file="$(find kortex_driver)/launch/kortex_arm_driver.launch">
<arg name="sim" value="false"/>
<arg name="ip_address" value="$(arg right_ip_address)"/>
<arg name="username" value="$(arg right_username)"/>
<arg name="password" value="$(arg right_password)"/>
<arg name="cyclic_data_publish_rate" value="$(arg right_cyclic_data_publish_rate)"/>
<arg name="api_rpc_timeout_ms" value="$(arg right_api_rpc_timeout_ms)"/>
<arg name="api_session_inactivity_timeout_ms" value="$(arg right_api_session_inactivity_timeout_ms)"/>
<arg name="api_connection_inactivity_timeout_ms" value="$(arg right_api_connection_inactivity_timeout_ms)"/>
<arg name="default_goal_time_tolerance" value="$(arg right_default_goal_time_tolerance)"/>
<arg name="default_goal_tolerance" value="$(arg right_default_goal_tolerance)"/>
<arg name="arm" value="$(arg right_arm)"/>
<arg name="gripper" value="$(arg right_gripper)"/>
<arg name="dof" value="$(arg right_dof)"/>
<arg name="prefix" value="$(arg right_prefix)"/>
<arg name="robot_name" value="$(arg right_robot_name)"/>
</include>
<!-- Start joint and robot state publisher -->
<node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher">
<param name="rate" value="$(arg cyclic_data_publish_rate)" />
<rosparam param="source_list" subst_value="true">
[$(arg left_robot_name)/base_feedback/joint_state, $(arg right_robot_name)/base_feedback/joint_state]
</rosparam>
</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="world" if="$(arg start_rviz)"/>
</launch>