-
Notifications
You must be signed in to change notification settings - Fork 568
/
Copy pathdemo_stereo_outdoor.launch
127 lines (106 loc) · 7 KB
/
demo_stereo_outdoor.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
<?xml version="1.0"?>
<launch>
<!--
Demo of outdoor stereo mapping.
From bag:
$ rosbag record
/stereo_camera/left/image_raw_throttle/compressed
/stereo_camera/right/image_raw_throttle/compressed
/stereo_camera/left/camera_info_throttle
/stereo_camera/right/camera_info_throttle
/tf
$ roslaunch rtabmap_demos demo_stereo_outdoor.launch
$ rosbag play -.-clock stereo_oudoorA.bag
-->
<!-- Choose visualization -->
<arg name="rviz" default="true" />
<arg name="rtabmap_viz" default="false" />
<arg name="local_bundle" default="true" />
<arg name="stereo_sync" default="false" />
<param name="use_sim_time" type="bool" value="True"/>
<!-- Just to uncompress images for stereo_image_rect -->
<node name="republish_left" type="republish" pkg="image_transport" args="compressed in:=/stereo_camera/left/image_raw_throttle raw out:=/stereo_camera/left/image_raw_throttle_relay" />
<node name="republish_right" type="republish" pkg="image_transport" args="compressed in:=/stereo_camera/right/image_raw_throttle raw out:=/stereo_camera/right/image_raw_throttle_relay" />
<!-- Run the ROS package stereo_image_proc for image rectification -->
<group ns="/stereo_camera" >
<node pkg="stereo_image_proc" type="stereo_image_proc" name="stereo_image_proc">
<remap from="left/image_raw" to="left/image_raw_throttle_relay"/>
<remap from="left/camera_info" to="left/camera_info_throttle"/>
<remap from="right/image_raw" to="right/image_raw_throttle_relay"/>
<remap from="right/camera_info" to="right/camera_info_throttle"/>
<param name="disparity_range" value="128"/>
</node>
<node if="$(arg stereo_sync)" pkg="nodelet" type="nodelet" name="stereo_sync" args="standalone rtabmap_sync/stereo_sync">
<remap from="left/image_rect" to="left/image_rect_color"/>
<remap from="right/image_rect" to="right/image_rect"/>
<remap from="left/camera_info" to="left/camera_info_throttle"/>
<remap from="right/camera_info" to="right/camera_info_throttle"/>
</node>
</group>
<group ns="rtabmap">
<!-- Stereo Odometry -->
<node pkg="rtabmap_odom" type="stereo_odometry" name="stereo_odometry" output="screen">
<remap from="left/image_rect" to="/stereo_camera/left/image_rect"/>
<remap from="right/image_rect" to="/stereo_camera/right/image_rect"/>
<remap from="left/camera_info" to="/stereo_camera/left/camera_info_throttle"/>
<remap from="right/camera_info" to="/stereo_camera/right/camera_info_throttle"/>
<remap from="rgbd_image" to="/stereo_camera/rgbd_image"/>
<remap from="odom" to="/stereo_odometry"/>
<param name="subscribe_rgbd" type="bool" value="$(arg stereo_sync)"/>
<param name="frame_id" type="string" value="base_footprint"/>
<param name="odom_frame_id" type="string" value="odom"/>
<param name="Odom/Strategy" type="string" value="0"/> <!-- 0=Frame-to-Map, 1=Frame=to=Frame -->
<param name="Vis/EstimationType" type="string" value="1"/> <!-- 0=3D->3D 1=3D->2D (PnP) -->
<param name="Vis/MaxDepth" type="string" value="0"/>
<param name="Odom/GuessMotion" type="string" value="true"/>
<param name="Vis/MinInliers" type="string" value="10"/>
<param unless="$(arg local_bundle)" name="OdomF2M/BundleAdjustment" type="string" value="0"/>
<param name="OdomF2M/MaxSize" type="string" value="1000"/>
<param name="GFTT/MinDistance" type="string" value="10"/>
<param name="GFTT/QualityLevel" type="string" value="0.00001"/>
<param name="GFTT/QualityLevel" type="string" value="0.00001"/>
</node>
<!-- Visual SLAM: args: "delete_db_on_start" and "udebug" -->
<node name="rtabmap" pkg="rtabmap_slam" type="rtabmap" output="screen" args="--delete_db_on_start">
<param name="frame_id" type="string" value="base_footprint"/>
<param unless="$(arg stereo_sync)" name="subscribe_stereo" type="bool" value="true"/>
<param name="subscribe_depth" type="bool" value="false"/>
<param name="subscribe_rgbd" type="bool" value="$(arg stereo_sync)"/>
<remap from="left/image_rect" to="/stereo_camera/left/image_rect_color"/>
<remap from="right/image_rect" to="/stereo_camera/right/image_rect"/>
<remap from="left/camera_info" to="/stereo_camera/left/camera_info_throttle"/>
<remap from="right/camera_info" to="/stereo_camera/right/camera_info_throttle"/>
<remap from="rgbd_image" to="/stereo_camera/rgbd_image"/>
<remap from="odom" to="/stereo_odometry"/>
<param name="queue_size" type="int" value="30"/>
<param name="map_negative_poses_ignored" type="bool" value="true"/>
<!-- RTAB-Map's parameters -->
<param name="Rtabmap/TimeThr" type="string" value="700"/>
<param name="Grid/DepthDecimation" type="string" value="4"/>
<param name="Grid/FlatObstacleDetected" type="string" value="true"/>
<param name="Kp/MaxDepth" type="string" value="0"/>
<param name="Kp/DetectorStrategy" type="string" value="6"/>
<param name="Vis/EstimationType" type="string" value="1"/> <!-- 0=3D->3D, 1=3D->2D (PnP) -->
<param name="Vis/MaxDepth" type="string" value="0"/>
<param name="RGBD/CreateOccupancyGrid" type="string" value="true"/>
</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 unless="$(arg stereo_sync)" name="subscribe_stereo" type="bool" value="true"/>
<param name="subscribe_odom_info" type="bool" value="true"/>
<param name="subscribe_rgbd" type="bool" value="$(arg stereo_sync)"/>
<param name="queue_size" type="int" value="10"/>
<param name="frame_id" type="string" value="base_footprint"/>
<remap from="left/image_rect" to="/stereo_camera/left/image_rect_color"/>
<remap from="right/image_rect" to="/stereo_camera/right/image_rect"/>
<remap from="left/camera_info" to="/stereo_camera/left/camera_info_throttle"/>
<remap from="right/camera_info" to="/stereo_camera/right/camera_info_throttle"/>
<remap from="rgbd_image" to="/stereo_camera/rgbd_image"/>
<remap from="odom_info" to="odom_info"/>
<remap from="odom" to="/stereo_odometry"/>
<remap from="mapData" to="mapData"/>
</node>
</group>
<!-- Visualisation RVIZ -->
<node if="$(arg rviz)" pkg="rviz" type="rviz" name="rviz" args="-d $(find rtabmap_demos)/launch/config/demo_stereo_outdoor.rviz"/>
</launch>