From c2944f7a98086ff97c253b115c9f3911ccd08607 Mon Sep 17 00:00:00 2001 From: duongtd Date: Mon, 27 Jul 2026 14:45:26 +0700 Subject: [PATCH] :optimal 27/7 14:45 --- plugins/obstacle_layer.cpp | 5 +++-- src/costmap_2d_robot.cpp | 8 ++++++-- 2 files changed, 9 insertions(+), 4 deletions(-) diff --git a/plugins/obstacle_layer.cpp b/plugins/obstacle_layer.cpp index 7c4851a..a655fde 100755 --- a/plugins/obstacle_layer.cpp +++ b/plugins/obstacle_layer.cpp @@ -226,12 +226,13 @@ bool ObstacleLayer::getParams(const std::string& config_file_name, robot::NodeHa 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\n", + "column_skip: %.3f m, column_cover: %.2f m, clear_left_border_px: %f 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.column_skip_distance, frustum_config.column_cover_distance, + frustum_config.clear_left_border_px); double obstacle_range = 2.5; obstacle_range = loadParam(layer[source],"obstacle_range", obstacle_range); diff --git a/src/costmap_2d_robot.cpp b/src/costmap_2d_robot.cpp index 582cc14..c5f28de 100644 --- a/src/costmap_2d_robot.cpp +++ b/src/costmap_2d_robot.cpp @@ -406,6 +406,7 @@ void Costmap2DROBOT::copyParentParameters(const std::string& costmap_name, double column_clear_max_height = -1.0; double column_skip_distance = 0.02; double column_cover_distance = -1.0; + int frustum_clear_left_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); @@ -429,8 +430,9 @@ void Costmap2DROBOT::copyParentParameters(const std::string& costmap_name, move_parameter(plugin_nh_element, costmap_plugin_nh_element, "column_clear_max_height", column_clear_max_height); 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); 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_clearing_enabled ? "true" : "false", frustum_clearing_pixel_step, frustum_min_range, frustum_max_range); + 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); } } } @@ -477,6 +479,7 @@ void Costmap2DROBOT::copyParentParameters(const std::string& costmap_name, double column_clear_max_height = -1.0; double column_skip_distance = 0.02; double column_cover_distance = -1.0; + int frustum_clear_left_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); @@ -500,8 +503,9 @@ void Costmap2DROBOT::copyParentParameters(const std::string& costmap_name, move_parameter(plugin_nh_element, costmap_plugin_nh_element, "column_clear_max_height", column_clear_max_height); 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); 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_clearing_enabled ? "true" : "false", frustum_clearing_pixel_step, frustum_min_range, frustum_max_range); + 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); } } }