diff --git a/include/robot_costmap_2d/voxel_layer.h b/include/robot_costmap_2d/voxel_layer.h index bcfd113..a234d1e 100755 --- a/include/robot_costmap_2d/voxel_layer.h +++ b/include/robot_costmap_2d/voxel_layer.h @@ -126,6 +126,7 @@ private: double azimuth = 0.0; ///< beam direction in the global frame double best_row_delta = std::numeric_limits::infinity(); 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 depth_column_stats_; unsigned int unknown_threshold_, mark_threshold_, size_z_; diff --git a/plugins/voxel_layer.cpp b/plugins/voxel_layer.cpp index 78aa738..11509dc 100755 --- a/plugins/voxel_layer.cpp +++ b/plugins/voxel_layer.cpp @@ -739,19 +739,22 @@ bool VoxelLayer::raytraceDepthFrustum(const DepthCameraObservation& observation, 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; bool valid = false; if (!readDepthMeters(depth, local_ray.u, local_ray.v, depth_m, valid)) 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 // component, so depth / z is the Euclidean range 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.has_ray = true; + if (in_left_border) + stat.in_left_border = true; if (valid) { @@ -899,6 +904,13 @@ bool VoxelLayer::clearDepthColumns(double ox, double oy, double cover_distance, if (!stat.has_ray) 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; end_dist = std::min(std::min(end_dist, max_range), far_distance); if (end_dist <= start_dist)