Skip to content

Commit a728e54

Browse files
Takui9Takui9
committed
update to ros2; fixed several small bugs
1 parent f39fb62 commit a728e54

5 files changed

Lines changed: 177 additions & 116 deletions

File tree

‎elevation_mapping_cupy/config/setups/g1/g1_parameters.yaml‎

Lines changed: 79 additions & 77 deletions
Original file line numberDiff line numberDiff line change
@@ -1,89 +1,91 @@
1-
#### Basic parameters ########
2-
resolution: 0.04 # resolution in m.
3-
map_length: 8 # map's size in m.
4-
sensor_noise_factor: 0.05 # point's noise is sensor_noise_factor*z^2 (z is distance from sensor).
5-
mahalanobis_thresh: 2.0 # points outside this distance is outlier.
6-
outlier_variance: 0.01 # if point is outlier, add this value to the cell.
7-
drift_compensation_variance_inler: 0.05 # cells under this value is used for drift compensation.
8-
max_drift: 0.1 # drift compensation happens only the drift is smaller than this value (for safety)
9-
drift_compensation_alpha: 0.1 # drift compensation alpha for smoother update of drift compensation
10-
time_variance: 0.0001 # add this value when update_variance is called.
11-
max_variance: 100.0 # maximum variance for each cell.
12-
initial_variance: 1000.0 # initial variance for each cell.
13-
traversability_inlier: 0.9 # cells with higher traversability are used for drift compensation.
14-
dilation_size: 3 # dilation filter size before traversability filter.
15-
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.
16-
min_height_drift_cnt: 100 # drift compensation only happens if the valid cells are more than this number.
17-
position_noise_thresh: 0.01 # if the position change is bigger than this value, the drift compensation happens.
18-
orientation_noise_thresh: 0.01 # if the orientation change is bigger than this value, the drift compensation happens.
19-
position_lowpass_alpha: 0.2 # lowpass filter alpha used for detecting movements.
20-
orientation_lowpass_alpha: 0.2 # lowpass filter alpha used for detecting movements.
21-
min_valid_distance: 0.5 # points with shorter distance will be filtered out.
22-
max_height_range: 1.0 # points higher than this value from sensor will be filtered out to disable ceiling.
23-
ramped_height_range_a: 0.3 # if z > max(d - ramped_height_range_b, 0) * ramped_height_range_a + ramped_height_range_c, reject.
24-
ramped_height_range_b: 1.0 # if z > max(d - ramped_height_range_b, 0) * ramped_height_range_a + ramped_height_range_c, reject.
25-
ramped_height_range_c: 0.2 # if z > max(d - ramped_height_range_b, 0) * ramped_height_range_a + ramped_height_range_c, reject.
26-
update_variance_fps: 5.0 # fps for updating variance.
27-
update_pose_fps: 10.0 # fps for updating pose and shift the center of map.
28-
time_interval: 0.1 # Time layer is updated with this interval.
29-
map_acquire_fps: 5.0 # Raw map is fetched from GPU memory in this fps.
30-
publish_statistics_fps: 1.0 # Publish statistics topic in this fps.
1+
elevation_mapping:
2+
ros__parameters:
3+
#### Basic parameters ########
4+
resolution: 0.04 # resolution in m.
5+
map_length: 8 # map's size in m.
6+
sensor_noise_factor: 0.05 # point's noise is sensor_noise_factor*z^2 (z is distance from sensor).
7+
mahalanobis_thresh: 2.0 # points outside this distance is outlier.
8+
outlier_variance: 0.01 # if point is outlier, add this value to the cell.
9+
drift_compensation_variance_inler: 0.05 # cells under this value is used for drift compensation.
10+
max_drift: 0.1 # drift compensation happens only the drift is smaller than this value (for safety)
11+
drift_compensation_alpha: 0.1 # drift compensation alpha for smoother update of drift compensation
12+
time_variance: 0.0001 # add this value when update_variance is called.
13+
max_variance: 100.0 # maximum variance for each cell.
14+
initial_variance: 1000.0 # initial variance for each cell.
15+
traversability_inlier: 0.9 # cells with higher traversability are used for drift compensation.
16+
dilation_size: 3 # dilation filter size before traversability filter.
17+
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.
18+
min_height_drift_cnt: 100 # drift compensation only happens if the valid cells are more than this number.
19+
position_noise_thresh: 0.01 # if the position change is bigger than this value, the drift compensation happens.
20+
orientation_noise_thresh: 0.01 # if the orientation change is bigger than this value, the drift compensation happens.
21+
position_lowpass_alpha: 0.2 # lowpass filter alpha used for detecting movements.
22+
orientation_lowpass_alpha: 0.2 # lowpass filter alpha used for detecting movements.
23+
min_valid_distance: 0.5 # points with shorter distance will be filtered out.
24+
max_height_range: 1.0 # points higher than this value from sensor will be filtered out to disable ceiling.
25+
ramped_height_range_a: 0.3 # if z > max(d - ramped_height_range_b, 0) * ramped_height_range_a + ramped_height_range_c, reject.
26+
ramped_height_range_b: 1.0 # if z > max(d - ramped_height_range_b, 0) * ramped_height_range_a + ramped_height_range_c, reject.
27+
ramped_height_range_c: 0.2 # if z > max(d - ramped_height_range_b, 0) * ramped_height_range_a + ramped_height_range_c, reject.
28+
update_variance_fps: 5.0 # fps for updating variance.
29+
update_pose_fps: 10.0 # fps for updating pose and shift the center of map.
30+
time_interval: 0.1 # Time layer is updated with this interval.
31+
map_acquire_fps: 5.0 # Raw map is fetched from GPU memory in this fps.
32+
publish_statistics_fps: 1.0 # Publish statistics topic in this fps.
3133

