feat(avoidance): wait and see ambiguous stopped vehicle (#6631)

* feat(avoidance): wait and see the ambiguous stopped vehicle

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

* fix(avoidance): wait and see objects around ego straight lane

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

* tmp

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

* refactor(avoidance): filtering logic for vehicle type objects

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

* fix(avoidance): wait with unsafe avoidance path

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

* fix(avoidance): use getRightLanelet

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

* refactor(avoidance): parameterize

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

---------

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

Author: satoshi-ota

planning/behavior_path_avoidance_by_lane_change_module/src/manager.cpp

@@ -133,10 +133,13 @@ void AvoidanceByLaneChangeModuleManager::init(rclcpp::Node * node)
 
   {
     const std::string ns = "avoidance.target_filtering.avoidance_for_ambiguous_vehicle.";
-    p.enable_force_avoidance_for_stopped_vehicle =
-      getOrDeclareParameter<bool>(*node, ns + "enable");
-    p.threshold_time_force_avoidance_for_stopped_vehicle =
-      getOrDeclareParameter<double>(*node, ns + "time_threshold");
+    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.time_threshold_for_ambiguous_vehicle =
+      getOrDeclareParameter<double>(*node, ns + "condition.time_threshold");
+    p.distance_threshold_for_ambiguous_vehicle =
+      getOrDeclareParameter<double>(*node, ns + "condition.distance_threshold");
     p.object_ignore_section_traffic_light_in_front_distance =
       getOrDeclareParameter<double>(*node, ns + "ignore_area.traffic_light.front_distance");
     p.object_ignore_section_crosswalk_in_front_distance =

planning/behavior_path_avoidance_module/config/avoidance.param.yaml

@@ -155,8 +155,10 @@
         # params for avoidance of vehicle type objects that are ambiguous as to whether they are parked.
         avoidance_for_ambiguous_vehicle:
           enable: false                                  # [-]
-          time_threshold: 1.0                            # [s]
-          distance_threshold: 1.0                        # [m]
+          closest_distance_to_wait_and_see: 10.0         # [m]
+          condition:
+            time_threshold: 1.0                          # [s]
+            distance_threshold: 1.0                      # [m]
           ignore_area:
             traffic_light:
               front_distance: 100.0                      # [m]

planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/data_structs.hpp

@@ -100,7 +100,7 @@ struct AvoidanceParameters
   bool enable_cancel_maneuver{false};
 
   // enable avoidance for all parking vehicle
-  bool enable_force_avoidance_for_stopped_vehicle{false};
+  bool enable_avoidance_for_ambiguous_vehicle{false};
 
   // enable yield maneuver.
   bool enable_yield_maneuver{false};
@@ -184,8 +184,9 @@ struct AvoidanceParameters
   double object_check_min_road_shoulder_width{0.0};
 
   // force avoidance
-  double threshold_time_force_avoidance_for_stopped_vehicle{0.0};
-  double force_avoidance_distance_threshold{0.0};
+  double closest_distance_to_wait_and_see_for_ambiguous_vehicle{0.0};
+  double time_threshold_for_ambiguous_vehicle{0.0};
+  double distance_threshold_for_ambiguous_vehicle{0.0};
 
   // when complete avoidance motion, there is a distance margin with the object
   // for longitudinal direction

planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/helper.hpp

@@ -289,6 +289,35 @@ 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());
+
+    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 +
+             parameters_->closest_distance_to_wait_and_see_for_ambiguous_vehicle;
+  }
+
   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/include/behavior_path_avoidance_module/parameter_helper.hpp

