refactor stuck/yield_stuck for right turn

Signed-off-by: Mamoru Sobue <mamoru.sobue@tier4.jp>

planning/behavior_velocity_intersection_module/README.md

@@ -186,7 +186,7 @@ If there is any object on the path inside the intersection and at the exit of th
 
 ## Yield stuck vehicle detection
 
-If there is any stopped object on the attention lanelet between the intersection point with ego path and the position which is `yield_stuck.distance_threshold` before that position, the object is regarded as yielding to ego vehicle. In this case ego is given the right-of-way by the yielding object but this module inserts stopline to prevent entry into the intersection.
+If there is any stopped object on the attention lanelet between the intersection point with ego path and the position which is `yield_stuck.distance_threshold` before that position, the object is regarded as yielding to ego vehicle. In this case ego is given the right-of-way by the yielding object but this module inserts stopline to prevent entry into the intersection. This scene happens when the object is yieling against ego or the object is waiting before the crosswalk around the exit of the intersection.
 
 ![yield_stuck_detection](./docs/yield-stuck.drawio.svg)
 

planning/behavior_velocity_intersection_module/src/scene_intersection.cpp

@@ -1056,17 +1056,26 @@ IntersectionModule::DecisionResult IntersectionModule::modifyPathVelocityDetail(
   // stuck vehicle detection is viable even if attention area is empty
   // so this needs to be checked before attention area validation
   const bool stuck_detected = checkStuckVehicle(planner_data_, path_lanelets);
-  if (stuck_detected && stuck_stopline_idx_opt) {
-    auto stuck_stopline_idx = stuck_stopline_idx_opt.value();
+  if (stuck_detected) {
     if (is_private_area_ && planner_param_.stuck_vehicle.enable_private_area_stuck_disregard) {
-      if (
-        default_stopline_idx_opt &&
-        fromEgoDist(stuck_stopline_idx) < -planner_param_.common.stopline_overshoot_margin) {
-        stuck_stopline_idx = default_stopline_idx_opt.value();
-      }
     } else {
-      return IntersectionModule::StuckStop{
-        closest_idx, stuck_stopline_idx, occlusion_peeking_stopline_idx_opt};
+      std::optional<size_t> stopline_idx = std::nullopt;
+      if (stuck_stopline_idx_opt) {
+        stopline_idx = stuck_stopline_idx_opt.value();
+      } else if (
+        default_stopline_idx_opt && fromEgoDist(default_stopline_idx_opt.value()) >
+                                      -planner_param_.common.stopline_overshoot_margin) {
+        stopline_idx = default_stopline_idx_opt.value();
+      } else if (
+        first_attention_stopline_idx_opt &&
+        fromEgoDist(first_attention_stopline_idx_opt.value()) >= 0.0) {
+        stopline_idx = closest_idx;
+      }
+
+      if (stopline_idx) {
+        return IntersectionModule::StuckStop{
+          closest_idx, stopline_idx.value(), occlusion_peeking_stopline_idx_opt};
+      }
     }
   }
 
@@ -1115,13 +1124,22 @@ IntersectionModule::DecisionResult IntersectionModule::modifyPathVelocityDetail(
   // filter objects
   auto target_objects = generateTargetObjects(intersection_lanelets, intersection_area);
 
-  // yield stuck vehicle detection is viable even if attention area is empty
-  // so this needs to be checked before attention area validation
   const bool yield_stuck_detected = checkYieldStuckVehicle(
     target_objects, interpolated_path_info, intersection_lanelets.attention_non_preceding_);
   if (yield_stuck_detected && stuck_stopline_idx_opt) {
-    const auto stuck_stopline_idx = stuck_stopline_idx_opt.value();
-    return IntersectionModule::YieldStuckStop{closest_idx, stuck_stopline_idx};
+    std::optional<size_t> stopline_idx = std::nullopt;
+    if (stuck_stopline_idx_opt) {
+      stopline_idx = stuck_stopline_idx_opt.value();
+    } else if (
+      fromEgoDist(default_stopline_idx) > -planner_param_.common.stopline_overshoot_margin) {
+      stopline_idx = default_stopline_idx;
+    } else if (fromEgoDist(first_attention_stopline_idx) >= 0.0) {
+      stopline_idx = closest_idx;
+    }
+
+    if (stopline_idx) {
+      return IntersectionModule::YieldStuckStop{closest_idx, stopline_idx.value()};
+    }
   }
 
   const double is_amber_or_red =