feat(avoidance): improve force avoidance judge logic in order to suppress unnecessary avoidance path

planning/behavior_path_planner/config/avoidance/avoidance.param.yaml

@@ -9,7 +9,6 @@
 
       # avoidance module common setting
       enable_bound_clipping: false
-      enable_force_avoidance_for_stopped_vehicle: false
       enable_yield_maneuver: true
       enable_yield_maneuver_during_shifting: false
       enable_cancel_maneuver: false
@@ -120,11 +119,6 @@
 
       # For target object filtering
       target_filtering:
-        # params for avoidance of not-parked objects
-        threshold_time_force_avoidance_for_stopped_vehicle: 10.0    # [s]
-        object_ignore_section_traffic_light_in_front_distance: 30.0 # [m]
-        object_ignore_section_crosswalk_in_front_distance: 30.0     # [m]
-        object_ignore_section_crosswalk_behind_distance: 30.0       # [m]
         # detection range
         object_check_goal_distance: 20.0                # [m]
         # filtering parking objects
@@ -141,6 +135,17 @@
           max_forward_distance: 150.0                    # [m]
           backward_distance: 10.0                        # [m]
 
+        # params for avoidance of vehicle type objects that are ambiguous as to whether they are parked.
+        force_avoidance:
+          enable: false                                  # [-]
+          time_threshold: 1.0                            # [s]
+          ignore_area:
+            traffic_light:
+              front_distance: 100.0                      # [m]
+            crosswalk:
+              front_distance: 30.0                       # [m]
+              behind_distance: 30.0                      # [m]
+
       # For safety check
       safety_check:
         # safety check configuration

planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/avoidance_module_data.hpp

@@ -381,6 +381,9 @@ struct ObjectData  // avoidance target
   // is stoppable under the constraints
   bool is_stoppable{false};
 
+  // is within intersection area
+  bool is_within_intersection{false};
+
   // unavoidable reason
   std::string reason{""};
 

planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/utils.hpp

@@ -39,6 +39,9 @@ bool isWithinCrosswalk(
   const ObjectData & object,
   const std::shared_ptr<const lanelet::routing::RoutingGraphContainer> & overall_graphs);
 
+bool isWithinIntersection(
+  const ObjectData & object, const std::shared_ptr<RouteHandler> & route_handler);
+
 bool isTargetObjectType(
   const PredictedObject & object, const std::shared_ptr<AvoidanceParameters> & parameters);
 

planning/behavior_path_planner/include/behavior_path_planner/utils/utils.hpp

