Skip to content

Commit 80c2990

Browse files
author
amilearning
committed
orin topics
1 parent b0d06c3 commit 80c2990

File tree

4 files changed

+240
-47
lines changed

4 files changed

+240
-47
lines changed
Lines changed: 189 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,189 @@
1+
elevation_mapping:
2+
ros__parameters:
3+
#### Basic parameters ########
4+
enable_normal_arrow_publishing: true
5+
cell_n: 40000 # number of cells in the map.
6+
data_type: 'float32' # data type for the map.
7+
average_weight: 0.5
8+
drift_compensation_variance_inlier: 0.1
9+
checker_layer: 'traversability' # layer name for checking the validity of the cell.
10+
min_filter_size: 5 # size of the filter for min filter.
11+
min_filter_iteration: 3 # number of iterations for min filter.
12+
initialized_variance: 10.0 # initial variance for each cell.
13+
resolution: 0.1 # resolution in m.
14+
map_length: 20.0 # map's size in m.
15+
sensor_noise_factor: 0.05 # point's noise is sensor_noise_factor*z^2 (z is distance from sensor).
16+
mahalanobis_thresh: 2.0 # points outside this distance is outlier.
17+
outlier_variance: 0.01 # if point is outlier, add this value to the cell.
18+
drift_compensation_variance_inler: 0.05 # cells under this value is used for drift compensation.
19+
max_drift: 0.1 # drift compensation happens only the drift is smaller than this value (for safety)
20+
drift_compensation_alpha: 0.1 # drift compensation alpha for smoother update of drift compensation
21+
time_variance: 0.0001 # add this value when update_variance is called.
22+
max_variance: 100.0 # maximum variance for each cell.
23+
initial_variance: 1000.0 # initial variance for each cell.
24+
traversability_inlier: 0.9 # cells with higher traversability are used for drift compensation.
25+
dilation_size: 3 # dilation filter size before traversability filter.
26+
wall_num_thresh: 20 # if there are more points than this value, only higher points than the current height are used to make the wall more sharp.
27+
min_height_drift_cnt: 100 # drift compensation only happens if the valid cells are more than this number.
28+
position_noise_thresh: 0.01 # if the position change is bigger than this value, the drift compensation happens.
29+
orientation_noise_thresh: 0.01 # if the orientation change is bigger than this value, the drift compensation happens.
30+
position_lowpass_alpha: 0.2 # lowpass filter alpha used for detecting movements.
31+
orientation_lowpass_alpha: 0.2 # lowpass filter alpha used for detecting movements.
32+
min_valid_distance: 0.5 # points with shorter distance will be filtered out.
33+
max_height_range: 1.0 # points higher than this value from sensor will be filtered out to disable ceiling.
34+
ramped_height_range_a: 0.3 # if z > max(d - ramped_height_range_b, 0) * ramped_height_range_a + ramped_height_range_c, reject.
35+
ramped_height_range_b: 1.0 # if z > max(d - ramped_height_range_b, 0) * ramped_height_range_a + ramped_height_range_c, reject.
36+
ramped_height_range_c: 0.2 # if z > max(d - ramped_height_range_b, 0) * ramped_height_range_a + ramped_height_range_c, reject.
37+
update_variance_fps: 5.0 # fps for updating variance.
38+
update_pose_fps: 10.0 # fps for updating pose and shift the center of map.
39+
time_interval: 0.1 # Time layer is updated with this interval.
40+
map_acquire_fps: 5.0 # Raw map is fetched from GPU memory in this fps.
41+
publish_statistics_fps: 1.0 # Publish statistics topic in this fps.
42+
43+
max_ray_length: 10.0 # maximum length for ray tracing.
44+
cleanup_step: 0.1 # subtitute this value from validity layer at visibiltiy cleanup.
45+
cleanup_cos_thresh: 0.1 # subtitute this value from validity layer at visibiltiy cleanup.
46+
47+
safe_thresh: 0.7 # if traversability is smaller, it is counted as unsafe cell.
48+
safe_min_thresh: 0.4 # polygon is unsafe if there exists lower traversability than this.
49+
max_unsafe_n: 10 # if the number of cells under safe_thresh exceeds this value, polygon is unsafe.
50+
51+
overlap_clear_range_xy: 4.0 # xy range [m] for clearing overlapped area. this defines the valid area for overlap clearance. (used for multi floor setting)
52+
overlap_clear_range_z: 2.0 # z range [m] for clearing overlapped area. cells outside this range will be cleared. (used for multi floor setting)
53+
pose_topic: '/odom'
54+
map_frame: 'odom' # The map frame where the odometry source uses.
55+
base_frame: 'base_link' # The robot's base frame. This frame will be a center of the map.
56+
corrected_map_frame: 'odom'
57+
58+
59+
60+
#### Feature toggles ########
61+
enable_edge_sharpen: true
62+
enable_visibility_cleanup: true
63+
enable_drift_compensation: true
64+
enable_overlap_clearance: true
65+
enable_pointcloud_publishing: false
66+
enable_drift_corrected_TF_publishing: false
67+
enable_normal_color: true # If true, the map contains 'color' layer corresponding to normal. Add 'color' layer to the publishers setting if you want to visualize.
68+
69+
70+
#### Traversability filter ########
71+
use_chainer: false # Use chainer as a backend of traversability filter or pytorch. If false, it uses pytorch. pytorch requires ~2GB more GPU memory compared to chainer but runs faster.
72+
weight_file: '/share/elevation_mapping_cupy/config/core/weights.dat' # Weight file for traversability filter
73+
plugin_config_file: '/share/elevation_mapping_cupy/config/core/plugin_config.yaml' # Configuration file for the plugin.
74+
#### Upper bound ########
75+
use_only_above_for_upper_bound: false
76+
77+
#### Initializer ########
78+
initialize_method: 'linear' # Choose one from 'nearest', 'linear', 'cubic'
79+
initialize_frame_id: ['base_footprint'] # One tf (like ['footprint'] ) initializes a square around it.
80+
initialize_tf_offset: [0.0, 0.0, 0.0, 0.0] # z direction. Should be same number as initialize_frame_id.
81+
dilation_size_initialize: 2 # dilation size after the init.
82+
initialize_tf_grid_size: 0.5 # This is not used if number of tf is more than 3.
83+
use_initializer_at_start: true # Use initializer when the node starts.
84+
85+
#### Default Plugins ########
86+
87+
pointcloud_channel_fusions:
88+
rgb: 'color' # 'color' fusion is used for the 'rgb' channel
89+
default: 'average' # 'average' fusion is used for channels not listed here
90+
91+
image_channel_fusions:
92+
rgb: 'color' # 'color' fusion is used for the 'rgb' channel
93+
default: 'exponential' # 'exponential' fusion is used for channels not listed here
94+
feat_.*: 'exponential' # 'exponential' fusion is also used for any channel starting with 'feat_' Regular expressions can be used for channel names
95+
96+
#### Subscribers ########
97+
# pointcloud_sensor_name:
98+
# topic_name: '/sensor/pointcloud_semantic'
99+
# data_type: pointcloud # pointcloud or image
100+
#
101+
# image_sensor_name:
102+
# topic_name: '/camera/image_semantic'
103+
# data_type: image # pointcloud or image
104+
# camera_info_topic_name: '/camera/depth/camera_info'
105+
# channel_info_topic_name: '/camera/channel_info'
106+
107+
108+
subscribers:
109+
pointcloud1:
110+
# topic_name: '/ouster/points'
111+
topic_name: '/a200/sensors/camera_0/points'
112+
data_type: 'pointcloud'
113+
image1:
114+
topic_name: '/a200/sensors/camera_0/color/image'
115+
info_name: '/a200/sensors/camera_0/color/camera_info'
116+
data_type: 'image'
117+
# channel_name: '/front_cam/channel_info'
118+
119+
# image_topic2: '/camera/rgb/image_raw2'
120+
# image_info2: '/camera/depth/camera_info2'
121+
# image_channel_info1: '/front_cam/channel_info'
122+
123+
# image: # for semantic images
124+
# topic_name: '/front_cam/semantic_image'
125+
# camera_info_topic_name: '/front_cam/camera/depth/camera_info_resized'
126+
# channel_info_topic_name: '/front_cam/channel_info'
127+
# data_type: image
128+
129+
#### Publishers ########
130+
# topic_name:
131+
# layers: # Choose from 'elevation', 'variance', 'traversability', 'time', 'normal_x', 'normal_y', 'normal_z', 'color', plugin_layer_names
132+
# basic_layers: # basic_layers for valid cell computation (e.g. Rviz): Choose a subset of `layers`.
133+
# fps: # Publish rate. Use smaller value than `map_acquire_fps`.
134+
135+
publishers:
136+
elevation_map_raw:
137+
layers: ['elevation', 'traversability', 'variance','rgb', 'normal_x', 'normal_y', 'normal_z']
138+
basic_layers: ['elevation']
139+
fps: 5.0
140+
elevation_map_recordable:
141+
layers: ['elevation', 'traversability']
142+
basic_layers: ['elevation', 'traversability']
143+
fps: 2.0
144+
elevation_map_filter:
145+
layers: ['min_filter', 'smooth', 'inpaint', 'elevation']
146+
basic_layers: ['min_filter']
147+
fps: 3.0
148+
149+
150+
# plugun info
151+
152+
min_filter:
153+
enable: True # whether to load this plugin
154+
fill_nan: False # Fill nans to invalid cells of elevation layer.
155+
is_height_layer: True # If this is a height layer (such as elevation) or not (such as traversability)
156+
layer_name: "min_filter" # The layer name.
157+
extra_params: # These params are passed to the plugin class on initialization.
158+
dilation_size: 1 # The patch size to apply
159+
iteration_n: 30 # The number of iterations
160+
161+
# Apply smoothing.
162+
smooth_filter:
163+
enable: True
164+
fill_nan: False
165+
is_height_layer: True
166+
layer_name: "smooth"
167+
extra_params:
168+
input_layer_name: "min_filter"
169+
170+
# Apply inpainting using opencv
171+
inpainting:
172+
enable: True
173+
fill_nan: False
174+
is_height_layer: True
175+
layer_name: "inpaint"
176+
extra_params:
177+
method: "telea" # telea or ns
178+
179+
# Apply smoothing for inpainted layer
180+
erosion:
181+
enable: True
182+
fill_nan: False
183+
is_height_layer: False
184+
layer_name: "erosion"
185+
extra_params:
186+
input_layer_name: "traversability"
187+
dilation_size: 3
188+
iteration_n: 20
189+
reverse: True