32-
max_ray_length: 10.0 # maximum length for ray tracing.
33-
cleanup_step: 0.1 # subtitute this value from validity layer at visibiltiy cleanup.
34-
cleanup_cos_thresh: 0.1 # subtitute this value from validity layer at visibiltiy cleanup.
34+
max_ray_length: 10.0 # maximum length for ray tracing.
35+
cleanup_step: 0.1 # subtitute this value from validity layer at visibiltiy cleanup.
36+
cleanup_cos_thresh: 0.1 # subtitute this value from validity layer at visibiltiy cleanup.
3537

36-
safe_thresh: 0.7 # if traversability is smaller, it is counted as unsafe cell.
37-
safe_min_thresh: 0.4 # polygon is unsafe if there exists lower traversability than this.
38-
max_unsafe_n: 10 # if the number of cells under safe_thresh exceeds this value, polygon is unsafe.
38+
safe_thresh: 0.7 # if traversability is smaller, it is counted as unsafe cell.
39+
safe_min_thresh: 0.4 # polygon is unsafe if there exists lower traversability than this.
40+
max_unsafe_n: 10 # if the number of cells under safe_thresh exceeds this value, polygon is unsafe.
3941

40-
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)
41-
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)
42+
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)
43+
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)
4244

43-
map_frame: 'map' # The map frame where the odometry source uses.
44-
base_frame: 'body' # The robot's base frame. This frame will be a center of the map.
45-
corrected_map_frame: 'odom_corrected'
45+
map_frame: 'map' # The map frame where the odometry source uses.
46+
base_frame: 'body' # The robot's base frame. This frame will be a center of the map.
47+
corrected_map_frame: 'odom_corrected'
4648

47-
#### Feature toggles ########
48-
enable_edge_sharpen: true
49-
enable_visibility_cleanup: true
50-
enable_drift_compensation: true
51-
enable_overlap_clearance: true
52-
enable_pointcloud_publishing: false
53-
enable_drift_corrected_TF_publishing: false
54-
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.
49+
#### Feature toggles ########
50+
enable_edge_sharpen: true
51+
enable_visibility_cleanup: true
52+
enable_drift_compensation: true
53+
enable_overlap_clearance: true
54+
enable_pointcloud_publishing: false
55+
enable_drift_corrected_TF_publishing: false
56+
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.
5557

56-
#### Traversability filter ########
57-
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.
58-
weight_file: '$(rospack find elevation_mapping_cupy)/config/core/weights.dat' # Weight file for traversability filter
58+
#### Traversability filter ########
59+
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.
60+
weight_file: '$(rospack find elevation_mapping_cupy)/config/core/weights.dat' # Weight file for traversability filter
5961

60-
#### Upper bound ########
61-
use_only_above_for_upper_bound: false
62+
#### Upper bound ########
63+
use_only_above_for_upper_bound: false
6264

63-
#### Plugins ########
64-
# plugin_config_file: '$(rospack find elevation_mapping_cupy)/config/anymal_plugin_config.yaml'
65+
#### Plugins ########
66+
# plugin_config_file: '$(rospack find elevation_mapping_cupy)/config/anymal_plugin_config.yaml'
6567

66-
#### Publishers ########
67-
# topic_name:
68-
# layers: # Choose from 'elevation', 'variance', 'traversability', 'time', 'normal_x', 'normal_y', 'normal_z', 'color', plugin_layer_names
69-
# basic_layers: # basic_layers for valid cell computation (e.g. Rviz): Choose a subset of `layers`.
70-
# fps: # Publish rate. Use smaller value than `map_acquire_fps`.
68+
#### Publishers ########
69+
# topic_name:
70+
# layers: # Choose from 'elevation', 'variance', 'traversability', 'time', 'normal_x', 'normal_y', 'normal_z', 'color', plugin_layer_names
71+
# basic_layers: # basic_layers for valid cell computation (e.g. Rviz): Choose a subset of `layers`.
72+
# fps: # Publish rate. Use smaller value than `map_acquire_fps`.
7173

