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

:optimal 27/7 13:48

Browse Source
This commit is contained in:
duongtd
2026-07-27 13:48:38 +07:00
parent 4693f71904
commit bdba382139
8 changed files with 56 additions and 187 deletions
Download Patch File Download Diff File Expand all files Collapse all files

47
config/config/costmap_common_params.yaml
View File

@@ -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

32
config/config/costmap_local_params.yaml
View File

@@ -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

12
config/config/costmap_local_params_plugins_no_virtual_walls.yaml
View File

@@ -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

47
config/config/costmap_params.yaml
View File

@@ -1,35 +1,38 @@
robot_costmap_2d:
# Derived from pnkx_nav_core/config/costmap_local_params.yaml.
# 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
global_frame: map
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: obstacles
type: VoxelLayer
- name: inflation
- name: static_layer
type: StaticLayer
- name: inflation_layer
type: InflationLayer
footprint:
- [0.30, 0.30]
- [0.30, -0.30]
- [-0.30, -0.30]
- [-0.30, 0.30]
- name: obstacle_layer
type: ObstacleLayer
transform_tolerance: 1.0
performance_metrics_enabled: true
- name: voxel_layer
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
width: 8.0
height: 8.0
resolution: 0.05
update_frequency: 1.0
width: 0.0
height: 0.0
resolution: 0.0
origin_x: 0.0
origin_y: 0.0
footprint_padding: 0.0
robot_radius: 0.0

4
config/config/inflation_layer_params.yaml
View File

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

69
config/config/obstacle_layer_params.yaml
View File

@@ -1,59 +1,18 @@
obstacle_layer:
enabled: true
track_unknown_space: false
transform_tolerance: 1.0
track_unknown_space: true
transform_tolerance: 0.2
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

17
config/config/voxel_layer_params.yaml
View File

@@ -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_voxels: 8
z_resolution: 0.2
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).
combination_method: 1

15
launch/depth_local_costmap_test.launch
View File

@@ -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
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"/>
<!-- Time-based decay removed (needed realtime clearing). Clearing is done by
the per-pixel frustum raytracing + full-column clearing every frame. -->
<arg name="obstacle_decay_time" default="0.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)"