works reliably

Author: Idate96

elevation_mapping_cupy/config/core/core_param.yaml

@@ -1,11 +1,11 @@
 elevation_mapping_node:
   ros__parameters:
     #### Basic parameters ########
-    resolution: 0.04                                # resolution in m.
-    map_length: 8.0                                 # map's size in m.
+    resolution: 0.1                                # resolution in m.
+    map_length: 15.0                               # map's size in m.
     sensor_noise_factor: 0.05                       # point's noise is sensor_noise_factor*z^2 (z is distance from sensor).
     mahalanobis_thresh: 2.0                         # points outside this distance is outlier.
-    outlier_variance: 0.01                          # if point is outlier, add this value to the cell.
+    outlier_variance: 0.0`1                          # if point is outlier, add this value to the cell.
     drift_compensation_variance_inler: 0.05         # cells under this value is used for drift compensation.
     max_drift: 0.1                                  # drift compensation happens only the drift is smaller than this value (for safety)
     drift_compensation_alpha: 0.1                   # drift compensation alpha for smoother update of drift compensation
@@ -20,8 +20,8 @@ elevation_mapping_node:
     orientation_noise_thresh: 0.01                  # if the orientation change is bigger than this value, the drift compensation happens.
     position_lowpass_alpha: 0.2                     # lowpass filter alpha used for detecting movements.
     orientation_lowpass_alpha: 0.2                  # lowpass filter alpha used for detecting movements.
-    min_valid_distance: 0.5                         # points with shorter distance will be filtered out.
-    max_height_range: 1.0                           # points higher than this value from sensor will be filtered out to disable ceiling.
+    min_valid_distance: 4.0                         # points with shorter distance will be filtered out.
+    max_height_range: 10.5                           # points higher than this value from sensor will be filtered out
     ramped_height_range_a: 0.3                      # if z > max(d - ramped_height_range_b, 0) * ramped_height_range_a + ramped_height_range_c, reject.
     ramped_height_range_b: 1.0                      # if z > max(d - ramped_height_range_b, 0) * ramped_height_range_a + ramped_height_range_c, reject.
     ramped_height_range_c: 0.2                      # if z > max(d - ramped_height_range_b, 0) * ramped_height_range_a + ramped_height_range_c, reject.
@@ -44,16 +44,20 @@ elevation_mapping_node:
     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)
     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)
 
-    map_frame: 'odom'                               # The map frame where the odometry source uses.
-    base_frame: 'base_footprint'                    # The robot's base frame. This frame will be a center of the map.
-    corrected_map_frame: 'odom'
+    map_frame: 'World'                               # The map frame where the odometry source uses.
+    base_frame: 'BASE'                    # The robot's base frame. This frame will be a center of the map.
+    corrected_map_frame: 'World'
 
 
+    # map_frame: 'World'                               # The map frame where the odometry source uses.
+    # base_frame: 'BASE'                    # The robot's base frame. This frame will be a center of the map.
+    # corrected_map_frame: 'World'
+
     #### Feature toggles ########
     enable_edge_sharpen: true
     enable_visibility_cleanup: true
     enable_drift_compensation: true
-    enable_overlap_clearance: true
+    enable_overlap_clearance: true`
     enable_pointcloud_publishing: false
     enable_drift_corrected_TF_publishing: false
     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.

elevation_mapping_cupy/config/setups/anymal/anymal_parameters.yaml

@@ -1,7 +1,7 @@
 /elevation_mapping_node:
   ros__parameters:
     #### Basic parameters ########
-    resolution: 0.04                                # resolution in m.
+    resolution: 0.04                               # resolution in m.
     map_length: 8                                   # map's size in m.
     sensor_noise_factor: 0.05                       # point's noise is sensor_noise_factor*z^2 (z is distance from sensor).
     mahalanobis_thresh: 2.0                         # points outside this distance is outlier.

elevation_mapping_cupy/config/setups/menzi/base.yaml

@@ -0,0 +1,25 @@
+elevation_mapping_node:
+  ros__parameters:
+    pointcloud_channel_fusions:
+      rgb: 'color'
+      default: 'average'
+
+    image_channel_fusions:
+      rgb: 'color'
+      default: 'exponential'
+      feat_.*: 'exponential'
+
+    subscribers:
+      front_cam:
+        topic_name: '/ouster_points_self_filtered'
+        data_type: pointcloud
+
+    publishers:
+      elevation_map_raw:
+        layers: ['elevation', 'traversability', 'variance','rgb']
+        basic_layers: ['elevation']
+        fps: 5.0
+      elevation_map_filter:
+        layers: ['min_filter', 'smooth', 'inpaint', 'elevation']
+        basic_layers: ['min_filter']
+        fps: 3.0

elevation_mapping_cupy/elevation_mapping_cupy/elevation_mapping.py

@@ -242,6 +242,26 @@ def compile_kernels(self):
         self.min_filtered = cp.zeros((self.cell_n, self.cell_n), dtype=self.data_type)
         self.min_filtered_mask = cp.zeros((self.cell_n, self.cell_n), dtype=self.data_type)
         self.mask = cp.zeros((self.cell_n, self.cell_n), dtype=self.data_type)
+
+        # Log parameter values before kernel initialization
+        self.logger.info("Initializing add_points_kernel with parameters:")
+        self.logger.info(f"  resolution: {self.resolution}")
+        self.logger.info(f"  cell_n: {self.cell_n}")
+        self.logger.info(f"  sensor_noise_factor: {self.param.sensor_noise_factor}")
+        self.logger.info(f"  mahalanobis_thresh: {self.param.mahalanobis_thresh}")
+        self.logger.info(f"  outlier_variance: {self.param.outlier_variance}")
+        self.logger.info(f"  wall_num_thresh: {self.param.wall_num_thresh}")
+        self.logger.info(f"  max_ray_length: {self.param.max_ray_length}")
+        self.logger.info(f"  cleanup_step: {self.param.cleanup_step}")
+        self.logger.info(f"  min_valid_distance: {self.param.min_valid_distance}")
+        self.logger.info(f"  max_height_range: {self.param.max_height_range}")
+        self.logger.info(f"  cleanup_cos_thresh: {self.param.cleanup_cos_thresh}")
+        self.logger.info(f"  ramped_height_range_a: {self.param.ramped_height_range_a}")
+        self.logger.info(f"  ramped_height_range_b: {self.param.ramped_height_range_b}")
+        self.logger.info(f"  ramped_height_range_c: {self.param.ramped_height_range_c}")
+        self.logger.info(f"  enable_edge_sharpen: {self.param.enable_edge_sharpen}")
+        self.logger.info(f"  enable_visibility_cleanup: {self.param.enable_visibility_cleanup}")
+
         self.add_points_kernel = add_points_kernel(
             self.resolution,
             self.cell_n,