elevation_mapping_cupy/config/core/core_param.yaml

Lines changed: 19 additions & 17 deletions
Original file line numberDiff line numberDiff line change
@@ -1,6 +1,7 @@
11
elevation_mapping:
22
ros__parameters:
3-
#### Basic parameters ########
3+
#### Basic parameters ########
4+
enable_normal_arrow_publishing: false
45
cell_n: 40000 # number of cells in the map.
56
data_type: 'float32' # data type for the map.
67
average_weight: 0.5
@@ -10,7 +11,7 @@ elevation_mapping:
1011
min_filter_iteration: 3 # number of iterations for min filter.
1112
initialized_variance: 10.0 # initial variance for each cell.
1213
resolution: 0.1 # resolution in m.
13-
map_length: 20.0 # map's size in m.
14+
map_length: 20.0 # map's size in m.
1415
sensor_noise_factor: 0.05 # point's noise is sensor_noise_factor*z^2 (z is distance from sensor).
1516
mahalanobis_thresh: 2.0 # points outside this distance is outlier.
1617
outlier_variance: 0.01 # if point is outlier, add this value to the cell.
@@ -63,8 +64,8 @@ elevation_mapping:
6364
enable_overlap_clearance: true
6465
enable_pointcloud_publishing: false
6566
enable_drift_corrected_TF_publishing: false
66-
enable_normal_color: false # If true, the map contains 'color' layer corresponding to normal. Add 'color' layer to the publishers setting if you want to visualize.
67-
enable_normal: false
67+
enable_normal_color: true # If true, the map contains 'color' layer corresponding to normal. Add 'color' layer to the publishers setting if you want to visualize.
68+
enable_normal: true
6869

