diff --git a/include/robot_costmap_2d/observation.h b/include/robot_costmap_2d/observation.h index 75fbca7..82b5785 100755 --- a/include/robot_costmap_2d/observation.h +++ b/include/robot_costmap_2d/observation.h @@ -77,6 +77,10 @@ struct DepthFrustumConfig /// 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). 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 diff --git a/include/robot_costmap_2d/voxel_layer.h b/include/robot_costmap_2d/voxel_layer.h index a234d1e..52984fb 100755 --- a/include/robot_costmap_2d/voxel_layer.h +++ b/include/robot_costmap_2d/voxel_layer.h @@ -126,7 +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 + bool in_border = false; ///< column lies in a left/right no-disparity strip }; std::vector depth_column_stats_; unsigned int unknown_threshold_, mark_threshold_, size_z_; diff --git a/plugins/obstacle_layer.cpp b/plugins/obstacle_layer.cpp index a655fde..0501557 100755 --- a/plugins/obstacle_layer.cpp +++ b/plugins/obstacle_layer.cpp @@ -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(std::max(1, frustum_pixel_step)); frustum_config.clear_left_border_px = static_cast(std::max(0, frustum_clear_left_border)); + frustum_config.clear_right_border_px = + static_cast(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); diff --git a/plugins/voxel_layer.cpp b/plugins/voxel_layer.cpp index 11509dc..0042be8 100755 --- a/plugins/voxel_layer.cpp +++ b/plugins/voxel_layer.cpp @@ -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; diff --git a/src/costmap_2d_robot.cpp b/src/costmap_2d_robot.cpp index c5f28de..a566046 100644 --- a/src/costmap_2d_robot.cpp +++ b/src/costmap_2d_robot.cpp @@ -407,6 +407,7 @@ void Costmap2DROBOT::copyParentParameters(const std::string& costmap_name, double column_skip_distance = 0.02; double column_cover_distance = -1.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, "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_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_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("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_cover_distance = -1.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, "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_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_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("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); } } }