-
Notifications
You must be signed in to change notification settings - Fork 97
/
Copy pathfetch.launch
198 lines (171 loc) · 8.2 KB
/
fetch.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
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
<launch>
<!-- add arg launch_teleop: yamaguchi & s-kitagawa (2019/04/19) -->
<arg name="launch_teleop" default="true" />
<arg name="launch_moveit" default="true" />
<arg name="use_base_camera_mount" default="true" />
<arg name="use_head_box" default="true" />
<arg name="launch_diagnostics_agg" default="false" />
<arg name="use_head_l515" default="true" />
<arg name="use_realsense" default="true" />
<arg name="use_fetch_description" default="false" />
<!-- realsense serial numbers -->
<arg name="RS_SERIAL_NO_T265" default="$(optenv RS_SERIAL_NO_T265)" />
<arg name="RS_SERIAL_NO_D435_FRONTRIGHT" default="$(optenv RS_SERIAL_NO_D435_FRONTRIGHT)" />
<arg name="RS_SERIAL_NO_D435_FRONTLEFT" default="$(optenv RS_SERIAL_NO_D435_FRONTLEFT)" />
<arg name="RS_SERIAL_NO_L515_HEAD" default="$(optenv RS_SERIAL_NO_L515_HEAD)" />
<!-- GDB Debug Option -->
<arg name="debug" default="false"/>
<arg unless="$(arg debug)" name="launch_prefix" value=""/>
<arg if="$(arg debug)" name="launch_prefix" value="gdb -x $(find fetch_bringup)/launch/gdb_settings.gdb --ex run --args"/>
<!-- calibration: s-kitagawa (2019/12/11) -->
<!-- Calibration -->
<param name="calibration_date" value="2020-12-04 02:08:50"/>
<param name="base_controller/track_width" value="0.37476"/>
<param name="head_camera/driver/z_offset_mm" value="4"/>
<param name="head_camera/driver/z_scaling" value="1.0202112"/>
<!-- use symbolic link: s-kitagawa (2020/09/18) -->
<arg name="rgb_camera_info_url" default="file:///etc/ros/$(env ROS_DISTRO)/depth_latest.yaml"/>
<arg name="depth_camera_info_url" default="file:///etc/ros/$(env ROS_DISTRO)/depth_latest.yaml"/>
<!-- Launch realsense cameras -->
<include file="$(find jsk_fetch_startup)/launch/fetch_realsense_bringup.launch" if="$(arg use_realsense)">
<arg name="respawn" value="true" />
<arg name="RS_SERIAL_NO_T265" value="$(arg RS_SERIAL_NO_T265)" />
<arg name="RS_SERIAL_NO_D435_FRONTRIGHT" value="$(arg RS_SERIAL_NO_D435_FRONTRIGHT)" />
<arg name="RS_SERIAL_NO_D435_FRONTLEFT" value="$(arg RS_SERIAL_NO_D435_FRONTLEFT)" />
<arg name="RS_SERIAL_NO_L515_HEAD" value="$(arg RS_SERIAL_NO_L515_HEAD)" />
</include>
<!-- Odometry -->
<param name="base_controller/publish_tf" value="false"/>
<!-- stop using graft: s-kitagawa (2019/10/17) -->
<!-- <include file="$(find fetch_bringup)/launch/include/graft.launch.xml"/> -->
<!-- Odometry without visual odom -->
<!-- use robot localization ukf -->
<!-- odometry topic mux -->
<node
pkg="topic_tools"
type="mux"
name="odom_topic_mux"
args="odom_combined odom_robot_localization odom_visual"
>
<remap from="/mux" to="/odom_topic_mux" />
<param name="~initial_topic" value="odom_visual" />
</node>
<!-- odometry tf mux -->
<node
pkg="topic_tools"
type="mux"
name="odom_tf_mux"
args="tf tf_odom_robot_localization tf_odom_visual"
>
<remap from="/mux" to="/odom_tf_mux" />
<param name="~initial_topic" value="tf_odom_visual" />
</node>
<!-- odometry mux selector -->
<node
pkg="jsk_fetch_startup"
type="odometry_mux_selector.py"
name="odometry_mux_selector"
>
<param name="~topic_odom_primary" value="odom_visual" />
<param name="~topic_odom_secondary" value="odom_robot_localization" />
<param name="~topic_tf_primary" value="tf_odom_visual" />
<param name="~topic_tf_secondary" value="tf_odom_robot_localization" />
<param name="~duration_timeout_topic" value="10.0" />
<remap from="~select_service_topic" to="/odom_topic_mux/select" />
<remap from="~select_service_tf" to="/odom_tf_mux/select" />
<remap from="~sound_play" to="sound_play" />
</node>
<!-- Odometry with robot localization ukf using wheel and imu -->
<node pkg="robot_localization" type="ukf_localization_node" name="ukf_se" clear_params="true">
<remap from="odometry/filtered" to="/odom_robot_localization" />
<remap from="/tf" to="/tf_odom_robot_localization"/>
<rosparam>
frequency: 50
sensor_timeout: 1.0
two_d_mode: true
publish_tf: true
publish_acceleration: false
map_frame: map
odom_frame: odom
base_link_frame: base_link
odom0: /odom_corrected
odom0_config: [true, true, false,
false, false, true,
true, true, false,
false, false, true,
false, false, false]
odom0_nodelay: true
odom0_differential: true
</rosparam>
</node>
<!-- Visual Odom -->
<include file="$(find jsk_fetch_startup)/launch/fetch_visual_odom.launch">
<arg name="topic_odom_out" value="/odom_visual" />
<arg name="odom_frame_id" value="odom" />
<arg name="topic_tf" value="tf_odom_visual" />
</include>
<!-- /imu has no frame_id information and there is no bug fix release in indigo. -->
<!-- see https://github.com/fetchrobotics/fetch_ros/issues/70 -->
<node name="imu_corrector" pkg="jsk_fetch_startup" type="imu_corrector.py">
<remap from="~input" to="/imu" />
<remap from="~output" to="/imu_corrected" />
</node>
<!-- /odom has no covariance value. -->
<node name="odom_corrector" pkg="jsk_fetch_startup" type="odom_corrector.py">
<remap from="~input" to="/odom" />
<remap from="~output" to="/odom_corrected" />
</node>
<!-- calibration: s-kitagawa (2019/12/11) -->
<!-- testing urdf with head box by shinjo (2020/02/13) -->
<!-- URDF -->
<param name="robot_description"
command="$(find xacro)/xacro $(find jsk_fetch_startup)/robots/jsk_fetch.urdf.xacro
ros_distro:=$(env ROS_DISTRO) use_fetch_description:=$(arg use_fetch_description)
base_camera_mount:=$(arg use_base_camera_mount) head_box:=$(arg use_head_box)
head_l515:=$(arg use_head_l515)" />
<!-- Drivers for Base -->
<node name="robot_driver" launch-prefix="$(arg launch_prefix)" pkg="fetch_drivers" type="robot_driver" output="screen">
<param name="firmware_tar_gz" value="$(find fetch_drivers)/firmware.tar.gz"/>
<param name="has_base" value="true"/>
<param name="has_torso" value="true"/>
<param name="has_head" value="true"/>
<param name="has_arm" value="true"/>
</node>
<!-- Drivers for Gripper -->
<node name="gripper_driver" pkg="fetch_drivers" type="gripper_driver" output="screen">
<param name="firmware_tar_gz" value="$(find fetch_drivers)/firmware.tar.gz"/>
</node>
<!-- Controllers -->
<rosparam file="$(find fetch_bringup)/config/default_controllers.yaml" command="load"/>
<!-- Joint State -> TF -->
<node name="robot_state_publisher" pkg="robot_state_publisher" type="state_publisher"/>
<!-- Head Camera -->
<include file="$(find fetch_bringup)/launch/include/head_camera.launch.xml">
<arg name="rgb_camera_info_url" value="$(arg rgb_camera_info_url)"/>
<arg name="depth_camera_info_url" value="$(arg depth_camera_info_url)"/>
</include>
<!-- Set image_transport_plugins parameters (png_level and disable_pub_plugins) -->
<rosparam file="$(find jsk_fetch_startup)/config/image_transport_plugins_params.yaml"
command="load"/>
<!-- Laser -->
<include file="$(find fetch_bringup)/launch/include/laser.launch.xml"/>
<!-- add arg launch_teleop: yamaguchi & s-kitagawa (2019/04/19) -->
<!-- Teleop -->
<include file="$(find fetch_bringup)/launch/include/teleop.launch.xml" if="$(arg launch_teleop)"/>
<!-- enable software runstop from joy: s-kitagawa (2019/11/09) -->
<!-- Software Runstop -->
<include file="$(find fetch_bringup)/launch/include/runstop.launch.xml">
<arg name="flags" value="-a -b -g -t"/>
</include>
<!-- include fetch moveit -->
<group if="$(arg launch_moveit)">
<include file="$(find fetch_moveit_config)/launch/move_group.launch" />
<!-- overwrite fetch.srdf -->
<!-- The semantic description that corresponds to the URDF -->
<param name="robot_description_semantic"
command="rosrun xacro xacro --inorder $(find jsk_fetch_startup)/launch/moveit/fetch.srdf.xacro head_l515:=$(arg use_head_l515)" />
</group>
<!-- Diagnostics Aggregator -->
<include if="$(arg launch_diagnostics_agg)"
file="$(find fetch_bringup)/launch/include/aggregator.launch.xml"/>
</launch>