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

:optimal test NUC 27/7 15:14

Browse Source
This commit is contained in:
duongtd
2026-07-27 15:14:39 +07:00
parent c17ac9fc06
commit 9f7e2f82f1
5 changed files with 39 additions and 17 deletions
Download Patch File Download Diff File Expand all files Collapse all files

12
plugins/obstacle_layer.cpp
View File

@@ -171,6 +171,9 @@ bool ObstacleLayer::getParams(const std::string& config_file_name, robot::NodeHa
int frustum_clear_left_border =
loadParam(layer[source], "frustum_clear_left_border_px",
loadParam(layer, "frustum_clear_left_border_px", 0));
int frustum_clear_right_border =
loadParam(layer[source], "frustum_clear_right_border_px",
loadParam(layer, "frustum_clear_right_border_px", 0));
if (priv_nh.hasParam("topic"))
priv_nh.getParam("topic", topic);
@@ -217,22 +220,27 @@ bool ObstacleLayer::getParams(const std::string& config_file_name, robot::NodeHa
priv_nh.getParam("column_cover_distance", frustum_config.column_cover_distance);
if (priv_nh.hasParam("frustum_clear_left_border_px"))
priv_nh.getParam("frustum_clear_left_border_px", frustum_clear_left_border);
if (priv_nh.hasParam("frustum_clear_right_border_px"))
priv_nh.getParam("frustum_clear_right_border_px", frustum_clear_right_border);
if (priv_nh.hasParam("frustum_depth_camera_topic"))
priv_nh.getParam("frustum_depth_camera_topic", depth_camera_data_topic_);
frustum_config.pixel_step = static_cast<unsigned int>(std::max(1, frustum_pixel_step));
frustum_config.clear_left_border_px =
static_cast<unsigned int>(std::max(0, frustum_clear_left_border));
frustum_config.clear_right_border_px =
static_cast<unsigned int>(std::max(0, frustum_clear_right_border));
robot::log_info("source %s: frustum_clearing_enabled: %s, pixel_step: %u, range: [%.2f, %.2f] m, "
"skip: %.3f m, column_clearing: %s, column_band: [%.2f, %.2f] m, "
"column_skip: %.3f m, column_cover: %.2f m, clear_left_border_px: %f px\n",
"column_skip: %.3f m, column_cover: %.2f m, clear_left_border_px: %u px, "
"clear_right_border_px: %u px\n",
source.c_str(), frustum_clearing_enabled ? "true" : "false",
frustum_config.pixel_step, frustum_config.min_range, frustum_config.max_range,
frustum_config.skip_distance, frustum_config.column_clearing ? "true" : "false",
frustum_config.column_min_height, frustum_config.column_max_height,
frustum_config.column_skip_distance, frustum_config.column_cover_distance,
frustum_config.clear_left_border_px);
frustum_config.clear_left_border_px, frustum_config.clear_right_border_px);
double obstacle_range = 2.5;
obstacle_range = loadParam(layer[source],"obstacle_range", obstacle_range);

30
plugins/voxel_layer.cpp
View File

@@ -744,15 +744,21 @@ bool VoxelLayer::raytraceDepthFrustum(const DepthCameraObservation& observation,
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).
// Edge stereo no-disparity strips (left and/or right, depending on the
// camera). An INVALID pixel in such a strip 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 turn bug) — skip only those. A
// VALID return there is a real measured surface, so it must still clear
// normally; otherwise the border becomes a clearing dead zone and obstacles
// there never get cleared. Invalid pixels OUTSIDE the strips still clear to
// max_range (ghost removal). Right edge measured inward from the last column;
// the unsigned test avoids underflow when the border exceeds the width.
const bool in_left_border = local_ray.u < frustum.clear_left_border_px;
if (in_left_border && !valid)
const bool in_right_border =
frustum.clear_right_border_px > 0 &&
local_ray.u + frustum.clear_right_border_px >= width;
const bool in_border = in_left_border || in_right_border;
if (in_border && !valid)
continue;
// depth images store z-depth; local_ray.z is the unit ray's optical axis
@@ -779,8 +785,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 (in_border)
stat.in_border = true;
if (valid)
{
@@ -904,11 +910,11 @@ 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
// In an edge 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)
if (stat.in_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;