From c6868282a95e3b50f171c6217b3b110876fe9a5c Mon Sep 17 00:00:00 2001
From: duongtd <tdd149cm@gmail.com>
Date: Thu, 23 Jul 2026 16:12:23 +0700
Subject: [PATCH] otimal

---
 config/config/obstacle_layer_params.yaml | 18 ++++++++++++++++
 config/config/voxel_layer_params.yaml    | 26 ++++--------------------
 config/costmap_common_params.yaml        |  6 ++++++
 launch/depth_local_costmap_test.launch   | 16 ++++++++++-----
 4 files changed, 39 insertions(+), 27 deletions(-)

diff --git a/config/config/obstacle_layer_params.yaml b/config/config/obstacle_layer_params.yaml
index 85c0c72..c00cc0f 100644
--- a/config/config/obstacle_layer_params.yaml
+++ b/config/config/obstacle_layer_params.yaml
@@ -33,6 +33,24 @@ obstacle_layer:
     frustum_clearing_pixel_step: 4
     frustum_min_range: 0.20
     frustum_max_range: 3.5
+    # 3D clearing rays stop this far [m] before the measured surface.
+    # Negative: legacy 2 * resolution (0.10 m at res 0.05).
+    frustum_skip_distance: 0.05
+    # Full-column clearing: nearest in-band return per pixel column certifies
+    # the beam free and wipes whole voxel columns along it. Measurement-driven,
+    # never touches cells outside the FOV.
+    frustum_column_clearing: true
+    # Height band [m]; floor returns below min do not shorten the beam.
+    # max < 0: use the layer max_obstacle_height.
+    column_clear_min_height: 0.10
+    column_clear_max_height: -1.0
+    # Column beams stop this far [m] before the nearest in-band return
+    # (tighter than frustum_skip_distance so ghosts hugging static obstacles
+    # still get cleared).
+    column_skip_distance: 0.02
+    # Full columns only cleared beyond this distance [m] (vertical FOV must
+    # cover the whole band). Negative: auto from camera intrinsics + mount.
+    column_cover_distance: -1.0
     observation_persistence: 0.0
     expected_update_rate: 0.5
     obstacle_range: 2.5
diff --git a/config/config/voxel_layer_params.yaml b/config/config/voxel_layer_params.yaml
index 4ec7884..206182a 100644
--- a/config/config/voxel_layer_params.yaml
+++ b/config/config/voxel_layer_params.yaml
@@ -13,25 +13,7 @@ voxel_layer:
   # the current FOV, occlusion shadows). 0 disables (default). Only enable on
   # camera-only costmaps: decay clears obstacles the sensor cannot see.
   obstacle_decay_time: 4.0
-  # Clearing rays stop this far [m] before the measured surface. Negative
-  # keeps the legacy 2 * resolution behaviour (0.10 m at res 0.05).
-  frustum_skip_distance: 0.05
-  # Full-column clearing: per depth-image pixel column, the nearest return in
-  # the height band certifies the beam free and wipes whole voxel columns
-  # along it. Removes marked voxels the per-pixel 3D rays cannot reach
-  # (above the vertical FOV at close range). Measurement-driven: keeps
-  # obstacles outside the FOV untouched (no decay).
-  frustum_column_clearing: true
-  # Height band [m]; floor returns below min do not shorten the beam.
-  # max < 0: use max_obstacle_height.
-  column_clear_min_height: 0.10
-  column_clear_max_height: -1.0
-  # Full columns only cleared beyond this distance [m] (vertical FOV must
-  # cover the whole band). Negative: auto from camera intrinsics + mount pose.
-  column_cover_distance: -1.0
-  publish_voxel_map: true
-  frustum_clearing_enabled: false
-  frustum_clearing_pixel_step: 4
-  frustum_min_range: 0.20
-  frustum_max_range: 3.5
-  frustum_depth_camera_topic: /camera/depth/data
+  # Depth frustum / column clearing is configured PER OBSERVATION SOURCE in
+  # obstacle_layer_params.yaml (pc_clearing: frustum_skip_distance,
+  # frustum_column_clearing, column_clear_min/max_height,
+  # column_skip_distance, column_cover_distance).
\ No newline at end of file
diff --git a/config/costmap_common_params.yaml b/config/costmap_common_params.yaml
index d9abf8a..1814f37 100644
--- a/config/costmap_common_params.yaml
+++ b/config/costmap_common_params.yaml
@@ -33,6 +33,12 @@ obstacles:
     frustum_clearing_pixel_step: 4
     frustum_min_range: 0.20
     frustum_max_range: 3.5
+    frustum_skip_distance: 0.05
+    frustum_column_clearing: true
+    column_clear_min_height: 0.10
+    column_clear_max_height: -1.0
+    column_skip_distance: 0.02
+    column_cover_distance: -1.0
     observation_persistence: 0.0
     expected_update_rate: 0.5
     obstacle_range: 2.5
diff --git a/launch/depth_local_costmap_test.launch b/launch/depth_local_costmap_test.launch
index 857b119..5e9c605 100644
--- a/launch/depth_local_costmap_test.launch
+++ b/launch/depth_local_costmap_test.launch
@@ -28,6 +28,10 @@
   <arg name="frustum_column_clearing" default="true"/>
   <arg name="column_clear_min_height" default="0.10"/>
   <arg name="column_clear_max_height" default="-1.0"/>
+  <!-- Column beams stop this far [m] before the nearest in-band return
+       (tighter than frustum_skip_distance so ghosts hugging static obstacles
+       still get cleared). -->
+  <arg name="column_skip_distance" default="0.02"/>
   <arg name="column_cover_distance" default="-1.0"/>
 
   <node pkg="depth_local_costmap_noetic_test"
@@ -81,11 +85,13 @@
     <param name="local_costmap/obstacles/frustum_depth_camera_topic" value="$(arg depth_camera_data_topic)"/>
 
     <param name="local_costmap/obstacles/obstacle_decay_time" value="$(arg obstacle_decay_time)"/>
-    <param name="local_costmap/obstacles/frustum_skip_distance" value="$(arg frustum_skip_distance)"/>
-    <param name="local_costmap/obstacles/frustum_column_clearing" value="$(arg frustum_column_clearing)"/>
-    <param name="local_costmap/obstacles/column_clear_min_height" value="$(arg column_clear_min_height)"/>
-    <param name="local_costmap/obstacles/column_clear_max_height" value="$(arg column_clear_max_height)"/>
-    <param name="local_costmap/obstacles/column_cover_distance" value="$(arg column_cover_distance)"/>
+    <!-- depth frustum / column clearing is per observation source -->
+    <param name="local_costmap/obstacles/pc_clearing/frustum_skip_distance" value="$(arg frustum_skip_distance)"/>
+    <param name="local_costmap/obstacles/pc_clearing/frustum_column_clearing" value="$(arg frustum_column_clearing)"/>
+    <param name="local_costmap/obstacles/pc_clearing/column_clear_min_height" value="$(arg column_clear_min_height)"/>
+    <param name="local_costmap/obstacles/pc_clearing/column_clear_max_height" value="$(arg column_clear_max_height)"/>
+    <param name="local_costmap/obstacles/pc_clearing/column_skip_distance" value="$(arg column_skip_distance)"/>
+    <param name="local_costmap/obstacles/pc_clearing/column_cover_distance" value="$(arg column_cover_distance)"/>
   </node>
 
   <node if="$(arg rviz)"