-
Notifications
You must be signed in to change notification settings - Fork 3
/
Copy pathslam3d.orogen
264 lines (207 loc) · 10 KB
/
slam3d.orogen
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
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
name "slam3d"
using_library 'slam3d_graph_boost'
using_library 'slam3d_sensor_pcl'
using_library 'slam3d_solver_g2o'
using_library 'transformer'
using_library 'maps'
import_types_from 'base'
import_types_from 'maps'
import_types_from 'pcl'
import_types_from 'slam3d/sensor/pcl/RegistrationParameters.hpp'
import_types_from 'GridConfiguration.hpp'
## Mapper using Pointclouds
task_context "PointcloudMapper" do
needs_configuration
input_port 'scan', '/base/samples/Pointcloud'
output_port 'cloud', '/base/samples/Pointcloud'
output_port 'mls', '/maps/grid/MLSMapSloped'
output_port 'robot2map', 'base/samples/RigidBodyState'
output_port 'odometry2map', 'base/samples/RigidBodyState'
periodic 0.01
transformer do
transform "laser", "robot"
transform "robot", "odometry"
max_latency 0.1
align_port "scan"
transform_output "robot2map", "robot" => "map"
transform_output "odometry2map", "odometry" => "map"
end
operation('generate_cloud')
.returns('/bool')
.doc("Generates and sends the accumulated pointcloud.")
operation('generate_map')
.returns('/bool')
.doc("Generates the envire map.")
operation('optimize')
.returns('/bool')
.doc("Starts the global optimization using the current SLAM backend.")
operation('force_add')
.returns('/bool')
.doc("Force to add one scan to the graph regardless of change in pose.")
operation('write_graph')
.returns('/bool')
.doc("Writes internal graph structure to file.")
operation('write_envire')
.returns('/bool')
.doc("Writes the serialized envire object to 'envire_path'.")
operation('write_ply')
.argument('folder_path', '/std/string')
.returns('/bool')
.doc("Writes the accumulated pointcloud as ply to the given folder.")
property('scan_resolution', '/double', 0.01)
.doc("Pointclouds are downsampled to this resolution before being added to the map.")
property('map_resolution', '/double', 0.01)
.dynamic()
.doc("Map-Pointcloud is downsampled to this resolution before being published.")
property('optimization_rate', '/int', 0)
.dynamic()
.doc("Optimize the graph every X scans, that are added to the map. Optimization
is additionally run automatically after every successful loop-closure.")
property('map_publish_rate', '/int', 0)
.dynamic()
.doc("Publish MLS-Map every X scans, that are added to the map.")
property('map_update_rate', '/int', 0)
.dynamic()
.doc("Add every X'th scan to the grid-map, even if it is not added to the graph.")
property('sequential_icp', '/bool', true)
.doc("Whether sequential scan matching is performed. Should normally be set to true.")
property('neighbor_radius', '/double', 1.0)
.doc("A new node is linked against neighbors within this radius, but not
more than max_neighbor_links.")
property('max_neighbor_links', '/int', 5)
.doc("Maximum number of additional constraints per node added to the graph.
This can be used to limit the computational costs of adding a node.")
property('min_loop_length', '/int', 10)
.doc("Minimum number of nodes that must be part of a loop for it to be a
valid loop closure. Use higher values to avoid that to many links are
created between nearby scan nodes.")
property('patch_building_range', '/int', 0)
.doc("If set to a value X > 0, pointclouds are matched against accumulated
pointclouds generated from all nodes in a local neighborhood of the target
scan. Local patches will include all nodes that are reachable by a maximum
of X edges from the target.")
property('map_outlier_radius', '/double', 0.2)
.dynamic()
.doc("Outlier rejection is performed on the accumulated pointcloud. A point
is considered an inlier if it has enough neighbors within this radius. The
parameter map_outlier_neighbors defines what is enough. This has no effect
on the grid map that is written to the envire_map port.")
property('map_outlier_neighbors', '/int', 2)
.dynamic()
.doc("Outlier rejection is performed on the accumulated pointcloud. A point
is considered an inlier if it has this many neighbors within a given radius.
The parameter map_outlier_radius defines this radius. This has no effect
on the grid map that is written to the envire_map port.")
property('min_translation', '/double', 0.1)
.doc("Minimum distance between two subsequent nodes. If odometry is
available, it is used to determine the traveled distance between scans,
if not ICP has to be performed. A too small value will result in high
cumputational costs and a rapidly growing graph. But distance should be
small enough to have enough overlap between scans for ICP.")
property('min_rotation', '/double', 0.25)
.doc("Minimum rotation difference between two subsequent nodes. This is
especially important for scanners with a horizonal opening angle. for
sensors with 360° coverage this can be set much higher if odometry is
reasonably accurate.")
property('use_odometry', '/bool', true)
.doc("Whether to use the robot's odometry. If set to true, the localization
result should be read from the odometry2map port. By integrating the
robot2odometry transformation from the odometry, the current pose in map
coordinates can be retrieved. If set to false, every incoming scan has to be
matched with ICP to calculate the robot's movement. The result is then read
from the robot2map port directly.")
property('add_odometry_edges', '/bool', true)
.doc("Whether to add odometry edges to the graph, should be true in most
cases. Set this to false if odometry should only be used to decide if a scan
should be added and to initialize ICP, but not be used for the graph. This
is mostly useful for debugging and result evaluation.")
property('start_pose', '/base/Pose')
.doc("Start pose of the robot in map coordinates. This can remain at the
origin for single robot mapping. In a distributed setup, the start poses of
all robots should reflect their relative poses at the start of the exploration.")
property('use_odometry_heading', '/bool', false)
.doc("Whether to initialize the robot's heading from odometry. This is useful
when the odometry includes global heading from a compass and will then
preserve this heading in the generated map, e.g. the map will be north aligned.")
property('use_odometry_pose', '/bool', false)
.doc("Similar to use_odometry_heading, but initializes full pose from odometry.")
property('gicp_config', '/slam3d/RegistrationParameters')
.doc("Parameters for Generalized ICP. This set is used for sequential scan
matching and should be quite robust, e.g. fail rarely. A quite good guess
for the transformation from odometry can be assumed. Scans might have only
little overlap depending on the environment.")
property('gicp_coarse_config', '/slam3d/RegistrationParameters')
.doc("Parameters for coarse Generalized ICP. This is performed first to verify
a possible loop closure. It should operate on lower density clouds to increase
the convergence radius and speedup computation. Maximum feature distance should
be lower to avoid invalid matches. The maximum fitness score should be lower
to reduce false positives. The resulting transformation is then used to
initialize the normal ICP used fpr sequential matches.")
property('log_type', '/int', 0)
.doc("Selects the type of logging that is used by slam3d. Standard(0) simply
writes to standard output, Base(1) uses ROCK's base-logging mechanism and
File(2) creates a file slam3d.log.")
property('log_level', '/int', 2)
.dynamic()
.doc("Set which type of messages are logged. Messages with the specified log-level
and above will be generated. Note that when Base logger is used, a DEBUG build is
required to see debug and info messages.
Levels are: Debug(0), Info(1), Warn(2), Error(3), Fatal(4)")
property('robot_name', '/std/string', "robot")
.doc("This robot's name. It will be attached to measurements that are send to other robots.")
property('map_frame', '/std/string', "map")
.doc("The frame that the map is represented in.")
property('grid_config', 'slam3d/GridConfiguration')
.doc("Size and resolution of generated grid map.")
property('grid_mls_config', '/maps/grid/MLSConfig')
.doc("Multi-Layer-Surface-Map specific configuration.")
property('envire_path', '/std/string', "slam3d_envire")
.doc("The path where to write the serialized envire map.")
property('apriori_ply_map', '/std/string')
.doc("Load a pointcloud in ply format to initialize the map. It will be
added as the first element to the graph. Note that no special treatment
of this node is implemented yet. Most importantly the usually greater
size of this cloud will not be taken into account when matching against
incoming regular scans.")
property('gravity_reference', 'base/Vector3d')
.doc("Direction of gravity (upward) in robot coordinates.")
property('initial_patch_radius', '/double', 0.0)
.doc("Fill ground plane in the first scan within this radius. This allows
navigation right after adding the first scan to the map.")
end
## Task to dump velodyne-Scans to KITTI-Format
task_context "PointcloudToBinary" do
input_port 'pcl', '/base/samples/Pointcloud'
input_port 'odometry', '/base/samples/RigidBodyState'
periodic 0.1
end
## Filter for pointclouds
task_context "PointcloudFilter" do
input_port 'input', '/base/samples/Pointcloud'
output_port 'output', '/base/samples/Pointcloud'
port_driven 'input'
property('min_height', '/double', -1)
.doc("Remove all points below this height.")
property('max_height', '/double', 1)
.doc("Remove all points above this height.")
property('min_distance', '/double', 0.5)
.doc("Remove all points within this radius of the sensor.")
property('max_distance', '/double', 10)
.doc("Remove all points beyond this radius of the sensor.")
property('pass_rate', '/int', 1)
.doc("Only pass every n'th scan.")
end
## Convert Depthmap to Pointcloud format
task_context "ScanConverter" do
error_states :MULTIPLE_INPUT_CONNECTIONS
input_port 'depth_map', '/base/samples/DepthMap'
input_port 'distance_image', '/base/samples/DistanceImage'
output_port 'cloud', '/base/samples/Pointcloud'
port_driven
end
## Convert LaserScan to Pointcloud
task_context "LineScanConverter" do
input_port 'scan', '/base/samples/LaserScan'
output_port 'cloud', '/base/samples/Pointcloud'
port_driven 'scan'
end