@@ -330,6 +330,8 @@ std::optional<double> getSignedDistanceFromBoundary(
 
 Polygon2d toPolygon2d(const lanelet::ConstLanelet & lanelet);
 
+Polygon2d toPolygon2d(const lanelet::BasicPolygon2d & polygon);
+
 std::vector<Polygon2d> getTargetLaneletPolygons(
   const lanelet::ConstLanelets & lanelets, const Pose & pose, const double check_length,
   const std::string & target_type);

planning/behavior_path_planner/src/scene_module/avoidance/manager.cpp

@@ -42,92 +42,96 @@
     p.resample_interval_for_output =
       getOrDeclareParameter<double>(*node, ns + "resample_interval_for_output");
     p.enable_bound_clipping = getOrDeclareParameter<bool>(*node, ns + "enable_bound_clipping");
     p.enable_cancel_maneuver = getOrDeclareParameter<bool>(*node, ns + "enable_cancel_maneuver");
-    p.enable_force_avoidance_for_stopped_vehicle =
-      getOrDeclareParameter<bool>(*node, ns + "enable_force_avoidance_for_stopped_vehicle");
     p.enable_yield_maneuver = getOrDeclareParameter<bool>(*node, ns + "enable_yield_maneuver");
     p.enable_yield_maneuver_during_shifting =
       getOrDeclareParameter<bool>(*node, ns + "enable_yield_maneuver_during_shifting");
     p.disable_path_update = getOrDeclareParameter<bool>(*node, ns + "disable_path_update");
     p.publish_debug_marker = getOrDeclareParameter<bool>(*node, ns + "publish_debug_marker");
     p.print_debug_info = getOrDeclareParameter<bool>(*node, ns + "print_debug_info");
   }
 
   // drivable area
   {
     std::string ns = "avoidance.";
     p.use_adjacent_lane = getOrDeclareParameter<bool>(*node, ns + "use_adjacent_lane");
     p.use_opposite_lane = getOrDeclareParameter<bool>(*node, ns + "use_opposite_lane");
     p.use_intersection_areas = getOrDeclareParameter<bool>(*node, ns + "use_intersection_areas");
     p.use_hatched_road_markings =
       getOrDeclareParameter<bool>(*node, ns + "use_hatched_road_markings");
   }
 
   // target object
   {
     const auto get_object_param = [&](std::string && ns) {
       ObjectParameter param{};
       param.is_target = getOrDeclareParameter<bool>(*node, ns + "is_target");
       param.execute_num = getOrDeclareParameter<int>(*node, ns + "execute_num");
       param.moving_speed_threshold =
         getOrDeclareParameter<double>(*node, ns + "moving_speed_threshold");
       param.moving_time_threshold =
         getOrDeclareParameter<double>(*node, ns + "moving_time_threshold");
       param.max_expand_ratio = getOrDeclareParameter<double>(*node, ns + "max_expand_ratio");
       param.envelope_buffer_margin =
         getOrDeclareParameter<double>(*node, ns + "envelope_buffer_margin");
       param.avoid_margin_lateral =
         getOrDeclareParameter<double>(*node, ns + "avoid_margin_lateral");
       param.safety_buffer_lateral =
         getOrDeclareParameter<double>(*node, ns + "safety_buffer_lateral");
       param.safety_buffer_longitudinal =
         getOrDeclareParameter<double>(*node, ns + "safety_buffer_longitudinal");
       param.use_conservative_buffer_longitudinal =
         getOrDeclareParameter<bool>(*node, ns + "use_conservative_buffer_longitudinal");
       return param;
     };
 
     const std::string ns = "avoidance.target_object.";
     p.object_parameters.emplace(
       ObjectClassification::MOTORCYCLE, get_object_param(ns + "motorcycle."));
     p.object_parameters.emplace(ObjectClassification::CAR, get_object_param(ns + "car."));
     p.object_parameters.emplace(ObjectClassification::TRUCK, get_object_param(ns + "truck."));
     p.object_parameters.emplace(ObjectClassification::TRAILER, get_object_param(ns + "trailer."));
     p.object_parameters.emplace(ObjectClassification::BUS, get_object_param(ns + "bus."));
     p.object_parameters.emplace(
       ObjectClassification::PEDESTRIAN, get_object_param(ns + "pedestrian."));
     p.object_parameters.emplace(ObjectClassification::BICYCLE, get_object_param(ns + "bicycle."));
     p.object_parameters.emplace(ObjectClassification::UNKNOWN, get_object_param(ns + "unknown."));
 
     p.lower_distance_for_polygon_expansion =
       getOrDeclareParameter<double>(*node, ns + "lower_distance_for_polygon_expansion");
     p.upper_distance_for_polygon_expansion =
       getOrDeclareParameter<double>(*node, ns + "upper_distance_for_polygon_expansion");
   }
 
   // target filtering
   {
     std::string ns = "avoidance.target_filtering.";
-    p.threshold_time_force_avoidance_for_stopped_vehicle = getOrDeclareParameter<double>(
-      *node, ns + "threshold_time_force_avoidance_for_stopped_vehicle");
-    p.object_ignore_section_traffic_light_in_front_distance = getOrDeclareParameter<double>(
-      *node, ns + "object_ignore_section_traffic_light_in_front_distance");
-    p.object_ignore_section_crosswalk_in_front_distance = getOrDeclareParameter<double>(
-      *node, ns + "object_ignore_section_crosswalk_in_front_distance");
-    p.object_ignore_section_crosswalk_behind_distance =
-      getOrDeclareParameter<double>(*node, ns + "object_ignore_section_crosswalk_behind_distance");
     p.object_check_goal_distance =
       getOrDeclareParameter<double>(*node, ns + "object_check_goal_distance");
     p.threshold_distance_object_is_on_center =
       getOrDeclareParameter<double>(*node, ns + "threshold_distance_object_is_on_center");
     p.object_check_shiftable_ratio =
       getOrDeclareParameter<double>(*node, ns + "object_check_shiftable_ratio");
     p.object_check_min_road_shoulder_width =
       getOrDeclareParameter<double>(*node, ns + "object_check_min_road_shoulder_width");
     p.object_last_seen_threshold =
       getOrDeclareParameter<double>(*node, ns + "object_last_seen_threshold");
   }
 
