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

4
include/robot_costmap_2d/observation.h
View File

@@ -77,6 +77,10 @@ struct DepthFrustumConfig
/// width of the black strip in the raw depth image (a few px margin). 0 /// width of the black strip in the raw depth image (a few px margin). 0
/// disables. Invalid pixels ELSEWHERE still clear (needed for ghost removal). /// disables. Invalid pixels ELSEWHERE still clear (needed for ghost removal).
unsigned int clear_left_border_px = 0; unsigned int clear_left_border_px = 0;
/// Same as clear_left_border_px but for the RIGHT edge, for cameras whose
/// stereo no-disparity strip sits on the right instead of the left. Measured
/// from the last image column inward. 0 disables.
unsigned int clear_right_border_px = 0;
}; };
class DepthCameraObservation class DepthCameraObservation

2
include/robot_costmap_2d/voxel_layer.h
View File

@@ -126,7 +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 bool in_border = false; ///< column lies in a left/right 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_;

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 = int frustum_clear_left_border =
loadParam(layer[source], "frustum_clear_left_border_px", loadParam(layer[source], "frustum_clear_left_border_px",
loadParam(layer, "frustum_clear_left_border_px", 0)); 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")) if (priv_nh.hasParam("topic"))
priv_nh.getParam("topic", 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); priv_nh.getParam("column_cover_distance", frustum_config.column_cover_distance);
if (priv_nh.hasParam("frustum_clear_left_border_px")) if (priv_nh.hasParam("frustum_clear_left_border_px"))
priv_nh.getParam("frustum_clear_left_border_px", frustum_clear_left_border); 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")) if (priv_nh.hasParam("frustum_depth_camera_topic"))
priv_nh.getParam("frustum_depth_camera_topic", depth_camera_data_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.pixel_step = static_cast<unsigned int>(std::max(1, frustum_pixel_step));
frustum_config.clear_left_border_px = frustum_config.clear_left_border_px =
static_cast<unsigned int>(std::max(0, frustum_clear_left_border)); 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, " 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, " "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", source.c_str(), frustum_clearing_enabled ? "true" : "false",
frustum_config.pixel_step, frustum_config.min_range, frustum_config.max_range, frustum_config.pixel_step, frustum_config.min_range, frustum_config.max_range,
frustum_config.skip_distance, frustum_config.column_clearing ? "true" : "false", frustum_config.skip_distance, frustum_config.column_clearing ? "true" : "false",
frustum_config.column_min_height, frustum_config.column_max_height, frustum_config.column_min_height, frustum_config.column_max_height,
frustum_config.column_skip_distance, frustum_config.column_cover_distance, 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; double obstacle_range = 2.5;
obstacle_range = loadParam(layer[source],"obstacle_range", obstacle_range); 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)) 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 // Edge stereo no-disparity strips (left and/or right, depending on the
// invalid (carries no free-space evidence), so clearing it out to max_range // camera). An INVALID pixel in such a strip is structurally invalid (carries
// erases obstacles rotating out of the FOV on that side (the CW-turn bug) — // no free-space evidence), so clearing it out to max_range erases obstacles
// skip only those. A VALID return here is a real measured surface, so it must // rotating out of the FOV on that side (the turn bug) — skip only those. A
// still clear normally; otherwise the whole left border becomes a clearing // VALID return there is a real measured surface, so it must still clear
// dead zone and obstacles there never get cleared. Invalid pixels OUTSIDE the // normally; otherwise the border becomes a clearing dead zone and obstacles
// strip still clear to max_range (ghost removal). // 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; 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; 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
@@ -779,8 +785,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) if (in_border)
stat.in_left_border = true; stat.in_border = true;
if (valid) if (valid)
{ {
@@ -904,11 +910,11 @@ 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 // 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 // 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 // erases obstacles turning out of view. Only an in-band measured surface may
// shorten (and thus clear) a border column. // 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; 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;

8
src/costmap_2d_robot.cpp
View File

@@ -407,6 +407,7 @@ void Costmap2DROBOT::copyParentParameters(const std::string& costmap_name,
double column_skip_distance = 0.02; double column_skip_distance = 0.02;
double column_cover_distance = -1.0; double column_cover_distance = -1.0;
int frustum_clear_left_border_px = 0; int frustum_clear_left_border_px = 0;
int frustum_clear_right_border_px = 0;
move_parameter(plugin_nh_element, costmap_plugin_nh_element, "topic", topic); move_parameter(plugin_nh_element, costmap_plugin_nh_element, "topic", topic);
move_parameter(plugin_nh_element, costmap_plugin_nh_element, "sensor_frame", sensor_frame); move_parameter(plugin_nh_element, costmap_plugin_nh_element, "sensor_frame", sensor_frame);
@@ -431,8 +432,9 @@ void Costmap2DROBOT::copyParentParameters(const std::string& costmap_name,
move_parameter(plugin_nh_element, costmap_plugin_nh_element, "column_skip_distance", column_skip_distance); move_parameter(plugin_nh_element, costmap_plugin_nh_element, "column_skip_distance", column_skip_distance);
move_parameter(plugin_nh_element, costmap_plugin_nh_element, "column_cover_distance", column_cover_distance); move_parameter(plugin_nh_element, costmap_plugin_nh_element, "column_cover_distance", column_cover_distance);
move_parameter(plugin_nh_element, costmap_plugin_nh_element, "frustum_clear_left_border_px", frustum_clear_left_border_px); move_parameter(plugin_nh_element, costmap_plugin_nh_element, "frustum_clear_left_border_px", frustum_clear_left_border_px);
move_parameter(plugin_nh_element, costmap_plugin_nh_element, "frustum_clear_right_border_px", frustum_clear_right_border_px);
robot::log_info("topic: %s data_type: %s clearing: %d marking: %d inf_is_valid: %d min_obstacle_height: %f max_obstacle_height: %f", topic.c_str(), data_type.c_str(), clearing, marking, inf_is_valid, min_obstacle_height, max_obstacle_height); robot::log_info("topic: %s data_type: %s clearing: %d marking: %d inf_is_valid: %d min_obstacle_height: %f max_obstacle_height: %f", topic.c_str(), data_type.c_str(), clearing, marking, inf_is_valid, min_obstacle_height, max_obstacle_height);
robot::log_info("frustum_clearing_enabled: %s, frustum_clearing_pixel_step: %d, frustum_min_range: %f, frustum_max_range: %f, frustum_clear_left_border_px: %d", frustum_clearing_enabled ? "true" : "false", frustum_clearing_pixel_step, frustum_min_range, frustum_max_range, frustum_clear_left_border_px); robot::log_info("frustum_clearing_enabled: %s, frustum_clearing_pixel_step: %d, frustum_min_range: %f, frustum_max_range: %f, frustum_clear_left_border_px: %d, frustum_clear_right_border_px: %d", frustum_clearing_enabled ? "true" : "false", frustum_clearing_pixel_step, frustum_min_range, frustum_max_range, frustum_clear_left_border_px, frustum_clear_right_border_px);
} }
} }
} }
@@ -480,6 +482,7 @@ void Costmap2DROBOT::copyParentParameters(const std::string& costmap_name,
double column_skip_distance = 0.02; double column_skip_distance = 0.02;
double column_cover_distance = -1.0; double column_cover_distance = -1.0;
int frustum_clear_left_border_px = 0; int frustum_clear_left_border_px = 0;
int frustum_clear_right_border_px = 0;
move_parameter(plugin_nh_element, costmap_plugin_nh_element, "topic", topic); move_parameter(plugin_nh_element, costmap_plugin_nh_element, "topic", topic);
move_parameter(plugin_nh_element, costmap_plugin_nh_element, "sensor_frame", sensor_frame); move_parameter(plugin_nh_element, costmap_plugin_nh_element, "sensor_frame", sensor_frame);
@@ -504,8 +507,9 @@ void Costmap2DROBOT::copyParentParameters(const std::string& costmap_name,
move_parameter(plugin_nh_element, costmap_plugin_nh_element, "column_skip_distance", column_skip_distance); move_parameter(plugin_nh_element, costmap_plugin_nh_element, "column_skip_distance", column_skip_distance);
move_parameter(plugin_nh_element, costmap_plugin_nh_element, "column_cover_distance", column_cover_distance); move_parameter(plugin_nh_element, costmap_plugin_nh_element, "column_cover_distance", column_cover_distance);
move_parameter(plugin_nh_element, costmap_plugin_nh_element, "frustum_clear_left_border_px", frustum_clear_left_border_px); move_parameter(plugin_nh_element, costmap_plugin_nh_element, "frustum_clear_left_border_px", frustum_clear_left_border_px);
move_parameter(plugin_nh_element, costmap_plugin_nh_element, "frustum_clear_right_border_px", frustum_clear_right_border_px);
robot::log_info("topic: %s data_type: %s clearing: %d marking: %d inf_is_valid: %d min_obstacle_height: %f max_obstacle_height: %f", topic.c_str(), data_type.c_str(), clearing, marking, inf_is_valid, min_obstacle_height, max_obstacle_height); robot::log_info("topic: %s data_type: %s clearing: %d marking: %d inf_is_valid: %d min_obstacle_height: %f max_obstacle_height: %f", topic.c_str(), data_type.c_str(), clearing, marking, inf_is_valid, min_obstacle_height, max_obstacle_height);
robot::log_info("frustum_clearing_enabled: %s, frustum_clearing_pixel_step: %d, frustum_min_range: %f, frustum_max_range: %f, frustum_clear_left_border_px: %d", frustum_clearing_enabled ? "true" : "false", frustum_clearing_pixel_step, frustum_min_range, frustum_max_range, frustum_clear_left_border_px); robot::log_info("frustum_clearing_enabled: %s, frustum_clearing_pixel_step: %d, frustum_min_range: %f, frustum_max_range: %f, frustum_clear_left_border_px: %d, frustum_clear_right_border_px: %d", frustum_clearing_enabled ? "true" : "false", frustum_clearing_pixel_step, frustum_min_range, frustum_max_range, frustum_clear_left_border_px, frustum_clear_right_border_px);
} }
} }
} }