72-
# publishers:
73-
# elevation_map_raw:
74-
# layers: ['elevation', 'traversability', 'variance']
75-
# basic_layers: ['elevation', 'traversability']
76-
# fps: 5.0
74+
# publishers:
75+
# elevation_map_raw:
76+
# layers: ['elevation', 'traversability', 'variance']
77+
# basic_layers: ['elevation', 'traversability']
78+
# fps: 5.0
7779

78-
# semantic_map_raw:
79-
# layers: ['elevation', 'traversability']
80-
# basic_layers: ['elevation', 'traversability']
81-
# fps: 5.0
80+
# semantic_map_raw:
81+
# layers: ['elevation', 'traversability']
82+
# basic_layers: ['elevation', 'traversability']
83+
# fps: 5.0
8284

83-
#### Initializer ########
84-
initialize_method: 'linear' # Choose one from 'nearest', 'linear', 'cubic'
85-
initialize_frame_id: ['base_link'] # One tf (like ['footprint'] ) initializes a square around it.
86-
initialize_tf_offset: [0.0, 0.0, 0.0, 0.0] # z direction. Should be same number as initialize_frame_id.
87-
dilation_size_initialize: 5 # dilation size after the init.
88-
initialize_tf_grid_size: 0.4 # This is not used if number of tf is more than 3.
89-
use_initializer_at_start: true # Use initializer when the node starts.
85+
#### Initializer ########
86+
initialize_method: 'linear' # Choose one from 'nearest', 'linear', 'cubic'
87+
initialize_frame_id: ['base_link'] # One tf (like ['footprint'] ) initializes a square around it.
88+
initialize_tf_offset: [0.0, 0.0, 0.0, 0.0] # z direction. Should be same number as initialize_frame_id.
89+
dilation_size_initialize: 5 # dilation size after the init.
90+
initialize_tf_grid_size: 0.4 # This is not used if number of tf is more than 3.
91+
use_initializer_at_start: true # Use initializer when the node starts.

‎elevation_mapping_cupy/config/setups/g1/g1_sensor_parameter.yaml‎

Lines changed: 37 additions & 35 deletions
Original file line numberDiff line numberDiff line change
@@ -1,42 +1,44 @@
1-
plugin_config_file: '$(rospack find elevation_mapping_cupy)/config/setups/g1/g1_plugin_config.yaml'
1+
elevation_mapping:
2+
ros__parameters:
3+
plugin_config_file: '$(rospack find elevation_mapping_cupy)/config/setups/g1/g1_plugin_config.yaml'
24

3-
pointcloud_channel_fusions:
4-
rgb: 'color'
5-
default: 'average'
5+
pointcloud_channel_fusions:
6+
rgb: 'color'
7+
default: 'average'
68

7-
image_channel_fusions:
8-
rgb: 'color'
9-
default: 'exponential'
10-
feat_.*: 'exponential'
11-
sem_.*: 'exponential'
9+
image_channel_fusions:
10+
rgb: 'color'
11+
default: 'exponential'
12+
feat_.*: 'exponential'
13+
sem_.*: 'exponential'
1214

13-
#### Publishers ########
14-
# topic_name:
15-
# layers: # Choose from 'elevation', 'variance', 'traversability', 'time', 'normal_x', 'normal_y', 'normal_z', 'color', plugin_layer_names
16-
# basic_layers: # basic_layers for valid cell computation (e.g. Rviz): Choose a subset of `layers`.
17-
# fps: # Publish rate. Use smaller value than `map_acquire_fps`.
15+
#### Publishers ########
16+
# topic_name:
17+
# layers: # Choose from 'elevation', 'variance', 'traversability', 'time', 'normal_x', 'normal_y', 'normal_z', 'color', plugin_layer_names
18+
# basic_layers: # basic_layers for valid cell computation (e.g. Rviz): Choose a subset of `layers`.
19+
# fps: # Publish rate. Use smaller value than `map_acquire_fps`.
1820

19-
publishers:
20-
elevation_map_raw:
21-
layers: ['elevation', 'traversability', 'variance', 'rgb', 'upper_bound']
22-
basic_layers: ['elevation', 'traversability']
23-
fps: 5.0
21+
publishers:
22+
elevation_map_raw:
23+
layers: ['elevation', 'traversability', 'variance', 'rgb', 'upper_bound']
24+
basic_layers: ['elevation', 'traversability']
25+
fps: 5.0
2426

