-
Notifications
You must be signed in to change notification settings - Fork 557
/
demo_husky.launch
154 lines (126 loc) · 7.54 KB
/
demo_husky.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
<?xml version="1.0"?>
<!-- -->
<launch>
<!-- Bringup the Husky with SICK (2D LiDAR), realsense camera (RGB-D camera) and velodyne (3D LiDAR):
$ export HUSKY_URDF_EXTRAS=$(rospack find rtabmap_demos)/launch/config/husky_velodyne_gpu_extra.urdf.xacro
$ export HUSKY_UST10_ENABLED=1
$ export HUSKY_REALSENSE_ENABLED=1
$ export HUSKY_REALSENSE_XYZ="0.2206 0 0.1"
$ roslaunch husky_gazebo husky_playpen.launch
$ roslaunch husky_viz view_robot.launch
For ICP odometry examples, rtabmap should be built with libpointmatcher.
Examples:
1) 6DoF mapping with 3D LiDAR
$ roslaunch rtabmap_demos demo_husky.launch lidar3d:=true slam2d:=false
2) 6DoF mapping with 3D LiDAR and RGB-D camera
$ roslaunch rtabmap_demos demo_husky.launch lidar3d:=true slam2d:=false camera:=true
3) 6DoF mapping with 3D LiDAR and RGB-D camera and ICP odometry (with wheel odometry as guess)
$ roslaunch rtabmap_demos demo_husky.launch lidar3d:=true slam2d:=false camera:=true icp_odometry:=true
4) 3DoF mapping with 3D LiDAR
$ roslaunch rtabmap_demos demo_husky.launch lidar3d:=true slam2d:=true
5) 3DoF mapping with 3D LiDAR and RGB-D camera
$ roslaunch rtabmap_demos demo_husky.launch lidar3d:=true slam2d:=true camera:=true
6) 3DoF mapping with 3D LiDAR and RGB-D camera and ICP odometry (with wheel odometry as guess)
$ roslaunch rtabmap_demos demo_husky.launch lidar3d:=true slam2d:=true camera:=true icp_odometry:=true
7) 3DoF mapping with 2D LiDAR
$ roslaunch rtabmap_demos demo_husky.launch lidar2d:=true slam2d:=true
8) 3DoF mapping with 2D LiDAR and RGB-D camera
$ roslaunch rtabmap_demos demo_husky.launch lidar2d:=true slam2d:=true camera:=true
9) 3DoF mapping with 2D LiDAR and RGB-D camera and ICP odometry (with wheel odometry as guess)
$ roslaunch rtabmap_demos demo_husky.launch lidar2d:=true slam2d:=true camera:=true icp_odometry:=true
10) 6DoF mapping with 3D LiDAR and RGB camera (depth generated by lidar projection) and ICP odometry (with wheel odometry as guess)
$ roslaunch rtabmap_demos demo_husky.launch lidar3d:=true slam2d:=false camera:=true icp_odometry:=true depth_from_lidar:=true rtabmap_viz:=true
11) 3DoF mapping with 3D LiDAR and RGB camera (depth generated by lidar projection) and ICP odometry (with wheel odometry as guess)
$ roslaunch rtabmap_demos demo_husky.launch lidar3d:=true slam2d:=true camera:=true icp_odometry:=true depth_from_lidar:=true rtabmap_viz:=true
Issues:
When setting icp_odometry:=true with navigation, sending a goal to move_base could cause errors like:
"Extrapolation Error: Lookup would require extrapolation into the future. Requested
time 1340.520000000 but the latest data is at time 1340.500000000, when looking up
transform from frame [odom] to frame [map]"
To fix this change "global_frame: odom" to "global_frame: icp_odom" in "husky_navigation/config/costmap_local.yaml"
-->
<arg name="navigation" default="true"/>
<arg name="localization" default="false"/>
<arg name="icp_odometry" default="false"/>
<arg name="rtabmap_viz" default="false"/>
<arg name="camera" default="false"/>
<arg name="lidar2d" default="false"/>
<arg name="lidar3d" default="false"/>
<arg name="lidar3d_ray_tracing" default="true"/>
<arg name="slam2d" default="true"/>
<arg name="depth_from_lidar" default="false"/>
<arg if="$(arg lidar3d)" name="cell_size" default="0.3"/>
<arg unless="$(arg lidar3d)" name="cell_size" default="0.05"/>
<arg if="$(eval not lidar2d and not lidar3d)" name="lidar_args" default=""/>
<arg if="$(arg lidar2d)" name="lidar_args" default="
--Reg/Strategy 1
--RGBD/NeighborLinkRefining true
--Grid/CellSize $(arg cell_size)
--Icp/PointToPlaneRadius 0
--Odom/ScanKeyFrameThr 0.5
--Icp/MaxTranslation 1"/>
<arg if="$(arg lidar3d)" name="lidar_args" default="
--Reg/Strategy 1
--RGBD/NeighborLinkRefining true
--ICP/PM true
--Icp/PMOutlierRatio 0.7
--Icp/VoxelSize $(arg cell_size)
--Icp/MaxCorrespondenceDistance 1
--Icp/PointToPlaneGroundNormalsUp 0.9
--Icp/Iterations 10
--Icp/Epsilon 0.001
--OdomF2M/ScanSubtractRadius $(arg cell_size)
--OdomF2M/ScanMaxSize 15000
--Odom/ScanKeyFrameThr 0.5
--Grid/ClusterRadius 1
--Grid/RangeMax 20
--Grid/RayTracing $(arg lidar3d_ray_tracing)
--Grid/CellSize $(arg cell_size)
--Icp/PointToPlaneRadius 0
--Icp/PointToPlaneNormalK 10
--Icp/MaxTranslation 1"/>
<!--- Run rtabmap -->
<remap from="/rtabmap/grid_map" to="/map"/>
<include file="$(find rtabmap_launch)/launch/rtabmap.launch">
<arg if="$(arg localization)" name="args" value="--Reg/Force3DoF $(arg slam2d) $(arg lidar_args)" />
<arg unless="$(arg localization)" name="args" value="--Reg/Force3DoF $(arg slam2d) $(arg lidar_args) -d" /> <!-- create new map -->
<arg name="localization" value="$(arg localization)" />
<arg name="visual_odometry" value="false" />
<arg name="approx_sync" value="$(eval camera or not icp_odometry)" />
<arg name="imu_topic" value="/imu/data" />
<arg unless="$(arg icp_odometry)" name="odom_topic" value="/odometry/filtered" />
<arg name="frame_id" value="base_link" />
<arg name="rtabmap_viz" value="$(arg rtabmap_viz)" />
<arg name="gps_topic" value="/navsat/fix"/>
<!-- 2D LiDAR -->
<arg name="subscribe_scan" value="$(arg lidar2d)" />
<arg if="$(arg lidar2d)" name="scan_topic" value="/front/scan" />
<arg unless="$(arg lidar2d)" name="scan_topic" value="/scan_not_used" />
<!-- 3D LiDAR -->
<arg name="subscribe_scan_cloud" value="$(arg lidar3d)" />
<arg if="$(arg lidar3d)" name="scan_cloud_topic" value="/velodyne_points" />
<arg unless="$(arg lidar3d)" name="scan_cloud_topic" value="/scan_cloud_not_used" />
<!-- If camera is used -->
<arg name="depth" value="$(eval camera and not depth_from_lidar)" />
<arg name="subscribe_rgb" value="$(eval camera)" />
<arg name="rgbd_sync" value="$(eval camera and not depth_from_lidar)" />
<arg name="rgb_topic" value="/realsense/color/image_raw" />
<arg name="camera_info_topic" value="/realsense/color/camera_info" />
<arg name="depth_topic" value="/realsense/depth/image_rect_raw" />
<arg name="approx_rgbd_sync" value="false" />
<!-- If depth generated from lidar projection (in case we have only a single RGB camera with a 3D lidar) -->
<arg name="gen_depth" value="$(arg depth_from_lidar)" />
<arg name="gen_depth_decimation" value="4" />
<arg name="gen_depth_fill_holes_size" value="3" />
<arg name="gen_depth_fill_iterations" value="1" />
<arg name="gen_depth_fill_holes_error" value="0.3" />
<!-- If icp_odometry is used -->
<arg if="$(arg icp_odometry)" name="icp_odometry" value="true" />
<arg if="$(arg icp_odometry)" name="odom_guess_frame_id" value="odom" />
<arg if="$(arg icp_odometry)" name="vo_frame_id" value="icp_odom" />
<arg unless="$(arg slam2d)" name="wait_imu_to_init" value="true" />
<arg if="$(arg lidar3d)" name="odom_args" value="--Icp/CorrespondenceRatio 0.01"/>
</include>
<!--- Run Move Base -->
<include if="$(arg navigation)" file="$(find husky_navigation)/launch/move_base.launch" />
</launch>