fix(intersection): yield stuck stop

planning/behavior_velocity_intersection_module/src/scene_intersection.cpp

@@ -1,4 +1,4 @@
 // Copyright 2020 Tier IV, Inc.
 //
 // Licensed under the Apache License, Version 2.0 (the "License");
 // you may not use this file except in compliance with the License.
@@ -1064,16 +1064,8 @@
   const bool yield_stuck_detected = checkYieldStuckVehicle(
     planner_data_, path_lanelets, intersection_lanelets.first_attention_area());
   if (yield_stuck_detected && stuck_stop_line_idx_opt) {
-    auto stuck_stop_line_idx = stuck_stop_line_idx_opt.value();
-    if (is_private_area_ && planner_param_.stuck_vehicle.enable_private_area_stuck_disregard) {
-      if (
-        default_stop_line_idx_opt &&
-        fromEgoDist(stuck_stop_line_idx) < -planner_param_.common.stop_overshoot_margin) {
-        stuck_stop_line_idx = default_stop_line_idx_opt.value();
-      }
-    } else {
-      return IntersectionModule::YieldStuckStop{closest_idx, stuck_stop_line_idx};
-    }
+    const auto stuck_stop_line_idx = stuck_stop_line_idx_opt.value();
+    return IntersectionModule::YieldStuckStop{closest_idx, stuck_stop_line_idx};
   }
 
   // if attention area is empty, collision/occlusion detection is impossible
@@ -1520,25 +1512,25 @@
                          .object_expected_deceleration));
     return dist_to_stop_line > braking_distance;
   };
   const auto isTolerableOvershoot = [&](const util::TargetObject & target_object) {
     if (
       !target_object.attention_lanelet || !target_object.dist_to_stop_line ||
       !target_object.stop_line) {
       return false;
     }
     const double dist_to_stop_line = target_object.dist_to_stop_line.value();
     const double v = target_object.object.kinematics.initial_twist_with_covariance.twist.linear.x;
     const double braking_distance =
       v * v /
       (2.0 * std::fabs(planner_param_.collision_detection.ignore_on_amber_traffic_light
                          .object_expected_deceleration));
     if (dist_to_stop_line > braking_distance) {
       return false;
     }
     const auto stopline_front = target_object.stop_line.value().front();
     const auto stopline_back = target_object.stop_line.value().back();
     tier4_autoware_utils::LineString2d object_line;
     object_line.emplace_back(
       (stopline_front.x() + stopline_back.x()) / 2.0,
       (stopline_front.y() + stopline_back.y()) / 2.0);
     const auto stopline_mid = object_line.front();
@@ -1551,14 +1543,14 @@
     }
     const auto collision_point = intersections.front();
     // distance from object expected stop position to collision point
     const double stopline_to_object = -1.0 * dist_to_stop_line + braking_distance;
     const double stopline_to_collision =
       std::hypot(collision_point.x() - stopline_mid.x(), collision_point.y() - stopline_mid.y());
     const double object2collision = stopline_to_collision - stopline_to_object;
     const double margin =
       planner_param_.collision_detection.ignore_on_red_traffic_light.object_margin_to_path;
     return (object2collision > margin) || (object2collision < 0);
   };
   // check collision between predicted_path and ego_area
   bool collision_detected = false;
   for (const auto & target_object : target_objects->all_attention_objects) {
@@ -1572,11 +1564,11 @@
       continue;
     }
     const bool is_tolerable_overshoot = isTolerableOvershoot(target_object);
     if (
       traffic_prioritized_level == util::TrafficPrioritizedLevel::FULLY_PRIORITIZED &&
       !expected_to_stop_before_stopline && is_tolerable_overshoot) {
       debug_data_.red_overshoot_ignore_targets.objects.push_back(object);
       continue;
     }
     for (const auto & predicted_path : object.kinematics.predicted_paths) {
       if (