-
Notifications
You must be signed in to change notification settings - Fork 1
/
Copy pathkinect_simulation.launch
154 lines (124 loc) · 8.44 KB
/
kinect_simulation.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
<launch>
<!--
Bringup Turtlebot:
$ roslaunch turtlebot_bringup minimal.launch
Mapping:
$ roslaunch rtabmap_ros demo_turtlebot_mapping.launch
Visualization:
$ roslaunch rtabmap_ros demo_turtlebot_rviz.launch
This launch file is a one to one replacement of the gmapping_demo.launch in the
"SLAM Map Building with TurtleBot" tutorial:
http://wiki.ros.org/turtlebot_navigation/Tutorials/indigo/Build%20a%20map%20with%20SLAM
For localization-only after a mapping session, add argument "localization:=true" to
demo_turtlebot_mapping.launch line above. Move the robot around until it can relocalize in
the previous map, then the 2D map should re-appear again. You can then follow the same steps
from 3.3.2 of the "Autonomous Navigation of a Known Map with TurtleBot" tutorial:
http://wiki.ros.org/turtlebot_navigation/Tutorials/Autonomously%20navigate%20in%20a%20known%20map
For turtlebot in simulation (Gazebo):
$ roslaunch turtlebot_gazebo turtlebot_world.launch
$ roslaunch rtabmap_ros demo_turtlebot_mapping.launch simulation:=true
$ roslaunch rtabmap_ros demo_turtlebot_rviz.launch
-->
<arg name="database_path" default="rtabmap.db"/>
<arg name="rgbd_odometry" default="false"/>
<arg name="rtabmapviz" default="false"/>
<arg name="localization" default="false"/>
<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="--delete_db_on_start"/>
<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="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"/>
<!-- 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)"/>
<remap unless="$(arg rgbd_odometry)" from="odom" to="/odom"/>
<!-- 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="700"/>
<param name="Mem/RehearsalSimilarity" type="string" value="0.30"/>
<param name="Optimizer/Slam2D" type="string" value="true"/>
<param name="Reg/Force3DoF" type="string" value="true"/>
<param name="GridGlobal/MinSize" type="string" value="20"/>
<param name="RGBD/OptimizeMaxError" type="string" value="0.1"/>
<!-- 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>
<!-- 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>
<!-- Find-Object -->
<node name="find_object_3d" pkg="find_object_2d" type="find_object_2d" output="screen">
<param name="gui" value="true" type="bool"/>
<param name="settings_path" value="$(find rtabmap_ros)/launch/config/find_object.ini" type="str"/>
<param name="subscribe_depth" value="true" type="bool"/>
<param name="objects_path" value="$(find rtabmap_ros)/launch/data/books" type="str"/>
<remap from="rgb/image" to="/camera/data_throttled_image"/>
<remap from="depth/image" to="/camera/data_throttled_image_depth"/>
<remap from="rgb/camera_info" to="/camera/data_throttled_camera_info"/>
<!-- param name="rgb/image_transport" type="string" value="compressed"/ -->
<!-- param name="depth_registered/image_transport" type="string" value="compressedDepth"/ -->
</node>
<!-- Save objects to database example -->
<node if="$(arg save_objects)" name="save_objects_example" pkg="rtabmap_ros" type="save_objects_example" output="screen">
<remap from="mapData" to="/rtabmap/mapData"/>
<param name="frame_id" value="base_footprint"/>
</node>
</group>
</launch>