+  {
+    std::string ns = "avoidance.target_filtering.force_avoidance.";
+    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.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 =
+      getOrDeclareParameter<double>(*node, ns + "ignore_area.crosswalk.front_distance");
+    p.object_ignore_section_crosswalk_behind_distance =
+      getOrDeclareParameter<double>(*node, ns + "ignore_area.crosswalk.behind_distance");
+  }
+
   {
     std::string ns = "avoidance.target_filtering.detection_area.";
     p.use_static_detection_area = getOrDeclareParameter<bool>(*node, ns + "static");

planning/behavior_path_planner/src/scene_module/lane_change/manager.cpp

@@ -274,72 +274,76 @@
     p.resample_interval_for_planning =
       getOrDeclareParameter<double>(*node, ns + "resample_interval_for_planning");
     p.resample_interval_for_output =
       getOrDeclareParameter<double>(*node, ns + "resample_interval_for_output");
-    p.enable_force_avoidance_for_stopped_vehicle =
-      getOrDeclareParameter<bool>(*node, ns + "enable_force_avoidance_for_stopped_vehicle");
   }
 
   // target object
   {
     const auto get_object_param = [&](std::string && ns) {
       ObjectParameter param{};
       param.is_target = getOrDeclareParameter<bool>(*node, ns + "is_target");
       param.execute_num = getOrDeclareParameter<int>(*node, ns + "execute_num");
       param.moving_speed_threshold =
         getOrDeclareParameter<double>(*node, ns + "moving_speed_threshold");
       param.moving_time_threshold =
         getOrDeclareParameter<double>(*node, ns + "moving_time_threshold");
       param.max_expand_ratio = getOrDeclareParameter<double>(*node, ns + "max_expand_ratio");
       param.envelope_buffer_margin =
         getOrDeclareParameter<double>(*node, ns + "envelope_buffer_margin");
       param.avoid_margin_lateral =
         getOrDeclareParameter<double>(*node, ns + "avoid_margin_lateral");
       param.safety_buffer_lateral =
         getOrDeclareParameter<double>(*node, ns + "safety_buffer_lateral");
       return param;
     };
 
     const std::string ns = "avoidance_by_lane_change.target_object.";
     p.object_parameters.emplace(
       ObjectClassification::MOTORCYCLE, get_object_param(ns + "motorcycle."));
     p.object_parameters.emplace(ObjectClassification::CAR, get_object_param(ns + "car."));
     p.object_parameters.emplace(ObjectClassification::TRUCK, get_object_param(ns + "truck."));
     p.object_parameters.emplace(ObjectClassification::TRAILER, get_object_param(ns + "trailer."));
     p.object_parameters.emplace(ObjectClassification::BUS, get_object_param(ns + "bus."));
     p.object_parameters.emplace(
       ObjectClassification::PEDESTRIAN, get_object_param(ns + "pedestrian."));
     p.object_parameters.emplace(ObjectClassification::BICYCLE, get_object_param(ns + "bicycle."));
     p.object_parameters.emplace(ObjectClassification::UNKNOWN, get_object_param(ns + "unknown."));
 
     p.lower_distance_for_polygon_expansion =
       getOrDeclareParameter<double>(*node, ns + "lower_distance_for_polygon_expansion");
     p.upper_distance_for_polygon_expansion =
       getOrDeclareParameter<double>(*node, ns + "upper_distance_for_polygon_expansion");
   }
 
   // target filtering
   {
     std::string ns = "avoidance.target_filtering.";
-    p.threshold_time_force_avoidance_for_stopped_vehicle = getOrDeclareParameter<double>(
-      *node, ns + "threshold_time_force_avoidance_for_stopped_vehicle");
-    p.object_ignore_section_traffic_light_in_front_distance = getOrDeclareParameter<double>(
-      *node, ns + "object_ignore_section_traffic_light_in_front_distance");
-    p.object_ignore_section_crosswalk_in_front_distance = getOrDeclareParameter<double>(
-      *node, ns + "object_ignore_section_crosswalk_in_front_distance");
-    p.object_ignore_section_crosswalk_behind_distance =
-      getOrDeclareParameter<double>(*node, ns + "object_ignore_section_crosswalk_behind_distance");
     p.object_check_goal_distance =
       getOrDeclareParameter<double>(*node, ns + "object_check_goal_distance");
     p.threshold_distance_object_is_on_center =
       getOrDeclareParameter<double>(*node, ns + "threshold_distance_object_is_on_center");
     p.object_check_shiftable_ratio =
       getOrDeclareParameter<double>(*node, ns + "object_check_shiftable_ratio");
     p.object_check_min_road_shoulder_width =
       getOrDeclareParameter<double>(*node, ns + "object_check_min_road_shoulder_width");
     p.object_last_seen_threshold =
       getOrDeclareParameter<double>(*node, ns + "object_last_seen_threshold");
   }
 
