-
Notifications
You must be signed in to change notification settings - Fork 1
/
3d-navigation-rtabmap.launch
118 lines (97 loc) · 6.85 KB
/
3d-navigation-rtabmap.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
<launch>
<!--To bringup the turtlebot/-->
<include file = "$(find turtlebot_bringup)/launch/minimal.launch"/>
<!--To start the joystick/-->
<!--include file = "$(find turtlebot_teleop)/launch/logitech.launch" /-->
<!--Let's set the default argument values for this launch file-->
<arg name="database_path" default="rtabmap.db"/>
<arg name="rgbd_odometry" default="false"/>
<arg name="rtabmapviz" default="false"/>
<arg name="localization" default="true"/>
<arg name="simulation" default="false"/>
<arg name="sw_registered" default="false"/>
<arg if="$(arg localization)" name="args" default=""/>
<arg unless="$(arg localization)" name="args" default="--Rtabmap/StartNewMapOnLoopClosure true"/>
<arg if="$(arg simulation)" name="rgb_topic" default="/camera/rgb/image_raw"/>
<arg unless="$(arg simulation)" name="rgb_topic" default="/camera/rgb/image_rect_color"/>
<arg if="$(arg simulation)" name="depth_topic" default="/camera/depth/image_raw"/>
<arg unless="$(arg simulation)" name="depth_topic" default="/camera/depth_registered/image_raw"/>
<arg name="camera_info_topic" default="/camera/rgb/camera_info"/>
<arg name="wait_for_transform" default="0.2"/>
<!--
robot_state_publisher's publishing frequency in "turtlebot_bringup/launch/includes/robot.launch.xml"
can be increase from 5 to 10 Hz to avoid some TF warnings.
-->
<!-- Navigation stuff (move_base) -->
<include unless="$(arg simulation)" file="$(find turtlebot_bringup)/launch/3dsensor.launch">
<arg if="$(arg sw_registered)" name="depth_registration" value="false"/>
<arg unless="$(arg sw_registered)" name="depth_registration" value="true"/>
</include>
<include file="$(find turtlebot_navigation)/launch/includes/move_base.launch.xml"/>
<!-- Mapping -->
<group ns="rtabmap">
<node name="rtabmap" pkg="rtabmap_ros" type="rtabmap" output="screen" args="$(arg args)">
<param name="Grid/FromDepth" type="string" value="true"/>
<param name="database_path" type="string" value="$(arg database_path)"/>
<param name="frame_id" type="string" value="base_footprint"/>
<param name="wait_for_transform_duration" type="double" value="$(arg wait_for_transform)"/>
<param name="subscribe_depth" type="bool" value="true"/>
<param name="subscribe_scan" type="bool" value="true"/>
<param name="map_negative_poses_ignored" type="bool" value="true"/>
<!-- When sending goals on /rtabmap/goal topic, use actionlib to communicate with move_base -->
<param name="use_action_for_goal" type="bool" value="true"/>
<remap from="move_base" to="/move_base"/>
<!-- inputs -->
<remap from="scan" to="/scan"/>
<remap from="rgb/image" to="$(arg rgb_topic)"/>
<remap from="depth/image" to="$(arg depth_topic)"/>
<remap from="rgb/camera_info" to="$(arg camera_info_topic)"/>
<!-- Fix odom covariance as in simulation the covariance in /odom topic is high (0.1 for linear and 0.05 for angular) -->
<param unless="$(arg rgbd_odometry)" name="odom_frame_id" value="odom"/>
<param unless="$(arg rgbd_odometry)" name="odom_tf_linear_variance" value="0.001"/>
<param unless="$(arg rgbd_odometry)" name="odom_tf_angular_variance" value="0.001"/>
<!-- output -->
<remap from="grid_map" to="/map"/>
<!-- RTAB-Map's parameters: do "rosrun rtabmap rtabmap (double-dash)params" to see the list of available parameters. -->
<param name="RGBD/ProximityBySpace" type="string" value="true"/> <!-- Local loop closure detection (using estimated position) with locations in WM -->
<param name="RGBD/OptimizeFromGraphEnd" type="string" value="false"/> <!-- Set to false to generate map correction between /map and /odom -->
<param name="Kp/MaxDepth" type="string" value="4.0"/>
<param name="Reg/Strategy" type="string" value="0"/> <!-- Loop closure transformation: 0=Visual, 1=ICP, 2=Visual+ICP -->
<param name="Icp/CorrespondenceRatio" type="string" value="0.3"/>
<param name="Vis/MinInliers" type="string" value="15"/> <!-- 3D visual words minimum inliers to accept loop closure -->
<param name="Vis/InlierDistance" type="string" value="0.1"/> <!-- 3D visual words correspondence distance -->
<param name="RGBD/AngularUpdate" type="string" value="0.1"/> <!-- Update map only if the robot is moving -->
<param name="RGBD/LinearUpdate" type="string" value="0.1"/> <!-- Update map only if the robot is moving -->
<param name="RGBD/ProximityPathMaxNeighbors" type="string" value="0"/>
<param name="Rtabmap/TimeThr" type="string" value="0"/>
<param name="Mem/RehearsalSimilarity" type="string" value="0.30"/>
<param name="Reg/Force3DoF" type="string" value="true"/>
<param name="GridGlobal/MinSize" type="string" value="20"/>
<!-- localization mode -->
<param if="$(arg localization)" name="Mem/IncrementalMemory" type="string" value="true"/>
<param unless="$(arg localization)" name="Mem/IncrementalMemory" type="string" value="true"/>
<param name="Mem/InitWMWithAllNodes" type="string" value="$(arg localization)"/>
</node>
<!-- Odometry : ONLY for testing without the actual robot! /odom TF should not be already published. -->
<node if="$(arg rgbd_odometry)" pkg="rtabmap_ros" type="rgbd_odometry" name="rgbd_odometry" output="screen">
<param name="frame_id" type="string" value="base_footprint"/>
<param name="wait_for_transform_duration" type="double" value="$(arg wait_for_transform)"/>
<param name="Reg/Force3DoF" type="string" value="true"/>
<param name="Vis/InlierDistance" type="string" value="0.05"/>
<remap from="rgb/image" to="$(arg rgb_topic)"/>
<remap from="depth/image" to="$(arg depth_topic)"/>
<remap from="rgb/camera_info" to="$(arg camera_info_topic)"/>
</node>
<!-- visualization with rtabmapviz -->
<node if="$(arg rtabmapviz)" pkg="rtabmap_ros" type="rtabmapviz" name="rtabmapviz" args="-d $(find rtabmap_ros)/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="frame_id" type="string" value="base_footprint"/>
<param name="wait_for_transform_duration" type="double" value="$(arg wait_for_transform)"/>
<remap from="rgb/image" to="$(arg rgb_topic)"/>
<remap from="depth/image" to="$(arg depth_topic)"/>
<remap from="rgb/camera_info" to="$(arg camera_info_topic)"/>
<remap from="scan" to="/scan"/>
</node>
</group>
</launch>