DuongTD/depth_local_costmap_noetic_test
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

otimal

Browse Source
This commit is contained in:
duongtd
2026-07-23 15:21:53 +07:00
parent 937aa00ec8
commit 490c24616b
6 changed files with 54 additions and 7 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
config/config/costmap_common_params.yaml
View File

@@ -30,7 +30,7 @@ obstacles:
marking: true
inf_is_valid: false
frustum_clearing_enabled: true
frustum_clearing_pixel_step: 8
frustum_clearing_pixel_step: 4
frustum_min_range: 0.20
frustum_max_range: 3.5
observation_persistence: 0.0

6
config/config/costmap_params.yaml
View File

@@ -1,7 +1,9 @@
robot_costmap_2d:
# Derived from pnkx_nav_core/config/costmap_local_params.yaml.
# NUC path is intentional for the user's local test workspace.
library_path: /home/robotics/DuongTD/dev_ws/devel/lib/libplugins.so
# 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

2
config/config/obstacle_layer_params.yaml
View File

@@ -30,7 +30,7 @@ obstacle_layer:
marking: true
inf_is_valid: false
frustum_clearing_enabled: true
frustum_clearing_pixel_step: 8
frustum_clearing_pixel_step: 4
frustum_min_range: 0.20
frustum_max_range: 3.5
observation_persistence: 0.0

23
config/config/voxel_layer_params.yaml
View File

@@ -8,9 +8,30 @@ voxel_layer:
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
# 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: 8
frustum_clearing_pixel_step: 4
frustum_min_range: 0.20
frustum_max_range: 3.5
frustum_depth_camera_topic: /camera/depth/data

2
config/costmap_common_params.yaml
View File

@@ -30,7 +30,7 @@ obstacles:
marking: true
inf_is_valid: false
frustum_clearing_enabled: true
frustum_clearing_pixel_step: 8
frustum_clearing_pixel_step: 4
frustum_min_range: 0.20
frustum_max_range: 3.5
observation_persistence: 0.0

26
launch/depth_local_costmap_test.launch
View File

@@ -12,6 +12,23 @@
<arg name="publish_odom_tf" default="false"/>
<arg name="publish_rate" default="5.0"/>
<arg name="rviz" default="true"/>
<arg name="frustum_clearing_pixel_step" default="4"/>
<!-- Camera-only ghost-obstacle handling: LETHAL cells not re-observed for
this many seconds are freed (covers cells outside the current FOV that
frustum clearing can never reach). 0 disables decay. -->
<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. -->
<arg name="frustum_skip_distance" default="0.05"/>
<!-- Full-column clearing from the depth image: the nearest in-band return
per pixel column certifies the beam free and wipes whole voxel columns,
removing ghost voxels above the vertical FOV that 3D rays miss.
Measurement-driven; obstacles outside the FOV are never touched. -->
<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"/>
<arg name="column_cover_distance" default="-1.0"/>
<node pkg="depth_local_costmap_noetic_test"
type="depth_local_costmap_noetic_test_node"
@@ -52,7 +69,7 @@
<param name="local_costmap/obstacles/pc_clearing/marking" value="true"/>
<param name="local_costmap/obstacles/pc_clearing/inf_is_valid" value="false"/>
<param name="local_costmap/obstacles/pc_clearing/frustum_clearing_enabled" value="true"/>
<param name="local_costmap/obstacles/pc_clearing/frustum_clearing_pixel_step" value="8"/>
<param name="local_costmap/obstacles/pc_clearing/frustum_clearing_pixel_step" value="$(arg frustum_clearing_pixel_step)"/>
<param name="local_costmap/obstacles/pc_clearing/frustum_min_range" value="0.20"/>
<param name="local_costmap/obstacles/pc_clearing/frustum_max_range" value="3.5"/>
<param name="local_costmap/obstacles/pc_clearing/observation_persistence" value="0.0"/>
@@ -62,6 +79,13 @@
<param name="local_costmap/obstacles/pc_clearing/min_obstacle_height" value="0.1"/>
<param name="local_costmap/obstacles/pc_clearing/max_obstacle_height" value="1.0"/>
<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)"/>
</node>
<node if="$(arg rviz)"