DuongTD/costmap_2d
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 16:12:55 +07:00
parent 0e84ac53cb
commit c888af3b7c
7 changed files with 170 additions and 132 deletions
Download Patch File Download Diff File Expand all files Collapse all files

27
include/robot_costmap_2d/voxel_layer.h
View File

@@ -114,25 +114,12 @@ private:
bool publish_voxel_;
robot_voxel_grid::VoxelGrid robot_voxel_grid_;
double z_resolution_, origin_z_;
/// Clearing rays stop this far [m] before the measured surface.
/// Negative keeps the legacy 2 * resolution behaviour.
double frustum_skip_distance_ = -1.0;
/// Full-column clearing: per depth-image pixel column, the nearest return
/// inside the obstacle height band certifies "no obstacle in this direction
/// closer than d". Cells along that 2D beam get their whole voxel column
/// cleared, removing marked voxels the per-pixel 3D rays cannot reach
/// (above the vertical FOV at close range).
bool frustum_column_clearing_ = false;
/// Height band [m] used to detect the nearest in-band return. Points below
/// the band (floor) do not shorten the beam. max < 0: use max_obstacle_height.
double column_clear_min_height_ = 0.10;
double column_clear_max_height_ = -1.0;
/// Full columns are only cleared beyond this distance [m], where the
/// vertical FOV covers the whole height band. Negative: derive each frame
/// from the camera intrinsics and mounting pose.
double column_cover_distance_ = -1.0;
/// Scratch for the full-column clearing pass (config lives per observation
/// source in DepthFrustumConfig): per depth-image pixel column, the nearest
/// return inside the obstacle height band certifies "no obstacle in this
/// direction closer than d". Cells along that 2D beam get their whole voxel
/// column cleared, removing marked voxels the per-pixel 3D rays cannot
/// reach (above the vertical FOV at close range).
struct DepthColumnStat
{
double min_band_dist = -1.0; ///< horizontal distance of nearest in-band return; < 0 = none
@@ -150,11 +137,13 @@ private:
{
unsigned int u;
unsigned int v;
unsigned int col; ///< pixel-column index in the cache (border column included)
double x;
double y;
double z;
};
std::vector<DepthRay> depth_ray_cache_;
unsigned int cached_column_count_ = 0;
unsigned int cached_depth_width_ = 0;
unsigned int cached_depth_height_ = 0;
unsigned int cached_depth_pixel_step_ = 0;