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

:optimal 27/7 14:45

Browse Source
This commit is contained in:
duongtd
2026-07-27 14:45:26 +07:00
parent ffe2f77c1b
commit c2944f7a98
2 changed files with 9 additions and 4 deletions
Download Patch File Download Diff File Expand all files Collapse all files

5
plugins/obstacle_layer.cpp
View File

@@ -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, " 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\n", "column_skip: %.3f m, column_cover: %.2f m, clear_left_border_px: %f 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);
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);

8
src/costmap_2d_robot.cpp
View File

@@ -406,6 +406,7 @@ void Costmap2DROBOT::copyParentParameters(const std::string& costmap_name,
double column_clear_max_height = -1.0; double column_clear_max_height = -1.0;
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;
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);
@@ -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_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_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);
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_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_clear_max_height = -1.0;
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;
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);
@@ -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_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_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);
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_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);
} }
} }
} }