fix(avoidance): wait with unsafe avoidance path

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

Author: satoshi-ota

planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/helper.hpp

@@ -289,6 +289,34 @@ class AvoidanceHelper
     });
   }
 
+  bool isReady(const ObjectDataArray & objects) const
+  {
+    if (objects.empty()) {
+      return true;
+    }
+
+    const auto object = objects.front();
+
+    if (!object.is_ambiguous) {
+      return true;
+    }
+
+    if (!object.avoid_margin.has_value()) {
+      return true;
+    }
+
+    const auto is_object_on_right = utils::avoidance::isOnRight(object);
+    const auto desire_shift_length =
+      getShiftLength(object, is_object_on_right, object.avoid_margin.value());
+
+    constexpr double BUFFER = 10.0;
+    const auto prepare_distance = getMinimumPrepareDistance();
+    const auto constant_distance = getFrontConstantDistance(object);
+    const auto avoidance_distance = getMinAvoidanceDistance(desire_shift_length);
+
+    return object.longitudinal < prepare_distance + constant_distance + avoidance_distance + BUFFER;
+  }
+
   bool isReady(const AvoidLineArray & new_shift_lines, const double current_shift_length) const
   {
     if (std::abs(current_shift_length) < 1e-3) {

planning/behavior_path_avoidance_module/src/scene.cpp

@@ -522,7 +522,8 @@ void AvoidanceModule::fillShiftLine(AvoidancePlanningData & data, DebugData & de
    */
   data.comfortable = helper_->isComfortable(data.new_shift_line);
   data.safe = isSafePath(data.candidate_path, debug);
-  data.ready = helper_->isReady(data.new_shift_line, path_shifter_.getLastShiftLength());
+  data.ready = helper_->isReady(data.new_shift_line, path_shifter_.getLastShiftLength()) &&
+               helper_->isReady(data.target_objects);
 }
 
 void AvoidanceModule::fillEgoStatus(

planning/behavior_path_avoidance_module/src/shift_line_generator.cpp

@@ -276,22 +276,6 @@ AvoidOutlines ShiftLineGenerator::generateAvoidOutline(
 
     // calculate feasible shift length based on behavior policy
     const auto feasible_shift_profile = get_shift_profile(o, desire_shift_length);
-
-    if (o.is_ambiguous) {
-      const auto prepare_distance = helper_->getMinimumPrepareDistance();
-      const auto constant_distance = helper_->getFrontConstantDistance(o);
-      const auto avoidance_distance = helper_->getMinAvoidanceDistance(desire_shift_length);
-      const auto decelaration_distance =
-        std::max(10.0, helper_->getFeasibleDecelDistance(0.0, false));
-      if (
-        o.longitudinal >
-        prepare_distance + constant_distance + avoidance_distance + decelaration_distance) {
-        o.reason = "AmbiguousStoppedVehicle(wait-and-see)";
-        o.is_avoidable = feasible_shift_profile.has_value();
-        continue;
-      }
-    }
-
     if (!feasible_shift_profile.has_value()) {
       if (o.avoid_required && is_forward_object(o)) {
         break;

planning/behavior_path_avoidance_module/src/utils.cpp

@@ -874,16 +874,19 @@ bool isSatisfiedWithVehicleCondition(
 
   if (object.is_within_intersection) {
     if (object.behavior == ObjectData::Behavior::DEVIATING) {
+      object.reason = "AmbiguousStoppedVehicle(wait-and-see)";
       object.is_ambiguous = true;
       return true;
     }
   } else {
     if (object.behavior == ObjectData::Behavior::MERGING) {
+      object.reason = "AmbiguousStoppedVehicle(wait-and-see)";
       object.is_ambiguous = true;
       return true;
     }
 
     if (object.behavior == ObjectData::Behavior::DEVIATING) {
+      object.reason = "AmbiguousStoppedVehicle(wait-and-see)";
       object.is_ambiguous = true;
       return true;
     }