@@ -141,12 +141,13 @@ AvoidanceParameters getParameter(rclcpp::Node * node)
 
   {
     const std::string ns = "avoidance.target_filtering.avoidance_for_ambiguous_vehicle.";
-    p.enable_force_avoidance_for_stopped_vehicle =
-      getOrDeclareParameter<bool>(*node, ns + "enable");
-    p.threshold_time_force_avoidance_for_stopped_vehicle =
-      getOrDeclareParameter<double>(*node, ns + "time_threshold");
-    p.force_avoidance_distance_threshold =
-      getOrDeclareParameter<double>(*node, ns + "distance_threshold");
+    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.time_threshold_for_ambiguous_vehicle =
+      getOrDeclareParameter<double>(*node, ns + "condition.time_threshold");
+    p.distance_threshold_for_ambiguous_vehicle =
+      getOrDeclareParameter<double>(*node, ns + "condition.distance_threshold");
     p.object_ignore_section_traffic_light_in_front_distance =
       getOrDeclareParameter<double>(*node, ns + "ignore_area.traffic_light.front_distance");
     p.object_ignore_section_crosswalk_in_front_distance =

planning/behavior_path_avoidance_module/src/debug.cpp

@@ -579,6 +579,8 @@ MarkerArray createDebugMarkerArray(
     addObjects(data.other_objects, std::string("ParallelToEgoLane"));
     addObjects(data.other_objects, std::string("MergingToEgoLane"));
     addObjects(data.other_objects, std::string("UnstableObject"));
+    addObjects(data.other_objects, std::string("AmbiguousStoppedVehicle"));
+    addObjects(data.other_objects, std::string("AmbiguousStoppedVehicle(wait-and-see)"));
   }
 
   // shift line pre-process

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/utils.cpp

@@ -399,7 +399,8 @@ bool isMergingToEgoLane(const ObjectData & object)
 }
 
 bool isParkedVehicle(
-  ObjectData & object, const std::shared_ptr<RouteHandler> & route_handler,
+  ObjectData & object, const AvoidancePlanningData & data,
+  const std::shared_ptr<RouteHandler> & route_handler,
   const std::shared_ptr<AvoidanceParameters> & parameters)
 {
   using lanelet::geometry::distance2d;
@@ -496,57 +497,36 @@ bool isParkedVehicle(
       object.shiftable_ratio > parameters->object_check_shiftable_ratio;
   }
 
-  return is_left_side_parked_vehicle || is_right_side_parked_vehicle;
-}
-
-bool isAmbiguousStoppedVehicle(
-  ObjectData & object, const AvoidancePlanningData & data,
-  const std::shared_ptr<const PlannerData> & planner_data,
-  const std::shared_ptr<AvoidanceParameters> & parameters)
-{
-  const auto stop_time_longer_than_threshold =
-    object.stop_time > parameters->threshold_time_force_avoidance_for_stopped_vehicle;
-
-  if (!stop_time_longer_than_threshold) {
+  if (!is_left_side_parked_vehicle && !is_right_side_parked_vehicle) {
     return false;
   }
 
   const auto & object_pose = object.object.kinematics.initial_pose_with_covariance.pose;
-  const auto is_moving_distance_longer_than_threshold =
-    tier4_autoware_utils::calcDistance2d(object.init_pose, object_pose) >
-    parameters->force_avoidance_distance_threshold;
-
-  if (is_moving_distance_longer_than_threshold) {
-    return false;
-  }
-
-  if (object.is_within_intersection) {
-    RCLCPP_DEBUG(rclcpp::get_logger(__func__), "object is in the intersection area.");
-    return false;
-  }
-
-  const auto rh = planner_data->route_handler;
-
-  if (
-    !!rh->getRoutingGraphPtr()->right(object.overhang_lanelet) &&
-    !!rh->getRoutingGraphPtr()->left(object.overhang_lanelet)) {
-    RCLCPP_DEBUG(rclcpp::get_logger(__func__), "object isn't on the edge lane.");
+  object.to_centerline =
+    lanelet::utils::getArcCoordinates(data.current_lanelets, object_pose).distance;
+  if (std::abs(object.to_centerline) < parameters->threshold_distance_object_is_on_center) {
     return false;
   }
 
-  if (!object.is_on_ego_lane) {
-    return true;
-  }
+  return true;
+}
 
+bool isCloseToStopFactor(
+  ObjectData & object, const AvoidancePlanningData & data,
+  const std::shared_ptr<const PlannerData> & planner_data,
+  const std::shared_ptr<AvoidanceParameters> & parameters)
+{
+  const auto & rh = planner_data->route_handler;
   const auto & ego_pose = planner_data->self_odometry->pose.pose;
+  const auto & object_pose = object.object.kinematics.initial_pose_with_covariance.pose;
 
   // force avoidance for stopped vehicle
-  bool not_parked_object = true;
+  bool is_close_to_stop_factor = false;
 
   // check traffic light
   const auto to_traffic_light = getDistanceToNextTrafficLight(object_pose, data.extend_lanelets);
   {
-    not_parked_object =
+    is_close_to_stop_factor =
       to_traffic_light < parameters->object_ignore_section_traffic_light_in_front_distance;
   }
 
@@ -558,12 +538,89 @@ bool isAmbiguousStoppedVehicle(
     const auto stop_for_crosswalk =
       to_crosswalk < parameters->object_ignore_section_crosswalk_in_front_distance &&
       to_crosswalk > -1.0 * parameters->object_ignore_section_crosswalk_behind_distance;
-    not_parked_object = not_parked_object || stop_for_crosswalk;
+    is_close_to_stop_factor = is_close_to_stop_factor || stop_for_crosswalk;
   }
 
   object.to_stop_factor_distance = std::min(to_traffic_light, to_crosswalk);
 
-  return !not_parked_object;
+  return is_close_to_stop_factor;
+}
+
+bool isNeverAvoidanceTarget(
+  ObjectData & object, const AvoidancePlanningData & data,
+  const std::shared_ptr<const PlannerData> & planner_data,
+  const std::shared_ptr<AvoidanceParameters> & parameters)
+{
+  const auto & object_pose = object.object.kinematics.initial_pose_with_covariance.pose;
+  const auto is_moving_distance_longer_than_threshold =
+    tier4_autoware_utils::calcDistance2d(object.init_pose, object_pose) >
+    parameters->distance_threshold_for_ambiguous_vehicle;
+  if (is_moving_distance_longer_than_threshold) {
+    object.reason = AvoidanceDebugFactor::MOVING_OBJECT;
+    return true;
+  }
+
+  if (object.is_within_intersection) {
+    if (object.behavior == ObjectData::Behavior::NONE) {
+      object.reason = "ParallelToEgoLane";
+      RCLCPP_DEBUG(rclcpp::get_logger(__func__), "object belongs to ego lane. never avoid it.");
+      return true;
+    }
+
+    if (object.behavior == ObjectData::Behavior::MERGING) {
+      object.reason = "MergingToEgoLane";
+      RCLCPP_DEBUG(rclcpp::get_logger(__func__), "object belongs to ego lane. never avoid it.");
+      return true;
+    }
+  }
+
+  if (object.is_on_ego_lane) {
+    if (
+      planner_data->route_handler->getRightLanelet(object.overhang_lanelet).has_value() &&
+      planner_data->route_handler->getLeftLanelet(object.overhang_lanelet).has_value()) {
+      object.reason = AvoidanceDebugFactor::NOT_PARKING_OBJECT;
+      RCLCPP_DEBUG(rclcpp::get_logger(__func__), "object isn't on the edge lane. never avoid it.");
+      return true;
+    }
+  }
+
+  if (isCloseToStopFactor(object, data, planner_data, parameters)) {
+    if (object.is_on_ego_lane && !object.is_parked) {
+      object.reason = AvoidanceDebugFactor::NOT_PARKING_OBJECT;
+      RCLCPP_DEBUG(rclcpp::get_logger(__func__), "object is close to stop factor. never avoid it.");
+      return true;
+    }
+  }
+
+  return false;
+}
+
+bool isObviousAvoidanceTarget(
+  ObjectData & object, [[maybe_unused]] const AvoidancePlanningData & data,
+  [[maybe_unused]] const std::shared_ptr<const PlannerData> & planner_data,
+  [[maybe_unused]] const std::shared_ptr<AvoidanceParameters> & parameters)
+{
+  if (!object.is_within_intersection) {
+    if (object.is_parked && object.behavior == ObjectData::Behavior::NONE) {
+      RCLCPP_DEBUG(rclcpp::get_logger(__func__), "object is obvious parked vehicle.");
+      return true;
+    }
+
+    if (!object.is_on_ego_lane && object.behavior == ObjectData::Behavior::NONE) {
+      RCLCPP_DEBUG(rclcpp::get_logger(__func__), "object is adjacent vehicle.");
+      return true;
+    }
+  }
+
+  if (!object.is_parked) {
+    object.reason = AvoidanceDebugFactor::NOT_PARKING_OBJECT;
+  }
+
+  if (object.behavior == ObjectData::Behavior::MERGING) {
+    object.reason = "MergingToEgoLane";
+  }
+
+  return false;
 }
 
 bool isSatisfiedWithCommonCondition(
@@ -666,50 +723,56 @@ bool isSatisfiedWithVehicleCondition(
 {
   object.behavior = getObjectBehavior(object, parameters);
   object.is_on_ego_lane = isOnEgoLane(object);
-  object.is_ambiguous = isAmbiguousStoppedVehicle(object, data, planner_data, parameters);
 
-  // from here condition check for vehicle type objects.
-  if (object.is_ambiguous && parameters->enable_force_avoidance_for_stopped_vehicle) {
+  if (isNeverAvoidanceTarget(object, data, planner_data, parameters)) {
+    return false;
+  }
+
+  if (isObviousAvoidanceTarget(object, data, planner_data, parameters)) {
     return true;
   }
 
-  // check vehicle shift ratio
-  if (object.is_on_ego_lane) {
-    if (object.is_parked) {
-      return true;
-    } else {
-      object.reason = AvoidanceDebugFactor::NOT_PARKING_OBJECT;
-      return false;
-    }
+  // from here, filtering for ambiguous vehicle.
+
+  if (!parameters->enable_avoidance_for_ambiguous_vehicle) {
+    object.reason = "AmbiguousStoppedVehicle";
+    return false;
   }
 
-  // Object is on center line -> ignore.
-  const auto & object_pose = object.object.kinematics.initial_pose_with_covariance.pose;
-  object.to_centerline =
-    lanelet::utils::getArcCoordinates(data.current_lanelets, object_pose).distance;
-  if (std::abs(object.to_centerline) < parameters->threshold_distance_object_is_on_center) {
-    object.reason = AvoidanceDebugFactor::TOO_NEAR_TO_CENTERLINE;
+  const auto stop_time_longer_than_threshold =
+    object.stop_time > parameters->time_threshold_for_ambiguous_vehicle;
+  if (!stop_time_longer_than_threshold) {
+    object.reason = "AmbiguousStoppedVehicle(wait-and-see)";
     return false;
   }
 
   if (object.is_within_intersection) {
-    std::string turn_direction = object.overhang_lanelet.attributeOr("turn_direction", "else");
-    if (turn_direction == "straight") {
+    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::NONE) {
-      object.reason = "ParallelToEgoLane";
-      return false;
+    if (object.behavior == ObjectData::Behavior::DEVIATING) {
+      object.reason = "AmbiguousStoppedVehicle(wait-and-see)";
+      object.is_ambiguous = true;
+      return true;
     }
-  }
 
-  if (object.behavior == ObjectData::Behavior::MERGING) {
-    object.reason = "MergingToEgoLane";
-    return false;
+    if (object.behavior == ObjectData::Behavior::NONE) {
+      object.is_ambiguous = false;
+      return true;
+    }
   }
 
-  return true;
+  object.reason = AvoidanceDebugFactor::NOT_PARKING_OBJECT;
+  return false;
 }
 
 bool isNoNeedAvoidanceBehavior(
@@ -1636,7 +1699,8 @@ void filterTargetObjects(
     if (filtering_utils::isVehicleTypeObject(o)) {
       o.is_within_intersection =
         filtering_utils::isWithinIntersection(o, planner_data->route_handler);
-      o.is_parked = filtering_utils::isParkedVehicle(o, planner_data->route_handler, parameters);
+      o.is_parked =
+        filtering_utils::isParkedVehicle(o, data, planner_data->route_handler, parameters);
       o.avoid_margin = filtering_utils::getAvoidMargin(o, planner_data, parameters);
 
       if (filtering_utils::isNoNeedAvoidanceBehavior(o, parameters)) {