-
Notifications
You must be signed in to change notification settings - Fork 11
/
Copy pathrgbd_mapping.launch
368 lines (318 loc) · 25.5 KB
/
rgbd_mapping.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
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298
299
300
301
302
303
304
305
306
307
308
309
310
311
312
313
314
315
316
317
318
319
320
321
322
323
324
325
326
327
328
329
330
331
332
333
334
335
336
337
338
339
340
341
342
343
344
345
346
347
348
349
350
351
352
353
354
355
356
357
358
359
360
361
362
363
364
365
366
367
368
<launch>
<!-- Convenience launch file to launch odometry, rtabmap and rtabmapviz nodes at once -->
<!-- For stereo:=false
Your RGB-D sensor should be already started with "depth_registration:=true".
Examples:
$ roslaunch freenect_launch freenect.launch depth_registration:=true
$ roslaunch openni2_launch openni2.launch depth_registration:=true -->
<!-- For stereo:=true
Your camera should be calibrated and publishing rectified left and right
images + corresponding camera_info msgs. You can use stereo_image_proc for image rectification.
Example:
$ roslaunch rtabmap_ros bumblebee.launch -->
<!-- Choose between RGB-D and stereo -->
<arg name="stereo" default="false"/>
<!-- Choose visualization -->
<arg name="rtabmapviz" default="false" />
<arg name="rviz" default="false" />
<!-- Localization-only mode -->
<arg name="localization" default="false"/>
<!-- sim time for convenience, if playing a rosbag -->
<arg name="use_sim_time" default="false"/>
<param if="$(arg use_sim_time)" name="use_sim_time" value="true"/>
<!-- Corresponding config files -->
<arg name="cfg" default="" /> <!-- To change RTAB-Map's parameters, set the path of config file (*.ini) generated by the standalone app -->
<arg name="gui_cfg" default="~/.ros/rtabmap_gui.ini" />
<arg name="rviz_cfg" default="$(find rtabmap_ros)/launch/config/rgbd.rviz" />
<arg name="frame_id" default="realsense/front/camera_link"/> <!-- Fixed frame id, you may set "base_link" or "base_footprint" if they are published -->
<arg name="odom_frame_id" default=""/> <!-- If set, TF is used to get odometry instead of the topic -->
<arg name="map_frame_id" default="map"/>
<arg name="ground_truth_frame_id" default=""/> <!-- e.g., "world" -->
<arg name="ground_truth_base_frame_id" default=""/> <!-- e.g., "tracker", a fake frame matching the frame "frame_id" (but on different TF tree) -->
<arg name="namespace" default="rtabmap"/>
<arg name="database_path" default="~/.ros/rtabmap.db"/>
<arg name="queue_size" default="5"/>
<arg name="wait_for_transform" default="0.2"/>
<arg name="args" default=""/> <!-- delete_db_on_start, udebug -->
<arg name="rtabmap_args" default="$(arg args)"/> <!-- deprecated, use "args" argument -->
<arg name="launch_prefix" default=""/> <!-- for debugging purpose, it fills launch-prefix tag of the nodes -->
<arg name="output" default="screen"/> <!-- Control node output (screen or log) -->
<arg name="publish_tf_map" default="true"/>
<!-- if timestamps of the input topics are synchronized using approximate or exact time policy-->
<arg if="$(arg stereo)" name="approx_sync" default="false"/>
<arg unless="$(arg stereo)" name="approx_sync" default="true"/>
<!-- RGB-D related topics -->
<arg name="rgb_topic" default="/realsense/front/color/image_raw" />
<arg name="depth_topic" default="/realsense/front/depth/image_rect_raw" />
<arg name="camera_info_topic" default="/realsense/front/color/camera_info" />
<arg name="depth_camera_info_topic" default="/realsense/front/depth/camera_info" />
<!-- stereo related topics -->
<arg name="stereo_namespace" default="/stereo_camera"/>
<arg name="left_image_topic" default="$(arg stereo_namespace)/left/image_rect_color" />
<arg name="right_image_topic" default="$(arg stereo_namespace)/right/image_rect" /> <!-- using grayscale image for efficiency -->
<arg name="left_camera_info_topic" default="$(arg stereo_namespace)/left/camera_info" />
<arg name="right_camera_info_topic" default="$(arg stereo_namespace)/right/camera_info" />
<!-- Already synchronized RGB-D related topic, with rtabmap_ros/rgbd_sync nodelet -->
<arg name="rgbd_sync" default="false"/> <!-- pre-sync rgb_topic, depth_topic, camera_info_topic -->
<arg name="approx_rgbd_sync" default="true"/> <!-- false=exact synchronization -->
<arg name="subscribe_rgbd" default="$(arg rgbd_sync)"/>
<arg name="rgbd_topic" default="rgbd_image" />
<arg name="depth_scale" default="1.0" />
<arg name="compressed" default="false"/> <!-- If you want to subscribe to compressed image topics -->
<arg name="rgb_image_transport" default="compressed"/> <!-- Common types: compressed, theora (see "rosrun image_transport list_transports") -->
<arg name="depth_image_transport" default="compressedDepth"/> <!-- Depth compatible types: compressedDepth (see "rosrun image_transport list_transports") -->
<arg name="subscribe_scan" default="false"/>
<arg name="scan_topic" default="/scan"/>
<arg name="subscribe_scan_cloud" default="true"/>
<arg name="scan_cloud_topic" default="/velodyne_points"/>
<arg name="scan_normal_k" default="0"/>
<arg name="visual_odometry" default="true"/> <!-- Launch rtabmap visual odometry node -->
<arg name="icp_odometry" default="true"/> <!-- Launch rtabmap icp odometry node -->
<arg name="odom_topic" default="odom"/> <!-- Odometry topic name -->
<arg name="vo_frame_id" default="$(arg odom_topic)"/> <!-- Visual/Icp odometry frame ID for TF -->
<arg name="publish_tf_odom" default="true"/>
<arg name="odom_tf_angular_variance" default="1"/> <!-- If TF is used to get odometry, this is the default angular variance -->
<arg name="odom_tf_linear_variance" default="1"/> <!-- If TF is used to get odometry, this is the default linear variance -->
<arg name="odom_args" default=""/> <!-- More arguments for odometry (overwrite same parameters in rtabmap_args) -->
<arg name="odom_sensor_sync" default="false"/>
<arg name="odom_guess_frame_id" default=""/>
<arg name="odom_guess_min_translation" default="0"/>
<arg name="odom_guess_min_rotation" default="0"/>
<arg name="imu_topic" default="/hyqreal/imu"/> <!-- only used with VIO approaches -->
<arg name="wait_imu_to_init" default="false"/>
<arg name="subscribe_user_data" default="false"/> <!-- user data synchronized subscription -->
<arg name="user_data_topic" default="/user_data"/>
<arg name="user_data_async_topic" default="/user_data_async" /> <!-- user data async subscription (rate should be lower than map update rate) -->
<arg name="gps_topic" default="/gps/fix" /> <!-- gps async subscription -->
<arg name="tag_topic" default="/tag_detections" /> <!-- apriltags async subscription -->
<arg name="tag_linear_variance" default="0.0001" />
<arg name="tag_angular_variance" default="9999" /> <!-- >=9999 means ignore rotation in optimization, when rotation estimation of the tag is not reliable -->
<!-- These arguments should not be modified directly, see referred topics without "_relay" suffix above -->
<arg if="$(arg compressed)" name="rgb_topic_relay" default="$(arg rgb_topic)_relay"/>
<arg unless="$(arg compressed)" name="rgb_topic_relay" default="$(arg rgb_topic)"/>
<arg if="$(arg compressed)" name="depth_topic_relay" default="$(arg depth_topic)_relay"/>
<arg unless="$(arg compressed)" name="depth_topic_relay" default="$(arg depth_topic)"/>
<arg if="$(arg compressed)" name="left_image_topic_relay" default="$(arg left_image_topic)_relay"/>
<arg unless="$(arg compressed)" name="left_image_topic_relay" default="$(arg left_image_topic)"/>
<arg if="$(arg compressed)" name="right_image_topic_relay" default="$(arg right_image_topic)_relay"/>
<arg unless="$(arg compressed)" name="right_image_topic_relay" default="$(arg right_image_topic)"/>
<arg if="$(arg rgbd_sync)" name="rgbd_topic_relay" default="$(arg rgbd_topic)"/>
<arg unless="$(arg rgbd_sync)" name="rgbd_topic_relay" default="$(arg rgbd_topic)_relay"/>
<node pkg="tf" type="static_transform_publisher" name="link1_broadcaster" args="-0.349709 0.0 0.277910 0 ${-15*PI/180} 0 base_link velodyne 100" />
<!-- Nodes -->
<group ns="$(arg namespace)">
<!-- relays -->
<group unless="$(arg stereo)">
<group unless="$(arg subscribe_rgbd)">
<node if="$(arg compressed)" name="republish_rgb" type="republish" pkg="image_transport" args="$(arg rgb_image_transport) in:=$(arg rgb_topic) raw out:=$(arg rgb_topic_relay)" />
<node if="$(arg compressed)" name="republish_depth" type="republish" pkg="image_transport" args="$(arg depth_image_transport) in:=$(arg depth_topic) raw out:=$(arg depth_topic_relay)" />
</group>
<group if="$(arg rgbd_sync)">
<node if="$(arg compressed)" name="republish_rgb" type="republish" pkg="image_transport" args="$(arg rgb_image_transport) in:=$(arg rgb_topic) raw out:=$(arg rgb_topic_relay)" />
<node if="$(arg compressed)" name="republish_depth" type="republish" pkg="image_transport" args="$(arg depth_image_transport) in:=$(arg depth_topic) raw out:=$(arg depth_topic_relay)" />
<node pkg="nodelet" type="nodelet" name="rgbd_sync" args="standalone rtabmap_ros/rgbd_sync" output="$(arg output)">
<remap from="rgb/image" to="$(arg rgb_topic_relay)"/>
<remap from="depth/image" to="$(arg depth_topic_relay)"/>
<remap from="rgb/camera_info" to="$(arg camera_info_topic)"/>
<remap from="rgbd_image" to="$(arg rgbd_topic_relay)"/>
<param name="approx_sync" type="bool" value="$(arg approx_rgbd_sync)"/>
<param name="queue_size" type="int" value="$(arg queue_size)"/>
<param name="depth_scale" type="double" value="$(arg depth_scale)"/>
</node>
</group>
</group>
<group if="$(arg stereo)">
<group unless="$(arg subscribe_rgbd)">
<node if="$(arg compressed)" name="republish_left" type="republish" pkg="image_transport" args="$(arg rgb_image_transport) in:=$(arg left_image_topic) raw out:=$(arg left_image_topic_relay)" />
<node if="$(arg compressed)" name="republish_right" type="republish" pkg="image_transport" args="$(arg rgb_image_transport) in:=$(arg right_image_topic) raw out:=$(arg right_image_topic_relay)" />
</group>
<group if="$(arg rgbd_sync)">
<node if="$(arg compressed)" name="republish_left" type="republish" pkg="image_transport" args="$(arg rgb_image_transport) in:=$(arg left_image_topic) raw out:=$(arg left_image_topic_relay)" />
<node if="$(arg compressed)" name="republish_right" type="republish" pkg="image_transport" args="$(arg rgb_image_transport) in:=$(arg right_image_topic) raw out:=$(arg right_image_topic_relay)" />
<node pkg="nodelet" type="nodelet" name="rgbd_sync" args="standalone rtabmap_ros/stereo_sync" output="$(arg output)">
<remap from="left/image_rect" to="$(arg left_image_topic_relay)"/>
<remap from="right/image_rect" to="$(arg right_image_topic_relay)"/>
<remap from="left/camera_info" to="$(arg left_camera_info_topic)"/>
<remap from="right/camera_info" to="$(arg right_camera_info_topic)"/>
<remap from="rgbd_image" to="$(arg rgbd_topic_relay)"/>
<param name="approx_sync" type="bool" value="$(arg approx_rgbd_sync)"/>
<param name="queue_size" type="int" value="$(arg queue_size)"/>
</node>
</group>
</group>
<group unless="$(arg rgbd_sync)">
<group if="$(arg subscribe_rgbd)">
<node name="republish_rgbd_image" type="rgbd_relay" pkg="rtabmap_ros">
<remap if="$(arg compressed)" from="rgbd_image" to="$(arg rgbd_topic)/compressed"/>
<remap if="$(arg compressed)" from="$(arg rgbd_topic)/compressed_relay" to="$(arg rgbd_topic_relay)"/>
<remap unless="$(arg compressed)" from="rgbd_image" to="$(arg rgbd_topic)"/>
<param if="$(arg compressed)" name="uncompress" value="true"/>
</node>
</group>
</group>
<!-- Visual odometry -->
<group unless="$(arg icp_odometry)">
<group if="$(arg visual_odometry)">
<!-- RGB-D Odometry -->
<node unless="$(arg stereo)" pkg="rtabmap_ros" type="rgbd_odometry" name="rgbd_odometry" output="$(arg output)" args="$(arg rtabmap_args) $(arg odom_args)" launch-prefix="$(arg launch_prefix)">
<remap from="rgb/image" to="$(arg rgb_topic_relay)"/>
<remap from="depth/image" to="$(arg depth_topic_relay)"/>
<remap from="rgb/camera_info" to="$(arg camera_info_topic)"/>
<remap from="rgbd_image" to="$(arg rgbd_topic_relay)"/>
<remap from="odom" to="$(arg odom_topic)"/>
<remap from="imu" to="$(arg imu_topic)"/>
<param name="frame_id" type="string" value="$(arg frame_id)"/>
<param name="odom_frame_id" type="string" value="$(arg vo_frame_id)"/>
<param name="publish_tf" type="bool" value="$(arg publish_tf_odom)"/>
<param name="ground_truth_frame_id" type="string" value="$(arg ground_truth_frame_id)"/>
<param name="ground_truth_base_frame_id" type="string" value="$(arg ground_truth_base_frame_id)"/>
<param name="wait_for_transform_duration" type="double" value="$(arg wait_for_transform)"/>
<param name="wait_imu_to_init" type="bool" value="$(arg wait_imu_to_init)"/>
<param name="approx_sync" type="bool" value="$(arg approx_sync)"/>
<param name="config_path" type="string" value="$(arg cfg)"/>
<param name="queue_size" type="int" value="$(arg queue_size)"/>
<param name="subscribe_rgbd" type="bool" value="$(arg subscribe_rgbd)"/>
<param name="guess_frame_id" type="string" value="$(arg odom_guess_frame_id)"/>
<param name="guess_min_translation" type="double" value="$(arg odom_guess_min_translation)"/>
<param name="guess_min_rotation" type="double" value="$(arg odom_guess_min_rotation)"/>
</node>
<!-- Stereo Odometry -->
<node if="$(arg stereo)" pkg="rtabmap_ros" type="stereo_odometry" name="stereo_odometry" output="$(arg output)" args="$(arg rtabmap_args) $(arg odom_args)" launch-prefix="$(arg launch_prefix)">
<remap from="left/image_rect" to="$(arg left_image_topic_relay)"/>
<remap from="right/image_rect" to="$(arg right_image_topic_relay)"/>
<remap from="left/camera_info" to="$(arg left_camera_info_topic)"/>
<remap from="right/camera_info" to="$(arg right_camera_info_topic)"/>
<remap from="rgbd_image" to="$(arg rgbd_topic_relay)"/>
<remap from="odom" to="$(arg odom_topic)"/>
<remap from="imu" to="$(arg imu_topic)"/>
<param name="frame_id" type="string" value="$(arg frame_id)"/>
<param name="odom_frame_id" type="string" value="$(arg vo_frame_id)"/>
<param name="publish_tf" type="bool" value="$(arg publish_tf_odom)"/>
<param name="ground_truth_frame_id" type="string" value="$(arg ground_truth_frame_id)"/>
<param name="ground_truth_base_frame_id" type="string" value="$(arg ground_truth_base_frame_id)"/>
<param name="wait_for_transform_duration" type="double" value="$(arg wait_for_transform)"/>
<param name="wait_imu_to_init" type="bool" value="$(arg wait_imu_to_init)"/>
<param name="approx_sync" type="bool" value="$(arg approx_sync)"/>
<param name="config_path" type="string" value="$(arg cfg)"/>
<param name="queue_size" type="int" value="$(arg queue_size)"/>
<param name="subscribe_rgbd" type="bool" value="$(arg subscribe_rgbd)"/>
<param name="guess_frame_id" type="string" value="$(arg odom_guess_frame_id)"/>
<param name="guess_min_translation" type="double" value="$(arg odom_guess_min_translation)"/>
<param name="guess_min_rotation" type="double" value="$(arg odom_guess_min_rotation)"/>
</node>
</group>
</group>
<!-- ICP Odometry -->
<node if="$(arg icp_odometry)" pkg="rtabmap_ros" type="icp_odometry" name="icp_odometry" output="$(arg output)" args="$(arg rtabmap_args) $(arg odom_args)" launch-prefix="$(arg launch_prefix)">
<remap from="scan" to="$(arg scan_topic)"/>
<remap from="scan_cloud" to="$(arg scan_cloud_topic)"/>
<remap from="odom" to="$(arg odom_topic)"/>
<remap from="imu" to="$(arg imu_topic)"/>
<param name="frame_id" type="string" value="$(arg frame_id)"/>
<param name="odom_frame_id" type="string" value="$(arg vo_frame_id)"/>
<param name="publish_tf" type="bool" value="$(arg publish_tf_odom)"/>
<param name="ground_truth_frame_id" type="string" value="$(arg ground_truth_frame_id)"/>
<param name="ground_truth_base_frame_id" type="string" value="$(arg ground_truth_base_frame_id)"/>
<param name="wait_for_transform_duration" type="double" value="$(arg wait_for_transform)"/>
<param name="wait_imu_to_init" type="bool" value="$(arg wait_imu_to_init)"/>
<param name="config_path" type="string" value="$(arg cfg)"/>
<param name="queue_size" type="int" value="$(arg queue_size)"/>
<param name="guess_frame_id" type="string" value="$(arg odom_guess_frame_id)"/>
<param name="guess_min_translation" type="double" value="$(arg odom_guess_min_translation)"/>
<param name="guess_min_rotation" type="double" value="$(arg odom_guess_min_rotation)"/>
</node>
<!-- Visual SLAM (robot side) -->
<!-- args: "delete_db_on_start" and "udebug" -->
<node name="rtabmap" pkg="rtabmap_ros" type="rtabmap" output="$(arg output)" args="$(arg rtabmap_args)" launch-prefix="$(arg launch_prefix)">
<param if="$(arg stereo)" name="subscribe_depth" type="bool" value="false"/>
<param unless="$(arg stereo)" name="subscribe_depth" type="bool" value="true"/>
<param name="subscribe_rgbd" type="bool" value="$(arg subscribe_rgbd)"/>
<param name="subscribe_stereo" type="bool" value="$(arg stereo)"/>
<param name="subscribe_scan" type="bool" value="$(arg subscribe_scan)"/>
<param name="subscribe_scan_cloud" type="bool" value="$(arg subscribe_scan_cloud)"/>
<param name="subscribe_user_data" type="bool" value="$(arg subscribe_user_data)"/>
<param if="$(arg visual_odometry)" name="subscribe_odom_info" type="bool" value="true"/>
<param if="$(arg icp_odometry)" name="subscribe_odom_info" type="bool" value="true"/>
<param name="frame_id" type="string" value="$(arg frame_id)"/>
<param name="map_frame_id" type="string" value="$(arg map_frame_id)"/>
<param name="odom_frame_id" type="string" value="$(arg odom_frame_id)"/>
<param name="publish_tf" type="bool" value="$(arg publish_tf_map)"/>
<param name="ground_truth_frame_id" type="string" value="$(arg ground_truth_frame_id)"/>
<param name="ground_truth_base_frame_id" type="string" value="$(arg ground_truth_base_frame_id)"/>
<param name="odom_tf_angular_variance" type="double" value="$(arg odom_tf_angular_variance)"/>
<param name="odom_tf_linear_variance" type="double" value="$(arg odom_tf_linear_variance)"/>
<param name="odom_sensor_sync" type="bool" value="$(arg odom_sensor_sync)"/>
<param name="wait_for_transform_duration" type="double" value="$(arg wait_for_transform)"/>
<param name="database_path" type="string" value="$(arg database_path)"/>
<param name="approx_sync" type="bool" value="$(arg approx_sync)"/>
<param name="config_path" type="string" value="$(arg cfg)"/>
<param name="queue_size" type="int" value="$(arg queue_size)"/>
<param name="scan_normal_k" type="int" value="$(arg scan_normal_k)"/>
<param name="landmark_linear_variance" type="double" value="$(arg tag_linear_variance)"/>
<param name="landmark_angular_variance" type="double" value="$(arg tag_angular_variance)"/>
<remap from="rgb/image" to="$(arg rgb_topic_relay)"/>
<remap from="depth/image" to="$(arg depth_topic_relay)"/>
<remap from="rgb/camera_info" to="$(arg camera_info_topic)"/>
<remap from="rgbd_image" to="$(arg rgbd_topic_relay)"/>
<remap from="left/image_rect" to="$(arg left_image_topic_relay)"/>
<remap from="right/image_rect" to="$(arg right_image_topic_relay)"/>
<remap from="left/camera_info" to="$(arg left_camera_info_topic)"/>
<remap from="right/camera_info" to="$(arg right_camera_info_topic)"/>
<remap from="scan" to="$(arg scan_topic)"/>
<remap from="scan_cloud" to="$(arg scan_cloud_topic)"/>
<remap from="user_data" to="$(arg user_data_topic)"/>
<remap from="user_data_async" to="$(arg user_data_async_topic)"/>
<remap from="gps/fix" to="$(arg gps_topic)"/>
<remap from="tag_detections" to="$(arg tag_topic)"/>
<remap from="odom" to="$(arg odom_topic)"/>
<remap from="imu" to="$(arg imu_topic)"/>
<!-- localization mode -->
<param if="$(arg localization)" name="Mem/IncrementalMemory" type="string" value="false"/>
<param unless="$(arg localization)" name="Mem/IncrementalMemory" type="string" value="true"/>
<param name="Mem/InitWMWithAllNodes" type="string" value="$(arg localization)"/>
</node>
<!-- Visualisation RTAB-Map -->
<node if="$(arg rtabmapviz)" pkg="rtabmap_ros" type="rtabmapviz" name="rtabmapviz" args="-d $(arg gui_cfg)" output="$(arg output)" launch-prefix="$(arg launch_prefix)">
<param if="$(arg stereo)" name="subscribe_depth" type="bool" value="false"/>
<param unless="$(arg stereo)" name="subscribe_depth" type="bool" value="true"/>
<param name="subscribe_rgbd" type="bool" value="$(arg subscribe_rgbd)"/>
<param name="subscribe_stereo" type="bool" value="$(arg stereo)"/>
<param name="subscribe_scan" type="bool" value="$(arg subscribe_scan)"/>
<param name="subscribe_scan_cloud" type="bool" value="$(arg subscribe_scan_cloud)"/>
<param if="$(arg visual_odometry)" name="subscribe_odom_info" type="bool" value="true"/>
<param if="$(arg icp_odometry)" name="subscribe_odom_info" type="bool" value="true"/>
<param name="frame_id" type="string" value="$(arg frame_id)"/>
<param name="odom_frame_id" type="string" value="$(arg odom_frame_id)"/>
<param name="wait_for_transform_duration" type="double" value="$(arg wait_for_transform)"/>
<param name="queue_size" type="int" value="$(arg queue_size)"/>
<param name="approx_sync" type="bool" value="$(arg approx_sync)"/>
<remap from="rgb/image" to="$(arg rgb_topic_relay)"/>
<remap from="depth/image" to="$(arg depth_topic_relay)"/>
<remap from="rgb/camera_info" to="$(arg camera_info_topic)"/>
<remap from="rgbd_image" to="$(arg rgbd_topic_relay)"/>
<remap from="left/image_rect" to="$(arg left_image_topic_relay)"/>
<remap from="right/image_rect" to="$(arg right_image_topic_relay)"/>
<remap from="left/camera_info" to="$(arg left_camera_info_topic)"/>
<remap from="right/camera_info" to="$(arg right_camera_info_topic)"/>
<remap from="scan" to="$(arg scan_topic)"/>
<remap from="scan_cloud" to="$(arg scan_cloud_topic)"/>
<remap from="odom" to="$(arg odom_topic)"/>
</node>
</group>
<!-- Visualization RVIZ -->
<node if="$(arg rviz)" pkg="rviz" type="rviz" name="rviz" args="-d $(arg rviz_cfg)"/>
<node if="$(arg rviz)" pkg="nodelet" type="nodelet" name="points_xyzrgb" args="standalone rtabmap_ros/point_cloud_xyzrgb" output="$(arg output)">
<remap if="$(arg stereo)" from="left/image" to="$(arg left_image_topic_relay)"/>
<remap if="$(arg stereo)" from="right/image" to="$(arg right_image_topic_relay)"/>
<remap if="$(arg stereo)" from="left/camera_info" to="$(arg left_camera_info_topic)"/>
<remap if="$(arg stereo)" from="right/camera_info" to="$(arg right_camera_info_topic)"/>
<remap unless="$(arg subscribe_rgbd)" from="rgb/image" to="$(arg rgb_topic_relay)"/>
<remap unless="$(arg subscribe_rgbd)" from="depth/image" to="$(arg depth_topic_relay)"/>
<remap unless="$(arg subscribe_rgbd)" from="rgb/camera_info" to="$(arg camera_info_topic)"/>
<remap from="rgbd_image" to="$(arg rgbd_topic_relay)"/>
<remap from="cloud" to="voxel_cloud" />
<param name="decimation" type="double" value="4"/>
<param name="voxel_size" type="double" value="0.0"/>
<param name="approx_sync" type="bool" value="$(arg approx_sync)"/>
</node>
</launch>