fix(static_obstacle_avoidance): don't automatically avoid ambiguous v…

…ehicle (#7851)

* fix(static_obstacle_avoidance): don't automatically avoid ambiguous vehicle

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

* chore(schema): update schema

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

---------

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

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>
 
@@ -325,10 +326,27 @@ class AvoidanceHelper
 
     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;
+    }
+
     if (!object.avoid_margin.has_value()) {
       return true;
     }
@@ -341,9 +359,32 @@ class AvoidanceHelper
     const auto constant_distance = getFrontConstantDistance(object);
     const auto avoidance_distance = getMinAvoidanceDistance(desire_shift_length);
 
-    return object.longitudinal <
-           prepare_distance + constant_distance + avoidance_distance +
-             parameters_->closest_distance_to_wait_and_see_for_ambiguous_vehicle;
+    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/schema/static_obstacle_avoidance.schema.json

@@ -716,9 +716,10 @@
             "avoidance_for_ambiguous_vehicle": {
               "type": "object",
               "properties": {
-                "enable": {
-                  "type": "boolean",
-                  "description": "Enable avoidance maneuver for ambiguous vehicles.",
+                "policy": {
+                  "type": "string",
+                  "enum": ["auto", "manual", "ignore"],
+                  "description": "Ego behavior policy for ambiguous vehicle.",
                   "default": "true"
                 },
                 "closest_distance_to_wait_and_see": {
@@ -778,14 +779,26 @@
                   },
                   "required": ["traffic_light", "crosswalk"],
                   "additionalProperties": false
+                },
+                "wait_and_see": {
+                  "type": "object",
+                  "properties": {
+                    "target_behaviors": {
+                      "type": "array",
+                      "default": ["MERGING", "DEVIATING"],
+                      "description": "This module doesn't avoid these behaviors vehicle until it gets closer than threshold."
+                    },
+                    "th_closest_distance": {
+                      "type": "number",
+                      "default": 10.0,
+                      "description": "Threshold to check whether the ego gets close enough the ambiguous vehicle."
+                    }
+                  },
+                  "required": ["target_behaviors", "th_closest_distance"],
+                  "additionalProperties": false
                 }
               },
-              "required": [
-                "enable",
-                "closest_distance_to_wait_and_see",
-                "condition",
-                "ignore_area"
-              ],
+              "required": ["policy", "condition", "ignore_area", "wait_and_see"],
               "additionalProperties": false
             },
             "intersection": {

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 = false;
-      return true;
-    }
+    object.info = ObjectInfo::AMBIGUOUS_STOPPED_VEHICLE;
+    object.is_ambiguous = true;
+    return true;
   }
 
   object.info = ObjectInfo::IS_NOT_PARKING_OBJECT;