fixup! fix(static_obstacle_avoidance): don't automatically avoid ambi…

…guous vehicle

planning/behavior_path_planner/autoware_behavior_path_avoidance_by_lane_change_module/src/manager.cpp

@@ -137,9 +137,11 @@ void AvoidanceByLaneChangeModuleManager::init(rclcpp::Node * node)
 
   {
     const std::string ns = "avoidance.target_filtering.avoidance_for_ambiguous_vehicle.";
-    p.enable_avoidance_for_ambiguous_vehicle = getOrDeclareParameter<bool>(*node, ns + "enable");
-    p.closest_distance_to_wait_and_see_for_ambiguous_vehicle =
-      getOrDeclareParameter<double>(*node, ns + "closest_distance_to_wait_and_see");
+    p.policy_ambiguous_vehicle = getOrDeclareParameter<std::string>(*node, ns + "policy");
+    p.wait_and_see_target_behaviors =
+      getOrDeclareParameter<std::vector<std::string>>(*node, ns + "wait_and_see.target_behaviors");
+    p.wait_and_see_th_closest_distance =
+      getOrDeclareParameter<double>(*node, ns + "wait_and_see.th_closest_distance");
     p.time_threshold_for_ambiguous_vehicle =
       getOrDeclareParameter<double>(*node, ns + "condition.th_stopped_time");
     p.distance_threshold_for_ambiguous_vehicle =

planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/config/static_obstacle_avoidance.param.yaml

@@ -138,8 +138,11 @@
 
         # params for avoidance of vehicle type objects that are ambiguous as to whether they are parked.
         avoidance_for_ambiguous_vehicle:
-          enable: true                                  # [-]
-          closest_distance_to_wait_and_see: 10.0        # [m]
+          # policy for ego behavior for ambiguous vehicle.
+          # "auto"   : generate candidate path. if RTC is running as AUTO mode, the ego avoids it automatically.
+          # "manual" : generate candidate path and wait operator approval even if RTC is running as AUTO mode.
+          # "ignore" : never avoid it.
+          policy: "auto"                                # [-]
           condition:
             th_stopped_time: 3.0                        # [s]
             th_moving_distance: 1.0                     # [m]
@@ -149,6 +152,9 @@
             crosswalk:
               front_distance: 30.0                      # [m]
               behind_distance: 30.0                     # [m]
+          wait_and_see:
+            target_behaviors: ["MERGING", "DEVIATING"]  # [-]
+            th_closest_distance: 10.0                   # [m]
 
         # params for filtering objects that are in intersection
         intersection:

planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware/behavior_path_static_obstacle_avoidance_module/data_structs.hpp

@@ -108,7 +108,7 @@ struct AvoidanceParameters
   bool enable_cancel_maneuver{false};
 
   // enable avoidance for all parking vehicle
-  bool enable_avoidance_for_ambiguous_vehicle{false};
+  std::string policy_ambiguous_vehicle{"ignore"};
 
   // enable yield maneuver.
   bool enable_yield_maneuver{false};
@@ -192,7 +192,8 @@ struct AvoidanceParameters
   double object_check_min_road_shoulder_width{0.0};
 
   // force avoidance
-  double closest_distance_to_wait_and_see_for_ambiguous_vehicle{0.0};
+  std::vector<std::string> wait_and_see_target_behaviors{"NONE", "MERGING", "DEVIATING"};
+  double wait_and_see_th_closest_distance{0.0};
   double time_threshold_for_ambiguous_vehicle{0.0};
   double distance_threshold_for_ambiguous_vehicle{0.0};
 

planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware/behavior_path_static_obstacle_avoidance_module/helper.hpp

@@ -23,6 +23,7 @@
 #include <algorithm>
 #include <limits>
 #include <memory>
+#include <string>
 #include <utility>
 #include <vector>
 
@@ -323,11 +324,67 @@ class AvoidanceHelper
       return true;
     }
 
-    if (objects.front().longitudinal < 0.0) {
+    const auto object = objects.front();
+
+    // if the object is NOT ambiguous, this module doesn't wait operator approval if RTC is running
+    // as AUTO mode.
+    if (!object.is_ambiguous) {
+      return true;
+    }
+
+    // check only front objects.
+    if (object.longitudinal < 0.0) {
+      return true;
+    }
+
+    // if the policy is "manual", this module generates candidate path and waits approval.
+    if (parameters_->policy_ambiguous_vehicle == "manual") {
+      return false;
+    }
+
+    // don't delay avoidance start position if it's not MERGING or DEVIATING vehicle.
+    if (!isWaitAndSeeTarget(object)) {
       return true;
     }
 
-    return !objects.front().is_ambiguous;
+    if (!object.avoid_margin.has_value()) {
+      return true;
+    }
+
+    const auto is_object_on_right = utils::static_obstacle_avoidance::isOnRight(object);
+    const auto desire_shift_length =
+      getShiftLength(object, is_object_on_right, object.avoid_margin.value());
+
+    const auto prepare_distance = getNominalPrepareDistance(0.0);
+    const auto constant_distance = getFrontConstantDistance(object);
+    const auto avoidance_distance = getMinAvoidanceDistance(desire_shift_length);
+
+    return object.longitudinal < prepare_distance + constant_distance + avoidance_distance +
+                                   parameters_->wait_and_see_th_closest_distance;
+  }
+
+  bool isWaitAndSeeTarget(const ObjectData & object) const
+  {
+    const auto & behaviors = parameters_->wait_and_see_target_behaviors;
+    if (object.behavior == ObjectData::Behavior::MERGING) {
+      return std::any_of(behaviors.begin(), behaviors.end(), [](const std::string & behavior) {
+        return behavior == "MERGING";
+      });
+    }
+
+    if (object.behavior == ObjectData::Behavior::DEVIATING) {
+      return std::any_of(behaviors.begin(), behaviors.end(), [](const std::string & behavior) {
+        return behavior == "DEVIATING";
+      });
+    }
+
+    if (object.behavior == ObjectData::Behavior::NONE) {
+      return std::any_of(behaviors.begin(), behaviors.end(), [](const std::string & behavior) {
+        return behavior == "NONE";
+      });
+    }
+
+    return false;
   }
 
   static bool isAbsolutelyNotAvoidable(const ObjectData & object)

planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware/behavior_path_static_obstacle_avoidance_module/parameter_helper.hpp

@@ -137,9 +137,11 @@ AvoidanceParameters getParameter(rclcpp::Node * node)
 
   {
     const std::string ns = "avoidance.target_filtering.avoidance_for_ambiguous_vehicle.";
-    p.enable_avoidance_for_ambiguous_vehicle = getOrDeclareParameter<bool>(*node, ns + "enable");
-    p.closest_distance_to_wait_and_see_for_ambiguous_vehicle =
-      getOrDeclareParameter<double>(*node, ns + "closest_distance_to_wait_and_see");
+    p.policy_ambiguous_vehicle = getOrDeclareParameter<std::string>(*node, ns + "policy");
+    p.wait_and_see_target_behaviors =
+      getOrDeclareParameter<std::vector<std::string>>(*node, ns + "wait_and_see.target_behaviors");
+    p.wait_and_see_th_closest_distance =
+      getOrDeclareParameter<double>(*node, ns + "wait_and_see.th_closest_distance");
     p.time_threshold_for_ambiguous_vehicle =
       getOrDeclareParameter<double>(*node, ns + "condition.th_stopped_time");
     p.distance_threshold_for_ambiguous_vehicle =

planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/manager.cpp

@@ -132,10 +132,9 @@ void StaticObstacleAvoidanceModuleManager::updateModuleParams(
 
   {
     const std::string ns = "avoidance.avoidance.lateral.avoidance_for_ambiguous_vehicle.";
-    updateParam<bool>(parameters, ns + "enable", p->enable_avoidance_for_ambiguous_vehicle);
+    updateParam<std::string>(parameters, ns + "policy", p->policy_ambiguous_vehicle);
     updateParam<double>(
-      parameters, ns + "closest_distance_to_wait_and_see",
-      p->closest_distance_to_wait_and_see_for_ambiguous_vehicle);
+      parameters, ns + "wait_and_see.th_closest_distance", p->wait_and_see_th_closest_distance);
     updateParam<double>(
       parameters, ns + "condition.th_stopped_time", p->time_threshold_for_ambiguous_vehicle);
     updateParam<double>(

planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/utils.cpp

@@ -880,7 +880,7 @@ bool isSatisfiedWithVehicleCondition(
 
   // from here, filtering for ambiguous vehicle.
 
-  if (!parameters->enable_avoidance_for_ambiguous_vehicle) {
+  if (parameters->policy_ambiguous_vehicle == "ignore") {
     object.info = ObjectInfo::AMBIGUOUS_STOPPED_VEHICLE;
     return false;
   }
@@ -889,6 +889,7 @@ bool isSatisfiedWithVehicleCondition(
     object.stop_time > parameters->time_threshold_for_ambiguous_vehicle;
   if (!stop_time_longer_than_threshold) {
     object.info = ObjectInfo::AMBIGUOUS_STOPPED_VEHICLE;
+    object.is_ambiguous = false;
     return false;
   }
 
@@ -897,6 +898,7 @@ bool isSatisfiedWithVehicleCondition(
     parameters->distance_threshold_for_ambiguous_vehicle;
   if (is_moving_distance_longer_than_threshold) {
     object.info = ObjectInfo::AMBIGUOUS_STOPPED_VEHICLE;
+    object.is_ambiguous = false;
     return false;
   }
 
@@ -907,22 +909,9 @@ bool isSatisfiedWithVehicleCondition(
       return true;
     }
   } else {
-    if (object.behavior == ObjectData::Behavior::MERGING) {
-      object.info = ObjectInfo::AMBIGUOUS_STOPPED_VEHICLE;
-      object.is_ambiguous = true;
-      return true;
-    }
-
-    if (object.behavior == ObjectData::Behavior::DEVIATING) {
-      object.info = ObjectInfo::AMBIGUOUS_STOPPED_VEHICLE;
-      object.is_ambiguous = true;
-      return true;
-    }
-
-    if (object.behavior == ObjectData::Behavior::NONE) {
-      object.is_ambiguous = true;
-      return true;
-    }
+    object.info = ObjectInfo::AMBIGUOUS_STOPPED_VEHICLE;
+    object.is_ambiguous = true;
+    return true;
   }
 
   object.info = ObjectInfo::IS_NOT_PARKING_OBJECT;