fix(behavior_path_planner): define hysteresis_factor_expand_rate (#5002)

* define hysteresis_factor_expand_rate Signed-off-by: kyoichi-sugahara <[email protected]> * add hysteresis_factor_expand_rate in SafetyCheckParams Signed-off-by: kyoichi-sugahara <[email protected]> * style(pre-commit): autofix * revert unnecessary change Signed-off-by: kyoichi-sugahara <[email protected]> --------- Signed-off-by: kyoichi-sugahara <[email protected]> Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com>
autowarefoundation · Sep 16, 2023 · 67eae01 · 67eae01
1 parent 3ae2018
commit 67eae01
Show file tree

Hide file tree

Showing 6 changed files with 12 additions and 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
@@ -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
@@ -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/...nclude/behavior_path_planner/utils/path_safety_checker/path_safety_checker_parameters.hpp b/...nclude/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
@@ -316,6 +316,8 @@ GoalPlannerModuleManager::GoalPlannerModuleManager(
   {
     p.safety_check_params.enable_safety_check =
       node->declare_parameter<bool>(safety_check_ns + "enable_safety_check");
+    p.safety_check_params.hysteresis_factor_expand_rate =
+      node->declare_parameter<double>(safety_check_ns + "hysteresis_factor_expand_rate");
     p.safety_check_params.backward_path_length =
       node->declare_parameter<double>(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
@@ -243,6 +243,8 @@ StartPlannerModuleManager::StartPlannerModuleManager(
   {
     p.safety_check_params.enable_safety_check =
       node->declare_parameter<bool>(safety_check_ns + "enable_safety_check");
+    p.safety_check_params.hysteresis_factor_expand_rate =
+      node->declare_parameter<double>(safety_check_ns + "hysteresis_factor_expand_rate");
     p.safety_check_params.backward_path_length =
       node->declare_parameter<double>(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
@@ -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);