fix(avoidance): don't output new candidate path if there is huge offs…

…et between the ego and previous output path

Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com>

planning/behavior_path_planner/config/avoidance/avoidance.param.yaml

@@ -127,6 +127,7 @@
         safety_check_idling_time: 1.5                    # [s]
         safety_check_accel_for_rss: 2.5                  # [m/ss]
         safety_check_hysteresis_factor: 2.0              # [-]
+        safety_check_ego_offset: 1.0                     # [m]
 
       # For avoidance maneuver
       avoidance:

planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/avoidance_module_data.hpp

@@ -187,6 +187,9 @@ struct AvoidanceParameters
   // transit hysteresis (unsafe to safe)
   double safety_check_hysteresis_factor;
 
+  // don't output new candidate path if the offset between ego and path is larger than this.
+  double safety_check_ego_offset;
+
   // keep target velocity in yield maneuver
   double yield_velocity;
 

planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp

@@ -494,6 +494,21 @@ void AvoidanceModule::fillEgoStatus(
 
   const auto can_yield_maneuver = canYieldManeuver(data);
 
+  const size_t ego_seg_idx =
+    planner_data_->findEgoSegmentIndex(helper_.getPreviousSplineShiftPath().path.points);
+  const auto offset = std::abs(motion_utils::calcLateralOffset(
+    helper_.getPreviousSplineShiftPath().path.points, getEgoPosition(), ego_seg_idx));
+
+  // don't output new candidate path if the offset between the ego and previous output path is
+  // larger than threshold.
+  // TODO(Satoshi OTA): remove this workaround
+  if (offset > parameters_->safety_check_ego_offset) {
+    data.safe_new_sl.clear();
+    data.candidate_path = helper_.getPreviousSplineShiftPath();
+    RCLCPP_WARN_THROTTLE(getLogger(), *clock_, 500, "unsafe. canceling candidate path...");
+    return;
+  }
+
   /**
    * If the avoidance path is safe, use unapproved_new_sl for avoidance path generation.
    */

planning/behavior_path_planner/src/scene_module/avoidance/manager.cpp

@@ -142,6 +142,7 @@ AvoidanceModuleManager::AvoidanceModuleManager(
     p.safety_check_accel_for_rss = get_parameter<double>(node, ns + "safety_check_accel_for_rss");
     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");
   }
 
   // avoidance maneuver (lateral)