:optimal 27/7 13:48

config/config/costmap_common_params.yaml

@@ -1,47 +0,0 @@
-robot_base_frame: base_link
-transform_tolerance: 1.0
-performance_metrics_enabled: true
-performance_metrics_period: 5.0
-obstacle_range: 3.0
-publish_voxel_map: true
-footprint_padding: 0.0
-
-obstacles:
-  observation_sources: pc_marking pc_clearing
-
-  pc_marking:
-    topic: /camera/depth/points_proc
-    data_type: PointCloud2
-    clearing: false
-    marking: true
-    inf_is_valid: false
-    frustum_clearing_enabled: false
-    observation_persistence: 0.0
-    expected_update_rate: 0.5
-    obstacle_range: 2.5
-    raytrace_range: 3.0
-    min_obstacle_height: 0.1
-    max_obstacle_height: 1.0
-
-  pc_clearing:
-    topic: /camera/depth/data
-    data_type: DepthCameraData
-    clearing: false
-    marking: true
-    inf_is_valid: false
-    frustum_clearing_enabled: true
-    frustum_clearing_pixel_step: 2
-    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: 0.2
-    observation_persistence: 0.0
-    expected_update_rate: 0.5
-    obstacle_range: 2.5
-    raytrace_range: 3.5
-    min_obstacle_height: 0.1
-    max_obstacle_height: 1.0

config/config/costmap_local_params.yaml

@@ -1,32 +0,0 @@
-local_costmap:
-  # Absolute NUC path is intentional for this local test package. It avoids
-  # boost::dll resolving a relative value such as "libplugins" to ./libplugins.
-  library_path: /home/robotics/DuongTD/dev_ws/devel/lib/libplugins.so
-  global_frame: odom
-  robot_base_frame: base_link
-  update_frequency: 10.0
-  publish_frequency: 5.0
-  rolling_window: true
-  track_unknown_space: false
-  raytrace_range: 3.0
-  resolution: 0.05
-  z_resolution: 0.15
-  z_voxels: 8
-  width: 8.0
-  height: 8.0
-  origin_x: 0.0
-  origin_y: 0.0
-  transform_tolerance: 1.0
-  performance_metrics_enabled: true
-  performance_metrics_period: 5.0
-  footprint_padding: 0.0
-  robot_radius: 0.0
-  footprint:
-    - [0.30, 0.30]
-    - [0.30, -0.30]
-    - [-0.30, -0.30]
-    - [-0.30, 0.30]
-
-  inflation:
-    cost_scaling_factor: 10.0
-    inflation_radius: 0.3

config/config/costmap_local_params_plugins_no_virtual_walls.yaml

@@ -1,12 +0,0 @@
-local_costmap:
-  frame_id: odom
-  plugins:
-    - {name: obstacles, type: "VoxelLayer"}
-    - {name: inflation, type: "InflationLayer"}
-
-  obstacles:
-    enabled: true
-    footprint_clearing_enabled: true
-
-  inflation:
-    enabled: true

config/config/costmap_params.yaml

@@ -1,35 +1,38 @@
 robot_costmap_2d:
-  # Derived from pnkx_nav_core/config/costmap_local_params.yaml.
+  global_frame: map
-  # Machine-specific absolute path: must point at the libplugins.so built in
-  # THIS machine's workspace (contains obstacle decay + ray dithering).
-  # NUC: /home/robotics/DuongTD/dev_ws/devel/lib/libplugins.so
-  library_path: /home/duongtd/T800_ws/devel/lib/libplugins.so
-  global_frame: odom
   robot_base_frame: base_link
-  rolling_window: true
+  rolling_window: false
   track_unknown_space: false
 
   plugins:
-    # Derived from costmap_local_params_plugins_no_virtual_walls.yaml.
+    - name: static_layer
-    - name: obstacles
+      type: StaticLayer
-      type: VoxelLayer
+
-    - name: inflation
+    - name: inflation_layer
       type: InflationLayer
 
-  footprint:
+    - name: obstacle_layer
-    - [0.30, 0.30]
+      type: ObstacleLayer
-    - [0.30, -0.30]
-    - [-0.30, -0.30]
-    - [-0.30, 0.30]
 
-  transform_tolerance: 1.0
+    - name: voxel_layer
-  performance_metrics_enabled: true
+      type: VoxelLayer
+
+  library_path: ./libplugins.so
+  footprint:
+    - [0.3, 0.3]
+    - [0.3, -0.3]
+    - [-0.3, -0.3]
+    - [-0.3, 0.3]
+
+  transform_tolerance: 0.0
+  performance_metrics_enabled: false
   performance_metrics_period: 5.0
-  update_frequency: 10.0
+  update_frequency: 1.0
-  width: 8.0
+  width: 0.0
-  height: 8.0
+  height: 0.0
-  resolution: 0.05
+  resolution: 0.0
   origin_x: 0.0
   origin_y: 0.0
