feat(safety_check): add hysteresis factor in safety check logic (auto…

…warefoundation#4735) * feat(safety_check): add hysteresis factor Signed-off-by: satoshi-ota <[email protected]> * feat(lane_change): add hysteresis factor Signed-off-by: satoshi-ota <[email protected]> * feat(avoidance): add hysteresis factor Signed-off-by: satoshi-ota <[email protected]> * feat(avoidance): add time series hysteresis Signed-off-by: satoshi-ota <[email protected]> --------- Signed-off-by: satoshi-ota <[email protected]>
tier4 · Sep 11, 2023 · 71277da · 71277da
1 parent 3beb77c
commit 71277da
Show file tree

Hide file tree

Showing 10 changed files with 28 additions and 14 deletions.
diff --git a/planning/behavior_path_planner/config/avoidance/avoidance.param.yaml b/planning/behavior_path_planner/config/avoidance/avoidance.param.yaml
@@ -145,8 +145,9 @@
         time_horizon_for_front_object: 10.0              # [s]
         time_horizon_for_rear_object: 10.0               # [s]
         safety_check_backward_distance: 100.0            # [m]
-        safety_check_hysteresis_factor: 2.0              # [-]
         safety_check_ego_offset: 1.0                     # [m]
+        hysteresis_factor_expand_rate: 2.0               # [-]
+        hysteresis_factor_safe_count: 10                 # [-]
         # rss parameters
         expected_front_deceleration: -1.0                # [m/ss]
         expected_rear_deceleration: -1.0                 # [m/ss]

diff --git a/...or_path_planner/include/behavior_path_planner/scene_module/avoidance/avoidance_module.hpp b/...or_path_planner/include/behavior_path_planner/scene_module/avoidance/avoidance_module.hpp
@@ -555,6 +555,8 @@ class AvoidanceModule : public SceneModuleInterface
 
   bool arrived_path_end_{false};
 
+  bool safe_{true};
+
   std::shared_ptr<AvoidanceParameters> parameters_;
 
   helper::avoidance::AvoidanceHelper helper_;
@@ -575,6 +577,8 @@ class AvoidanceModule : public SceneModuleInterface
 
   ObjectDataArray registered_objects_;
 
+  mutable size_t safe_count_{0};
+
   mutable ObjectDataArray ego_stopped_objects_;
 
   mutable ObjectDataArray stopped_objects_;

diff --git a/...vior_path_planner/include/behavior_path_planner/utils/avoidance/avoidance_module_data.hpp b/...vior_path_planner/include/behavior_path_planner/utils/avoidance/avoidance_module_data.hpp
@@ -201,7 +201,8 @@ struct AvoidanceParameters
   double safety_check_backward_distance;
 
   // transit hysteresis (unsafe to safe)
-  double safety_check_hysteresis_factor;
+  size_t hysteresis_factor_safe_count;
+  double hysteresis_factor_expand_rate;
 
   // don't output new candidate path if the offset between ego and path is larger than this.
   double safety_check_ego_offset;

diff --git a/...ior_path_planner/include/behavior_path_planner/utils/path_safety_checker/safety_check.hpp b/...ior_path_planner/include/behavior_path_planner/utils/path_safety_checker/safety_check.hpp
@@ -101,7 +101,7 @@ bool checkCollision(
   const ExtendedPredictedObject & target_object,
   const PredictedPathWithPolygon & target_object_path,
   const BehaviorPathPlannerParameters & common_parameters, const RSSparams & rss_parameters,
-  CollisionCheckDebug & debug);
+  const double hysteresis_factor, CollisionCheckDebug & debug);
 
 /**
  * @brief Check collision between ego path footprints with extra longitudinal stopping margin and

diff --git a/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp b/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp
@@ -1917,6 +1917,8 @@ bool AvoidanceModule::isSafePath(
     return true;
   }
 
+  const auto hysteresis_factor = safe_ ? 1.0 : parameters_->hysteresis_factor_expand_rate;
+
   const auto safety_check_target_objects = utils::avoidance::getSafetyCheckTargetObjects(
     avoidance_data_, planner_data_, parameters_, is_right_shift.value());
 
@@ -1937,13 +1939,16 @@ bool AvoidanceModule::isSafePath(
       CollisionCheckDebug collision{};
       if (!utils::path_safety_checker::checkCollision(
             shifted_path.path, ego_predicted_path, object, obj_path, p, parameters_->rss_params,
-            collision)) {
+            hysteresis_factor, collision)) {
+        safe_count_ = 0;
         return false;
       }
     }
   }
 
-  return true;
+  safe_count_++;
+
+  return safe_ || safe_count_ > parameters_->hysteresis_factor_safe_count;
 }
 
 void AvoidanceModule::generateExtendedDrivableArea(BehaviorModuleOutput & output) const
@@ -2589,6 +2594,8 @@ void AvoidanceModule::updateData()
   fillShiftLine(avoidance_data_, debug_data_);
   fillEgoStatus(avoidance_data_, debug_data_);
   fillDebugData(avoidance_data_, debug_data_);
+
+  safe_ = avoidance_data_.safe;
 }
 
 void AvoidanceModule::processOnEntry()

diff --git a/planning/behavior_path_planner/src/scene_module/avoidance/manager.cpp b/planning/behavior_path_planner/src/scene_module/avoidance/manager.cpp
@@ -155,9 +155,10 @@ AvoidanceModuleManager::AvoidanceModuleManager(
     p.safety_check_time_resolution = get_parameter<double>(node, ns + "time_resolution");
     p.safety_check_backward_distance =
       get_parameter<double>(node, ns + "safety_check_backward_distance");
-    p.safety_check_hysteresis_factor =
-      get_parameter<double>(node, ns + "safety_check_hysteresis_factor");
     p.safety_check_ego_offset = get_parameter<double>(node, ns + "safety_check_ego_offset");
+    p.hysteresis_factor_expand_rate =
+      get_parameter<double>(node, ns + "hysteresis_factor_expand_rate");
+    p.hysteresis_factor_safe_count = get_parameter<int>(node, ns + "hysteresis_factor_safe_count");
   }
 
   // safety check rss params

diff --git a/planning/behavior_path_planner/src/scene_module/lane_change/manager.cpp b/planning/behavior_path_planner/src/scene_module/lane_change/manager.cpp
@@ -294,8 +294,8 @@ AvoidanceByLaneChangeModuleManager::AvoidanceByLaneChangeModuleManager(
   // safety check
   {
     std::string ns = "avoidance.safety_check.";
-    p.safety_check_hysteresis_factor =
-      get_parameter<double>(node, ns + "safety_check_hysteresis_factor");
+    p.hysteresis_factor_expand_rate =
+      get_parameter<double>(node, ns + "hysteresis_factor_expand_rate");
   }
 
   avoidance_parameters_ = std::make_shared<AvoidanceByLCParameters>(p);

diff --git a/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp b/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp
@@ -1277,7 +1277,7 @@ PathSafetyStatus NormalLaneChange::isLaneChangePathSafe(
       obj, lane_change_parameters_->use_all_predicted_path);
     for (const auto & obj_path : obj_predicted_paths) {
       if (!utils::path_safety_checker::checkCollision(
-            path, ego_predicted_path, obj, obj_path, common_parameters, rss_params,
+            path, ego_predicted_path, obj, obj_path, common_parameters, rss_params, 1.0,
             current_debug_data.second)) {
         path_safety_status.is_safe = false;
         updateDebugInfo(current_debug_data, path_safety_status.is_safe);

diff --git a/planning/behavior_path_planner/src/utils/avoidance/utils.cpp b/planning/behavior_path_planner/src/utils/avoidance/utils.cpp
@@ -645,7 +645,7 @@ void fillAvoidanceNecessity(
   }
 
   // TRUE -> ? (check with hysteresis factor)
-  object_data.avoid_required = check_necessity(parameters->safety_check_hysteresis_factor);
+  object_data.avoid_required = check_necessity(parameters->hysteresis_factor_expand_rate);
 }
 
 void fillObjectStoppableJudge(

diff --git a/planning/behavior_path_planner/src/utils/path_safety_checker/safety_check.cpp b/planning/behavior_path_planner/src/utils/path_safety_checker/safety_check.cpp
@@ -238,7 +238,7 @@ bool checkCollision(
   const ExtendedPredictedObject & target_object,
   const PredictedPathWithPolygon & target_object_path,
   const BehaviorPathPlannerParameters & common_parameters, const RSSparams & rss_parameters,
-  CollisionCheckDebug & debug)
+  double hysteresis_factor, CollisionCheckDebug & debug)
 {
   debug.lerped_path.reserve(target_object_path.path.size());
 
@@ -291,8 +291,8 @@ bool checkCollision(
     const auto min_lon_length =
       calcMinimumLongitudinalLength(front_object_velocity, rear_object_velocity, rss_parameters);
 
-    const auto & lon_offset = std::max(rss_dist, min_lon_length);
-    const auto & lat_margin = rss_parameters.lateral_distance_max_threshold;
+    const auto & lon_offset = std::max(rss_dist, min_lon_length) * hysteresis_factor;
+    const auto & lat_margin = rss_parameters.lateral_distance_max_threshold * hysteresis_factor;
     const auto & extended_ego_polygon =
       is_object_front
         ? createExtendedPolygon(ego_pose, ego_vehicle_info, lon_offset, lat_margin, debug)