+  {
+    std::string ns = "avoidance.target_filtering.force_avoidance.";
+    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.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 =
+      getOrDeclareParameter<double>(*node, ns + "ignore_area.crosswalk.front_distance");
+    p.object_ignore_section_crosswalk_behind_distance =
+      getOrDeclareParameter<double>(*node, ns + "ignore_area.crosswalk.behind_distance");
+  }
+
   {
     std::string ns = "avoidance.target_filtering.detection_area.";
     p.use_static_detection_area = getOrDeclareParameter<bool>(*node, ns + "static");

planning/behavior_path_planner/src/utils/avoidance/utils.cpp

@@ -1,4 +1,4 @@
 // Copyright 2021 Tier IV, Inc.
 //
 // Licensed under the Apache License, Version 2.0 (the "License");
 // you may not use this file except in compliance with the License.
@@ -292,6 +292,81 @@
   return false;
 }
 
+bool isWithinIntersection(
+  const ObjectData & object, const std::shared_ptr<RouteHandler> & route_handler)
+{
+  const std::string id = object.overhang_lanelet.attributeOr("intersection_area", "else");
+  if (id == "else") {
+    return false;
+  }
+
+  const auto object_polygon = tier4_autoware_utils::toPolygon2d(object.object);
+
+  const auto polygon = route_handler->getLaneletMapPtr()->polygonLayer.get(std::atoi(id.c_str()));
+
+  return boost::geometry::within(
+    object_polygon, utils::toPolygon2d(lanelet::utils::to2D(polygon.basicPolygon())));
+}
+
+bool isForceAvoidanceTarget(
+  ObjectData & object, const lanelet::ConstLanelets & extend_lanelets,
+  const std::shared_ptr<const PlannerData> & planner_data,
+  const std::shared_ptr<AvoidanceParameters> & parameters)
+{
+  if (!parameters->enable_force_avoidance_for_stopped_vehicle) {
+    return false;
+  }
+
+  const auto stop_time_longer_than_threshold =
+    object.stop_time > parameters->threshold_time_force_avoidance_for_stopped_vehicle;
+
+  if (!stop_time_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.");
+    return false;
+  }
+
+  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;
+
+  // check traffic light
+  const auto to_traffic_light = utils::getDistanceToNextTrafficLight(object_pose, extend_lanelets);
+  {
+    not_parked_object =
+      to_traffic_light < parameters->object_ignore_section_traffic_light_in_front_distance;
+  }
+
+  // check crosswalk
+  const auto to_crosswalk =
+    utils::getDistanceToCrosswalk(ego_pose, extend_lanelets, *rh->getOverallGraphPtr()) -
+    object.longitudinal;
+  {
+    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;
+  }
+
+  object.to_stop_factor_distance = std::min(to_traffic_light, to_crosswalk);
+
+  return !not_parked_object;
+}
+
 double calcShiftLength(
   const bool & is_object_on_right, const double & overhang_dist, const double & avoid_margin)
 {
@@ -1127,42 +1202,14 @@
       continue;
     }
 
