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

:optimal test NUC 27/7 14:52

Browse Source
This commit is contained in:
duongtd
2026-07-27 14:52:19 +07:00
parent c2944f7a98
commit c17ac9fc06
2 changed files with 21 additions and 8 deletions
Download Patch File Download Diff File Expand all files Collapse all files

1
include/robot_costmap_2d/voxel_layer.h
View File

@@ -126,6 +126,7 @@ private:
double azimuth = 0.0; ///< beam direction in the global frame double azimuth = 0.0; ///< beam direction in the global frame
double best_row_delta = std::numeric_limits<double>::infinity(); double best_row_delta = std::numeric_limits<double>::infinity();
bool has_ray = false; ///< column had at least one readable pixel bool has_ray = false; ///< column had at least one readable pixel
bool in_left_border = false; ///< column lies in the left no-disparity strip
}; };
std::vector<DepthColumnStat> depth_column_stats_; std::vector<DepthColumnStat> depth_column_stats_;
unsigned int unknown_threshold_, mark_threshold_, size_z_; unsigned int unknown_threshold_, mark_threshold_, size_z_;

28
plugins/voxel_layer.cpp
View File

@@ -739,19 +739,22 @@ bool VoxelLayer::raytraceDepthFrustum(const DepthCameraObservation& observation,
for (const DepthRay& local_ray : depth_ray_cache_) for (const DepthRay& local_ray : depth_ray_cache_)
{ {
// Skip the left-edge stereo no-disparity strip: those columns are
// permanently invalid, so clearing through them erases obstacles rotating
// out of the FOV on that side. Marking has no data there either, so nothing
// is lost. Invalid pixels ELSEWHERE still clear to max_range (ghost removal
// when an obstacle leaves and only far / open space remains behind it).
if (local_ray.u < frustum.clear_left_border_px)
continue;
double depth_m = 0.0; double depth_m = 0.0;
bool valid = false; bool valid = false;
if (!readDepthMeters(depth, local_ray.u, local_ray.v, depth_m, valid)) if (!readDepthMeters(depth, local_ray.u, local_ray.v, depth_m, valid))
continue; continue;
// Left-edge stereo no-disparity strip. An INVALID pixel here is structurally
// invalid (carries no free-space evidence), so clearing it out to max_range
// erases obstacles rotating out of the FOV on that side (the CW-turn bug) —
// skip only those. A VALID return here is a real measured surface, so it must
// still clear normally; otherwise the whole left border becomes a clearing
// dead zone and obstacles there never get cleared. Invalid pixels OUTSIDE the
// strip still clear to max_range (ghost removal).
const bool in_left_border = local_ray.u < frustum.clear_left_border_px;
if (in_left_border && !valid)
continue;
// depth images store z-depth; local_ray.z is the unit ray's optical axis // depth images store z-depth; local_ray.z is the unit ray's optical axis
// component, so depth / z is the Euclidean range // component, so depth / z is the Euclidean range
const double euclid_range = valid ? depth_m / local_ray.z : 0.0; const double euclid_range = valid ? depth_m / local_ray.z : 0.0;
@@ -776,6 +779,8 @@ bool VoxelLayer::raytraceDepthFrustum(const DepthCameraObservation& observation,
stat.best_row_delta = row_delta; stat.best_row_delta = row_delta;
} }
stat.has_ray = true; stat.has_ray = true;
if (in_left_border)
stat.in_left_border = true;
if (valid) if (valid)
{ {
@@ -899,6 +904,13 @@ bool VoxelLayer::clearDepthColumns(double ox, double oy, double cover_distance,
if (!stat.has_ray) if (!stat.has_ray)
continue; continue;
// In the left stereo strip, never clear a whole column out to max_range on a
// missing in-band return: that is exactly the no-free-space-evidence case that
// erases obstacles turning out of view. Only an in-band measured surface may
// shorten (and thus clear) a border column.
if (stat.in_left_border && stat.min_band_dist < 0.0)
continue;
double end_dist = stat.min_band_dist >= 0.0 ? stat.min_band_dist - skip_dist : max_range; double end_dist = stat.min_band_dist >= 0.0 ? stat.min_band_dist - skip_dist : max_range;
end_dist = std::min(std::min(end_dist, max_range), far_distance); end_dist = std::min(std::min(end_dist, max_range), far_distance);
if (end_dist <= start_dist) if (end_dist <= start_dist)