25-
elevation_map_recordable:
26-
layers: ['elevation', 'traversability', 'variance', 'rgb']
27-
basic_layers: ['elevation', 'traversability']
28-
fps: 2.0
27+
elevation_map_recordable:
28+
layers: ['elevation', 'traversability', 'variance', 'rgb']
29+
basic_layers: ['elevation', 'traversability']
30+
fps: 2.0
2931

30-
filtered_elevation_map:
31-
layers: ['inpaint', 'smooth', 'min_filter', 'max_filter', 'upper_bound', 'min_smooth_filter']
32-
basic_layers: ['inpaint']
33-
fps: 5.0
32+
filtered_elevation_map:
33+
layers: ['inpaint', 'smooth', 'min_filter', 'max_filter', 'upper_bound', 'min_smooth_filter']
34+
basic_layers: ['inpaint']
35+
fps: 5.0
3436

35-
#### Subscribers ########
36-
subscribers:
37-
Livox:
38-
topic_name: /cloud_registered_body_1
39-
data_type: pointcloud
40-
D435:
41-
topic_name: /camera/depth_registered/points
42-
data_type: pointcloud
37+
#### Subscribers ########
38+
subscribers:
39+
Livox:
40+
topic_name: /cloud_registered_body_1
41+
data_type: pointcloud
42+
D435:
43+
topic_name: /camera/depth_registered/points
44+
data_type: pointcloud

‎elevation_mapping_cupy/launch/g1.launch‎

Lines changed: 9 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,9 @@
1+
<launch>
2+
<!-- Elevation mapping node -->
3+
<node pkg="elevation_mapping_cupy" type="elevation_mapping_node" name="elevation_mapping" output="screen">
4+
<rosparam command="load" file="$(find elevation_mapping_cupy)/config/setups/g1/g1_parameters.yaml"/>
5+
<rosparam command="load" file="$(find elevation_mapping_cupy)/config/setups/g1/g1_sensor_parameter.yaml"/>
6+
</node>
7+
<node pkg="tf2_ros" type="static_transform_publisher" name="mid360_to_d435_broadcaster"
8+
args="0.05734 0.01750 0.01369 0 0.79064 0 body camera_link" />
9+
</launch>

‎elevation_mapping_cupy/launch/g1.launch.py‎

Lines changed: 44 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,44 @@
1+
from launch import LaunchDescription
2+
from launch_ros.actions import Node
3+
from launch.substitutions import PathJoinSubstitution
4+
from ament_index_python.packages import get_package_share_directory
5+
6+
7+
def generate_launch_description():
8+
elevation_mapping_cupy_dir = get_package_share_directory('elevation_mapping_cupy')
9+
10+
return LaunchDescription([
11+
Node(
12+
package='elevation_mapping_cupy',
13+
executable='elevation_mapping_node.py',
14+
name='elevation_mapping',
15+
parameters=[
16+
PathJoinSubstitution([
17+
elevation_mapping_cupy_dir,
18+
'config',
19+
'setups',
20+
'g1',
21+
'g1_parameters.yaml'
22+
]),
23+
PathJoinSubstitution([
24+
elevation_mapping_cupy_dir,
25+
'config',
26+
'setups',
27+
'g1',
28+
'g1_sensor_parameter.yaml'
29+
])
30+
],
31+
output='screen'
32+
),
33+
Node(
34+
package='tf2_ros',
35+
executable='static_transform_publisher',
36+
name='mid360_to_d435_broadcaster',
37+
arguments=[
38+
'0.05734', '0.01750', '0.01369',
39+
'0', '0.79064', '0',
40+
'body', 'camera_link',
41+
],
42+
output='screen'
43+
)
44+
])

‎elevation_mapping_cupy/scripts/elevation_mapping_node.py‎

Lines changed: 8 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -61,10 +61,14 @@ def __init__(self):
6161
plugin_config_file=plugin_config_file
6262
)
6363

64-
self.declare_parameter('masked_replace_service_mask_layer_name', 'mask')
65-
self.declare_parameter('save_map_default_topic', 'elevation_map')
66-
self.declare_parameter('save_map_storage_id', 'mcap')
67-
self.declare_parameter('service_namespace', '/elevation_mapping_cupy')
64+
if not self.has_parameter('masked_replace_service_mask_layer_name'):
65+
self.declare_parameter('masked_replace_service_mask_layer_name', 'mask')
66+
if not self.has_parameter('save_map_default_topic'):
67+
self.declare_parameter('save_map_default_topic', 'elevation_map')
68+
if not self.has_parameter('save_map_storage_id'):
69+
self.declare_parameter('save_map_storage_id', 'mcap')
70+
if not self.has_parameter('service_namespace'):
71+
self.declare_parameter('service_namespace', '/elevation_mapping_cupy')
6872

6973
# Read ROS parameters (including YAML)
7074
self.initialize_ros()

0 commit comments

Comments
 (0)