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

otimal

Browse Source
This commit is contained in:
duongtd
2026-07-23 16:12:55 +07:00
parent 0e84ac53cb
commit c888af3b7c
7 changed files with 170 additions and 132 deletions
Download Patch File Download Diff File Expand all files Collapse all files

60
plugins/obstacle_layer.cpp
View File

@@ -133,8 +133,7 @@ bool ObstacleLayer::getParams(const std::string& config_file_name, robot::NodeHa
bool inf_is_valid = false, clearing=false, marking=true;
bool frustum_clearing_enabled = false;
int frustum_pixel_step = 8;
double frustum_min_range = 0.2;
double frustum_max_range = 3.0;
DepthFrustumConfig frustum_config;
robot::NodeHandle priv_nh(nh, source);
topic = loadParam(layer[source],"topic", topic);
@@ -147,10 +146,28 @@ bool ObstacleLayer::getParams(const std::string& config_file_name, robot::NodeHa
inf_is_valid = loadParam(layer[source],"inf_is_valid", false);
clearing = loadParam(layer[source],"clearing", false);
marking = loadParam(layer[source],"marking", true);
frustum_clearing_enabled = loadParam(layer, "frustum_clearing_enabled", false);
frustum_pixel_step = loadParam(layer, "frustum_clearing_pixel_step", 8);
frustum_min_range = loadParam(layer, "frustum_min_range", 0.2);
frustum_max_range = loadParam(layer, "frustum_max_range", 3.0);
// frustum params are per-source; the layer-level key is kept as a
// fallback for older YAMLs
frustum_clearing_enabled = loadParam(layer[source], "frustum_clearing_enabled",
loadParam(layer, "frustum_clearing_enabled", false));
frustum_pixel_step = loadParam(layer[source], "frustum_clearing_pixel_step",
loadParam(layer, "frustum_clearing_pixel_step", 8));
frustum_config.min_range = loadParam(layer[source], "frustum_min_range",
loadParam(layer, "frustum_min_range", 0.2));
frustum_config.max_range = loadParam(layer[source], "frustum_max_range",
loadParam(layer, "frustum_max_range", 3.0));
frustum_config.skip_distance =
loadParam(layer[source], "frustum_skip_distance", frustum_config.skip_distance);
frustum_config.column_clearing =
loadParam(layer[source], "frustum_column_clearing", frustum_config.column_clearing);
frustum_config.column_min_height =
loadParam(layer[source], "column_clear_min_height", frustum_config.column_min_height);
frustum_config.column_max_height =
loadParam(layer[source], "column_clear_max_height", frustum_config.column_max_height);
frustum_config.column_skip_distance =
loadParam(layer[source], "column_skip_distance", frustum_config.column_skip_distance);
frustum_config.column_cover_distance =
loadParam(layer[source], "column_cover_distance", frustum_config.column_cover_distance);
if (priv_nh.hasParam("topic"))
priv_nh.getParam("topic", topic);
@@ -180,13 +197,34 @@ bool ObstacleLayer::getParams(const std::string& config_file_name, robot::NodeHa
frustum_pixel_step = std::max(1, frustum_pixel_step);
}
if (priv_nh.hasParam("frustum_min_range"))
priv_nh.getParam("frustum_min_range", frustum_min_range);
priv_nh.getParam("frustum_min_range", frustum_config.min_range);
if (priv_nh.hasParam("frustum_max_range"))
priv_nh.getParam("frustum_max_range", frustum_max_range);
priv_nh.getParam("frustum_max_range", frustum_config.max_range);
if (priv_nh.hasParam("frustum_skip_distance"))
priv_nh.getParam("frustum_skip_distance", frustum_config.skip_distance);
if (priv_nh.hasParam("frustum_column_clearing"))
priv_nh.getParam("frustum_column_clearing", frustum_config.column_clearing);
if (priv_nh.hasParam("column_clear_min_height"))
priv_nh.getParam("column_clear_min_height", frustum_config.column_min_height);
if (priv_nh.hasParam("column_clear_max_height"))
priv_nh.getParam("column_clear_max_height", frustum_config.column_max_height);
if (priv_nh.hasParam("column_skip_distance"))
priv_nh.getParam("column_skip_distance", frustum_config.column_skip_distance);
if (priv_nh.hasParam("column_cover_distance"))
priv_nh.getParam("column_cover_distance", frustum_config.column_cover_distance);
if (priv_nh.hasParam("frustum_depth_camera_topic"))
priv_nh.getParam("frustum_depth_camera_topic", depth_camera_data_topic_);
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_pixel_step, frustum_min_range, frustum_max_range);
frustum_config.pixel_step = static_cast<unsigned int>(std::max(1, frustum_pixel_step));
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",
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);
double obstacle_range = 2.5;
obstacle_range = loadParam(layer[source],"obstacle_range", obstacle_range);
@@ -244,8 +282,8 @@ bool ObstacleLayer::getParams(const std::string& config_file_name, robot::NodeHa
depth_observation_buffers_.push_back(
boost::shared_ptr < ObservationBuffer
> (new ObservationBuffer(topic, observation_keep_time, expected_update_rate, min_obstacle_height,
max_obstacle_height, obstacle_range, raytrace_range, frustum_pixel_step,
frustum_min_range, frustum_max_range, *tf_, global_frame_,
max_obstacle_height, obstacle_range, raytrace_range, frustum_config,
*tf_, global_frame_,
sensor_frame, transform_tolerance)));
}