+
   footprint_padding: 0.0
   robot_radius: 0.0

config/config/inflation_layer_params.yaml

@@ -1,5 +1,5 @@
 inflation_layer:
   enabled: true
   inflate_unknown: false
-  cost_scaling_factor: 10.0
+  cost_scaling_factor: 15.0
-  inflation_radius: 0.30
+  inflation_radius: 0.55

config/config/obstacle_layer_params.yaml

@@ -1,59 +1,18 @@
 obstacle_layer:
-  enabled: true
+  track_unknown_space: true
-  track_unknown_space: false
+  transform_tolerance: 0.2
-  transform_tolerance: 1.0
+  topic: "map"
+  sensor_frame: laser_frame
+  observation_persistence: 0.0
+  expected_update_rate: 0.0
+  data_type: PointCloud
+  min_obstacle_height: 0.0
+  max_obstacle_height: 2.0
+  inf_is_valid: false
+  clearing: false
+  marking: true
+  obstacle_range: 2.5
+  raytrace_range: 3.0
   footprint_clearing_enabled: true
   combination_method: 1
 
-  # Subset of costmap_common_params.yaml for the topics available in this
-  # local depth-camera test.
-  observation_sources: pc_marking pc_clearing
-
-  pc_marking:
-    topic: /camera/depth/points_proc
-    data_type: PointCloud2
-    clearing: false
-    marking: true
-    inf_is_valid: false
-    frustum_clearing_enabled: false
-    observation_persistence: 0.0
-    expected_update_rate: 0.5
-    obstacle_range: 2.5
-    raytrace_range: 3.0
-    min_obstacle_height: 0.1
-    max_obstacle_height: 1.0
-
-  pc_clearing:
-    topic: /camera/depth/data
-    data_type: DepthCameraData
-    clearing: false
-    marking: true
-    inf_is_valid: false
-    frustum_clearing_enabled: true
-    frustum_clearing_pixel_step: 2
-    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: 0.2
-    observation_persistence: 0.0
-    expected_update_rate: 0.5
-    obstacle_range: 2.5
-    raytrace_range: 3.5
-    min_obstacle_height: 0.1
-    max_obstacle_height: 1.0

config/config/voxel_layer_params.yaml

@@ -1,19 +1,10 @@
 voxel_layer:
   enabled: true
   footprint_clearing_enabled: true
-  max_obstacle_height: 1.0
+  max_obstacle_height: 3.0
   origin_z: 0.0
-  z_resolution: 0.15
+  z_resolution: 0.2
-  z_voxels: 8
+  z_voxels: 16
   unknown_threshold: 15.0
   mark_threshold: 0
   combination_method: 1
-  # Camera-only ghost handling: LETHAL cells not re-observed for this many
-  # seconds are freed. Covers cells frustum clearing can never reach (outside
-  # 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
-  # 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).

launch/depth_local_costmap_test.launch

@@ -17,10 +17,9 @@
   <arg name="feeder_performance_metrics_period" default="5.0"/>
   <arg name="rviz" default="false"/>
   <arg name="frustum_clearing_pixel_step" default="2"/>
-  <!-- Camera-only ghost-obstacle handling: LETHAL cells not re-observed for
+  <!-- Time-based decay removed (needed realtime clearing). Clearing is done by
-       this many seconds are freed (covers cells outside the current FOV that
+       the per-pixel frustum raytracing + full-column clearing every frame. -->
-       frustum clearing can never reach). 0 disables decay. -->
+  <arg name="obstacle_decay_time" default="0.0"/>
-  <arg name="obstacle_decay_time" default="4.0"/>
   <!-- Clearing rays stop this far [m] before the measured surface. Smaller
        than the legacy 2*resolution (0.10) so near-floor / near-wall ghost
        cells inside the old skip band still get cleared. -->
@@ -37,6 +36,13 @@
        still get cleared). -->
   <arg name="column_skip_distance" default="0.02"/>
   <arg name="column_cover_distance" default="0.2"/>
+  <!-- Upper bound [px] on left-edge columns auto-excluded from depth clearing.
+       The RealSense stereo no-disparity strip is permanently invalid there and
+       WIDENS as the scene gets closer; each frame the actual leading run of
+       all-invalid columns is measured and skipped, capped at this value. Set to
+       the WIDEST black strip observed (when closest to obstacles) + a margin.
+       0 disables. -->
+  <arg name="frustum_clear_left_border_px" default="80"/>
 
   <node pkg="depth_local_costmap_noetic_test"
         type="depth_local_costmap_noetic_test_node"
@@ -100,6 +106,7 @@
     <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)"/>
+    <param name="local_costmap/obstacles/pc_clearing/frustum_clear_left_border_px" value="$(arg frustum_clear_left_border_px)"/>
   </node>
 
   <node if="$(arg rviz)"