-    // from here condition check for vehicle type objects.
+    o.is_within_intersection = isWithinIntersection(o, rh);
 
-    const auto stop_time_longer_than_threshold =
-      o.stop_time > parameters->threshold_time_force_avoidance_for_stopped_vehicle;
-
-    if (stop_time_longer_than_threshold && parameters->enable_force_avoidance_for_stopped_vehicle) {
-      // force avoidance for stopped vehicle
-      bool not_parked_object = true;
-
-      // check traffic light
-      const auto to_traffic_light =
-        utils::getDistanceToNextTrafficLight(object_pose, extend_lanelets);
-      {
-        not_parked_object =
-          to_traffic_light < parameters->object_ignore_section_traffic_light_in_front_distance;
-      }
-
-      // check crosswalk
-      const auto to_crosswalk =
-        utils::getDistanceToCrosswalk(ego_pose, extend_lanelets, *rh->getOverallGraphPtr()) -
-        o.longitudinal;
-      {
-        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;
-      }
-
-      o.to_stop_factor_distance = std::min(to_traffic_light, to_crosswalk);
-
-      if (!not_parked_object) {
-        o.last_seen = now;
-        o.avoid_margin = avoid_margin;
-        data.target_objects.push_back(o);
-        continue;
-      }
+    // from here condition check for vehicle type objects.
+    if (isForceAvoidanceTarget(o, extend_lanelets, planner_data, parameters)) {
+      o.last_seen = now;
+      o.avoid_margin = avoid_margin;
+      data.target_objects.push_back(o);
+      continue;
     }
 
     // Object is on center line -> ignore.

planning/behavior_path_planner/src/utils/utils.cpp

@@ -1,4 +1,4 @@
 // Copyright 2021-2023 Tier IV, Inc.
 //
 // Licensed under the Apache License, Version 2.0 (the "License");
 // you may not use this file except in compliance with the License.
@@ -2708,6 +2708,17 @@
            : tier4_autoware_utils::inverseClockwise(polygon);
 }
 
+Polygon2d toPolygon2d(const lanelet::BasicPolygon2d & polygon)
+{
+  Polygon2d ret;
+  for (const auto & p : polygon) {
+    ret.outer().emplace_back(p.x(), p.y());
+  }
+  ret.outer().push_back(ret.outer().front());
+
+  return tier4_autoware_utils::isClockwise(ret) ? ret : tier4_autoware_utils::inverseClockwise(ret);
+}
+
 std::vector<Polygon2d> getTargetLaneletPolygons(
   const lanelet::PolygonLayer & map_polygons, lanelet::ConstLanelets & lanelets, const Pose & pose,
   const double check_length, const std::string & target_type)