From 67eae01a947f0e9752700e8e2881e7ef0147133d Mon Sep 17 00:00:00 2001 From: Kyoichi Sugahara Date: Sat, 16 Sep 2023 18:17:49 +0900 Subject: [PATCH] fix(behavior_path_planner): define hysteresis_factor_expand_rate (#5002) * define hysteresis_factor_expand_rate Signed-off-by: kyoichi-sugahara * add hysteresis_factor_expand_rate in SafetyCheckParams Signed-off-by: kyoichi-sugahara * style(pre-commit): autofix * revert unnecessary change Signed-off-by: kyoichi-sugahara --------- Signed-off-by: kyoichi-sugahara Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> --- .../config/goal_planner/goal_planner.param.yaml | 2 ++ .../config/start_planner/start_planner.param.yaml | 2 ++ .../path_safety_checker/path_safety_checker_parameters.hpp | 4 +++- .../src/scene_module/goal_planner/manager.cpp | 2 ++ .../src/scene_module/start_planner/manager.cpp | 2 ++ .../src/scene_module/start_planner/start_planner_module.cpp | 2 +- 6 files changed, 12 insertions(+), 2 deletions(-) diff --git a/planning/behavior_path_planner/config/goal_planner/goal_planner.param.yaml b/planning/behavior_path_planner/config/goal_planner/goal_planner.param.yaml index 709f271ac265c..779e7af61699f 100644 --- a/planning/behavior_path_planner/config/goal_planner/goal_planner.param.yaml +++ b/planning/behavior_path_planner/config/goal_planner/goal_planner.param.yaml @@ -162,6 +162,8 @@ lateral_distance_max_threshold: 1.0 longitudinal_distance_min_threshold: 1.0 longitudinal_velocity_delta_time: 1.0 + # hysteresis factor to expand/shrink polygon with the value + hysteresis_factor_expand_rate: 1.0 # temporary backward_path_length: 30.0 forward_path_length: 100.0 diff --git a/planning/behavior_path_planner/config/start_planner/start_planner.param.yaml b/planning/behavior_path_planner/config/start_planner/start_planner.param.yaml index b62262423fa72..586676fbbb0f0 100644 --- a/planning/behavior_path_planner/config/start_planner/start_planner.param.yaml +++ b/planning/behavior_path_planner/config/start_planner/start_planner.param.yaml @@ -129,6 +129,8 @@ lateral_distance_max_threshold: 1.0 longitudinal_distance_min_threshold: 1.0 longitudinal_velocity_delta_time: 1.0 + # hysteresis factor to expand/shrink polygon + hysteresis_factor_expand_rate: 1.0 # temporary backward_path_length: 30.0 forward_path_length: 100.0 diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/path_safety_checker/path_safety_checker_parameters.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/path_safety_checker/path_safety_checker_parameters.hpp index 475b38f5bb824..b2433c076b1a8 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/path_safety_checker/path_safety_checker_parameters.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/path_safety_checker/path_safety_checker_parameters.hpp @@ -169,7 +169,9 @@ struct ObjectsFilteringParams */ struct SafetyCheckParams { - bool enable_safety_check; ///< Enable safety checks. + bool enable_safety_check; ///< Enable safety checks. + double + hysteresis_factor_expand_rate; ///< Hysteresis factor to expand/shrink polygon with the value. double backward_path_length; ///< Length of the backward lane for path generation. double forward_path_length; ///< Length of the forward path lane for path generation. RSSparams rss_params; ///< Parameters related to the RSS model. diff --git a/planning/behavior_path_planner/src/scene_module/goal_planner/manager.cpp b/planning/behavior_path_planner/src/scene_module/goal_planner/manager.cpp index 9d794b178b60e..2c7053cfb3629 100644 --- a/planning/behavior_path_planner/src/scene_module/goal_planner/manager.cpp +++ b/planning/behavior_path_planner/src/scene_module/goal_planner/manager.cpp @@ -316,6 +316,8 @@ GoalPlannerModuleManager::GoalPlannerModuleManager( { p.safety_check_params.enable_safety_check = node->declare_parameter(safety_check_ns + "enable_safety_check"); + p.safety_check_params.hysteresis_factor_expand_rate = + node->declare_parameter(safety_check_ns + "hysteresis_factor_expand_rate"); p.safety_check_params.backward_path_length = node->declare_parameter(safety_check_ns + "backward_path_length"); p.safety_check_params.forward_path_length = diff --git a/planning/behavior_path_planner/src/scene_module/start_planner/manager.cpp b/planning/behavior_path_planner/src/scene_module/start_planner/manager.cpp index 09166cd72a83e..57c9690e60d4b 100644 --- a/planning/behavior_path_planner/src/scene_module/start_planner/manager.cpp +++ b/planning/behavior_path_planner/src/scene_module/start_planner/manager.cpp @@ -243,6 +243,8 @@ StartPlannerModuleManager::StartPlannerModuleManager( { p.safety_check_params.enable_safety_check = node->declare_parameter(safety_check_ns + "enable_safety_check"); + p.safety_check_params.hysteresis_factor_expand_rate = + node->declare_parameter(safety_check_ns + "hysteresis_factor_expand_rate"); p.safety_check_params.backward_path_length = node->declare_parameter(safety_check_ns + "backward_path_length"); p.safety_check_params.forward_path_length = diff --git a/planning/behavior_path_planner/src/scene_module/start_planner/start_planner_module.cpp b/planning/behavior_path_planner/src/scene_module/start_planner/start_planner_module.cpp index 58e493bf40f50..3e480e0f517ea 100644 --- a/planning/behavior_path_planner/src/scene_module/start_planner/start_planner_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/start_planner/start_planner_module.cpp @@ -1004,7 +1004,7 @@ bool StartPlannerModule::isSafePath() const current_lanes, route_handler, filtered_objects, objects_filtering_params_); const double hysteresis_factor = - status_.is_safe_dynamic_objects ? 1.0 : parameters_->hysteresis_factor_expand_rate; + status_.is_safe_dynamic_objects ? 1.0 : safety_check_params_->hysteresis_factor_expand_rate; utils::start_goal_planner_common::updateSafetyCheckTargetObjectsData( start_planner_data_, filtered_objects, target_objects_on_lane, ego_predicted_path);