-
Notifications
You must be signed in to change notification settings - Fork 570
/
Copy pathdemo_multi-session_mapping.launch
96 lines (76 loc) · 4.78 KB
/
demo_multi-session_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
<?xml version="1.0"?>
<launch>
<!-- MULTI-SESSION MAPPING VERSION -->
<!-- Choose visualization -->
<arg name="rviz" default="false" />
<arg name="rtabmap_viz" default="true" />
<arg name="rtabmap_args" default="" />
<param name="use_sim_time" type="bool" value="True"/>
<group ns="rtabmap">
<!-- SLAM (robot side) -->
<!-- args: "delete_db_on_start" and "udebug" -->
<node name="rtabmap" pkg="rtabmap_slam" type="rtabmap" output="screen" args="$(arg rtabmap_args)">
<param name="frame_id" type="string" value="base_footprint"/>
<param name="subscribe_depth" type="bool" value="true"/>
<param name="subscribe_scan" type="bool" value="true"/>
<remap from="odom" to="/base_controller/odom"/>
<remap from="scan" to="/base_scan"/>
<remap from="rgb/image" to="/data_throttled_image"/>
<remap from="depth/image" to="/data_throttled_image_depth"/>
<remap from="rgb/camera_info" to="/data_throttled_camera_info"/>
<param name="rgb/image_transport" type="string" value="compressed"/>
<param name="depth/image_transport" type="string" value="compressedDepth"/>
<param name="queue_size" type="int" value="10"/>
<!-- RTAB-Map's parameters: do "rosrun rtabmap rtabmap (double-dash)params" to see the list of available parameters. -->
<param name="RGBD/NeighborLinkRefining" type="string" value="false"/>
<param name="RGBD/ProximityBySpace" type="string" value="false"/> <!-- Referred paper did only global loop closure detection -->
<param name="RGBD/ProximityByTime" type="string" value="false"/>
<param name="RGBD/OptimizeFromGraphEnd" type="string" value="true"/>
<param name="Reg/Strategy" type="string" value="1"/>
<param name="Icp/Iterations" type="string" value="30"/>
<param name="Icp/VoxelSize" type="string" value="0"/>
<param name="Vis/MinInliers" type="string" value="5"/>
<param name="Vis/MaxDepth" type="string" value="4.0"/>
<param name="RGBD/AngularUpdate" type="string" value="0.01"/>
<param name="RGBD/LinearUpdate" type="string" value="0.01"/>
<param name="Rtabmap/TimeThr" type="string" value="700"/>
<param name="Mem/RehearsalSimilarity" type="string" value="0.45"/>
<param name="Kp/TfIdfLikelihoodUsed" type="string" value="false"/>
<param name="Bayes/FullPredictionUpdate" type="string" value="true"/>
<param name="Kp/DetectorStrategy" type="string" value="0"/> <!-- use SURF -->
<param name="Kp/MaxFeatures" type="string" value="400"/>
<param name="Reg/Force3DoF" type="string" value="true"/>
<param name="RGBD/OptimizeMaxError" type="string" value="0.25"/>
<param name="Optimizer/Strategy" type="string" value="0"/> <!-- TORO is the most stable for multi-session mapping -->
<param name="Optimizer/Iterations" type="string" value="100"/>
<param name="Kp/IncrementalFlann" type="string" value="false"/> <!-- Referred paper didn't use incremental FLANN -->
<param name="Grid/FromDepth" type="string" value="false"/>
</node>
<!-- Visualisation RTAB-Map -->
<node if="$(arg rtabmap_viz)" pkg="rtabmap_viz" type="rtabmap_viz" name="rtabmap_viz" args="-d $(find rtabmap_demos)/launch/config/rgbd_gui.ini" output="screen">
<param name="subscribe_depth" type="bool" value="true"/>
<param name="subscribe_scan" type="bool" value="true"/>
<param name="queue_size" type="int" value="10"/>
<param name="frame_id" type="string" value="base_footprint"/>
<remap from="rgb/image" to="/data_throttled_image"/>
<remap from="depth/image" to="/data_throttled_image_depth"/>
<remap from="rgb/camera_info" to="/data_throttled_camera_info"/>
<remap from="scan" to="/base_scan"/>
<remap from="odom" to="/base_controller/odom"/>
<param name="rgb/image_transport" type="string" value="compressed"/>
<param name="depth/image_transport" type="string" value="compressedDepth"/>
</node>
</group>
<!-- Visualisation RVIZ -->
<node if="$(arg rviz)" pkg="rviz" type="rviz" name="rviz" args="-d $(find rtabmap_demos)/launch/config/demo_robot_mapping.rviz"/>
<node pkg="nodelet" type="nodelet" name="points_xyzrgb" args="standalone rtabmap_util/point_cloud_xyzrgb">
<remap from="rgb/image" to="/data_throttled_image"/>
<remap from="depth/image" to="/data_throttled_image_depth"/>
<remap from="rgb/camera_info" to="/data_throttled_camera_info"/>
<remap from="cloud" to="voxel_cloud" />
<param name="rgb/image_transport" type="string" value="compressed"/>
<param name="depth/image_transport" type="string" value="compressedDepth"/>
<param name="queue_size" type="int" value="10"/>
<param name="voxel_size" type="double" value="0.01"/>
</node>
</launch>