6970
#### Traversability filter ########
7071
use_chainer: false # Use chainer as a backend of traversability filter or pytorch. If false, it uses pytorch. pytorch requires ~2GB more GPU memory compared to chainer but runs faster.
@@ -105,12 +106,13 @@ elevation_mapping:
105106

106107
subscribers:
107108
pointcloud1:
109+
# topic_name: '/ouster/points'
108110
topic_name: '/a200/sensors/camera_0/points'
109111
data_type: 'pointcloud'
110-
image1:
111-
topic_name: '/a200/sensors/camera_0/color/image'
112-
info_name: '/a200/sensors/camera_0/color/camera_info'
113-
data_type: 'image'
112+
# image1:
113+
# topic_name: '/a200/sensors/camera_0/color/image'
114+
# info_name: '/a200/sensors/camera_0/color/camera_info'
115+
# data_type: 'image'
114116
# channel_name: '/front_cam/channel_info'
115117

116118
# image_topic2: '/camera/rgb/image_raw2'
@@ -133,17 +135,17 @@ elevation_mapping:
133135

134136
publishers:
135137
elevation_map_raw:
136-
layers: ['elevation', 'traversability', 'variance','rgb']
138+
layers: ['elevation', 'traversability', 'variance','normal_x', 'normal_y', 'normal_z']
137139
basic_layers: ['elevation']
138140
fps: 5.0
139-
elevation_map_recordable:
140-
layers: ['elevation', 'traversability']
141-
basic_layers: ['elevation', 'traversability']
142-
fps: 2.0
143-
elevation_map_filter:
144-
layers: ['min_filter', 'smooth', 'inpaint', 'elevation']
145-
basic_layers: ['min_filter']
146-
fps: 3.0
141+
# elevation_map_recordable:
142+
# layers: ['elevation', 'traversability']
143+
# basic_layers: ['elevation', 'traversability']
144+
# fps: 2.0
145+
# elevation_map_filter:
146+
# layers: ['min_filter', 'smooth', 'inpaint', 'elevation']
147+
# basic_layers: ['min_filter']
148+
# fps: 3.0
147149

148150

149151
# plugun info

elevation_mapping_cupy/include/elevation_mapping_cupy/elevation_mapping_ros.hpp

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -14,7 +14,7 @@
1414

1515
// Pybind
1616
#include <pybind11/embed.h> // everything needed for embedding
17-
17+
#include <rclcpp/qos.hpp>
1818
// ROS
1919
#include <geometry_msgs/msg/polygon_stamped.hpp>
2020
#include <image_transport/image_transport.hpp>

0 commit comments

Comments
 (0)