feat(lane_change): improve delay lane change logic

planning/behavior_path_planner/autoware_behavior_path_lane_change_module/README.md

@@ -351,6 +351,71 @@ stop
 @enduml
 ```
 
+#### Delay Lane Change Check
+
+In certain situations, when there are stopped vehicles along the target lane ahead of Ego vehicle, to avoid getting stuck, it is desired to perform the lane change maneuver after the stopped vehicle.
+To do so, all static objects ahead of ego along the target lane are checked in order from closest to furthest, if any object satisfies the following conditions, lane change will be delayed and candidate path will be rejected.
+
+1. The distance from object to terminal end is sufficient to perform lane change
+2. The distance to object is less than the lane changing length
+3. The distance from object to next object is sufficient to perform lane change
+
+If the parameter `check_only_parked_vehicle` is set to `true`, the check will only consider objects which are determined as parked.
+
+The following flow chart illustrates the delay lane change check.
+
+```plantuml
+@startuml
+skinparam defaultTextAlignment center
+skinparam backgroundColor #White
+
+start
+if (Is target objects, candidate path, OR current lane path empty?) then (yes)
+  #LightPink:Return false;
+  stop
+else (no)
+endif
+
+:Start checking objects from closest to furthest;
+repeat
+  if (Is distance from object to terminal sufficient) then (yes)
+  else (no)
+    #LightPink:Return false;
+    stop
+  endif
+
+  if (Is distance to object less than lane changing length) then (yes)
+  else (no)
+    if (Is only check parked vehicles and vehicle is not parked) then (yes)
+    else (no)
+      if(Is last object OR distance to next object is sufficient) then (yes)
+        #LightGreen: Return true;
+        stop
+      else (no)
+      endif
+    endif
+  endif
+  repeat while (Is finished checking all objects) is (FALSE)
+
+#LightPink: Return false;
+stop
+
+@enduml
+```
+
+The following figures demonstrate different situations under which will or will not be triggered:
+
+1. Delay lane change will be triggered as there is sufficient distance ahead.
+   ![delay lane change 1](./images/delay_lane_change_1.drawio.svg)
+2. Delay lane change will NOT be triggered as there is no sufficient distance ahead
+   ![delay lane change 2](./images/delay_lane_change_2.drawio.svg)
+3. Delay lane change will be triggered by fist NPC as there is sufficient distance ahead.
+   ![delay lane change 3](./images/delay_lane_change_3.drawio.svg)
+4. Delay lane change will be triggered by second NPC as there is sufficient distance ahead
+   ![delay lane change 4](./images/delay_lane_change_4.drawio.svg)
+5. Delay lane change will NOT be triggered as there is no sufficient distance ahead.
+   ![delay lane change 5](./images/delay_lane_change_5.drawio.svg)
+
 #### Candidate Path's Safety check
 
 See [safety check utils explanation](../autoware_behavior_path_planner_common/docs/behavior_path_planner_safety_check.md)
@@ -828,8 +893,6 @@ The following parameters are configurable in [lane_change.param.yaml](https://gi
 | `trajectory.lat_acc_sampling_num`            | [-]    | int    | Number of possible lane-changing trajectories that are being influenced by lateral acceleration                        | 3                  |
 | `trajectory.max_longitudinal_acc`            | [m/s2] | double | maximum longitudinal acceleration for lane change                                                                      | 1.0                |
 | `trajectory.min_longitudinal_acc`            | [m/s2] | double | maximum longitudinal deceleration for lane change                                                                      | -1.0               |
-| `object_check_min_road_shoulder_width`       | [m]    | double | Width considered as a road shoulder if the lane does not have a road shoulder                                          | 0.5                |
-| `object_shiftable_ratio_threshold`           | [-]    | double | Vehicles around the center line within this distance ratio will be excluded from parking objects                       | 0.6                |
 | `min_length_for_turn_signal_activation`      | [m]    | double | Turn signal will be activated if the ego vehicle approaches to this length from minimum lane change length             | 10.0               |
 | `lateral_acceleration.velocity`              | [m/s]  | double | Reference velocity for lateral acceleration calculation (look up table)                                                | [0.0, 4.0, 10.0]   |
 | `lateral_acceleration.min_values`            | [m/s2] | double | Min lateral acceleration values corresponding to velocity (look up table)                                              | [0.4, 0.4, 0.4]    |
@@ -860,6 +923,15 @@ The following parameters are used to judge lane change completion.
 | `stuck_detection.velocity`  | [m/s] | double | Velocity threshold for ego vehicle stuck detection  | 0.1           |
 | `stuck_detection.stop_time` | [s]   | double | Stop time threshold for ego vehicle stuck detection | 3.0           |
 
+### Delay Lane Change
+
+| Name                                              | Unit | Type   | Description                                                                                           | Default value |
+| :------------------------------------------------ | ---- | ------ | ----------------------------------------------------------------------------------------------------- | ------------- |
+| `delay_lane_change.enable`                        | [-]  | bool   | Flag to enable/disable lane change delay feature                                                      | true          |
+| `delay_lane_change.check_only_parked_vehicle`     | [-]  | bool   | Flag to limit delay feature for only parked vehicles                                                  | false         |
+| `delay_lane_change.min_road_shoulder_width`       | [m]  | double | Width considered as road shoulder if lane doesn't have road shoulder when checking for parked vehicle | 0.5           |
+| `delay_lane_change.th_parked_vehicle_shift_ratio` | [-]  | double | Stopped vehicles beyond this distance ratio from center line will be considered as parked             | 0.6           |
+
 ### Collision checks
 
 #### Target Objects

planning/behavior_path_planner/autoware_behavior_path_lane_change_module/config/lane_change.param.yaml

@@ -6,10 +6,6 @@
       backward_length_buffer_for_blocking_object: 3.0 # [m]
       backward_length_from_intersection: 5.0 # [m]
 
-      # side walk parked vehicle
-      object_check_min_road_shoulder_width: 0.5  # [m]
-      object_shiftable_ratio_threshold: 0.6
-
       # turn signal
       min_length_for_turn_signal_activation: 10.0 # [m]
 
@@ -25,6 +21,13 @@
         lon_acc_sampling_num: 5
         lat_acc_sampling_num: 3
 
+      # delay lane change
+      delay_lane_change:
+        enable: true
+        check_only_parked_vehicle: false
+        min_road_shoulder_width: 0.5  # [m]
+        th_parked_vehicle_shift_ratio: 0.6
+
       # safety check
       safety_check:
         allow_loose_check_for_cancel: true

planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/parameters.hpp

@@ -127,11 +127,20 @@ struct TrajectoryParameters
   LateralAccelerationMap lat_acc_map{};
 };
 
+struct DelayParameters
+{
+  bool enable{true};
+  bool check_only_parked_vehicle{false};
+  double min_road_shoulder_width{0.5};
+  double th_parked_vehicle_shift_ratio{0.6};
+};
+
 struct Parameters
 {
   TrajectoryParameters trajectory{};
   SafetyParameters safety{};
   CancelParameters cancel{};
+  DelayParameters delay{};
 
   // lane change parameters
   double backward_lane_length{200.0};

planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/utils.hpp

@@ -132,14 +132,29 @@ bool isParkedObject(
   const ExtendedPredictedObject & object, const double buffer_to_bound,
   const double ratio_threshold);
 
-bool passed_parked_objects(
+/**
+ * @brief Checks if delaying of lane change maneuver is necessary
+ *
+ * @details Scans through the provided target objects (assumed to be ordered from closest to
+ * furthest), and returns true if any of the objects satisfy the following conditions:
+ *  - Not near the end of current lanes
+ *  - There is sufficient distance from object to next one to do lane change
+ * If the parameter delay_lc_param.check_only_parked_vehicle is set to True, only objects
+ * which pass isParkedObject() check will be considered.
+ *
+ * @param common_data_ptr Shared pointer to CommonData that holds necessary lanes info, parameters,
+ *                        and transient data.
+ * @param lane_change_path Candidate lane change path to apply checks on.
+ * @param target_objects Relevant objects to consider for delay LC checks (assumed to only include
+ *                       target lane leading static objects).
+ * @param object_debug Collision check debug struct to be updated if any of the target objects
+ *                     satisfy the conditions.
+ * @return bool True if conditions to delay lane change are met
+ */
+bool is_delay_lane_change(
   const CommonDataPtr & common_data_ptr, const LaneChangePath & lane_change_path,
-  const std::vector<ExtendedPredictedObject> & objects, CollisionCheckDebugMap & object_debug);
-
-std::optional<size_t> getLeadingStaticObjectIdx(
-  const RouteHandler & route_handler, const LaneChangePath & lane_change_path,
-  const std::vector<ExtendedPredictedObject> & objects,
-  const double object_check_min_road_shoulder_width, const double object_shiftable_ratio_threshold);
+  const std::vector<ExtendedPredictedObject> & target_objects,
+  CollisionCheckDebugMap & object_debug);
 
 lanelet::BasicPolygon2d create_polygon(
   const lanelet::ConstLanelets & lanes, const double start_dist, const double end_dist);

planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/manager.cpp

@@ -98,111 +98,114 @@
         lateral_acc_velocity.at(i), min_lateral_acc.at(i), max_lateral_acc.at(i));
     }
   }
 
-  // parked vehicle detection
-  p.object_check_min_road_shoulder_width =
-    getOrDeclareParameter<double>(*node, parameter("object_check_min_road_shoulder_width"));
-  p.th_object_shiftable_ratio =
-    getOrDeclareParameter<double>(*node, parameter("object_shiftable_ratio_threshold"));
-
   // turn signal
   p.min_length_for_turn_signal_activation =
     getOrDeclareParameter<double>(*node, parameter("min_length_for_turn_signal_activation"));
 
   // lane change regulations
   p.regulate_on_crosswalk = getOrDeclareParameter<bool>(*node, parameter("regulation.crosswalk"));
   p.regulate_on_intersection =
     getOrDeclareParameter<bool>(*node, parameter("regulation.intersection"));
   p.regulate_on_traffic_light =
     getOrDeclareParameter<bool>(*node, parameter("regulation.traffic_light"));
 
   // ego vehicle stuck detection
   p.th_stop_velocity = getOrDeclareParameter<double>(*node, parameter("stuck_detection.velocity"));
   p.th_stop_time = getOrDeclareParameter<double>(*node, parameter("stuck_detection.stop_time"));
 
   // safety
   {
     p.safety.enable_loose_check_for_cancel =
       getOrDeclareParameter<bool>(*node, parameter("safety_check.allow_loose_check_for_cancel"));
     p.safety.enable_target_lane_bound_check =
       getOrDeclareParameter<bool>(*node, parameter("safety_check.enable_target_lane_bound_check"));
     p.safety.th_stopped_object_velocity = getOrDeclareParameter<double>(
       *node, parameter("safety_check.stopped_object_velocity_threshold"));
     p.safety.lane_expansion_left_offset =
       getOrDeclareParameter<double>(*node, parameter("safety_check.lane_expansion.left_offset"));
     p.safety.lane_expansion_right_offset =
       getOrDeclareParameter<double>(*node, parameter("safety_check.lane_expansion.right_offset"));
 
     // collision check
     p.safety.collision_check.enable_for_prepare_phase_in_general_lanes =
       getOrDeclareParameter<bool>(
         *node, parameter("collision_check.enable_for_prepare_phase.general_lanes"));
     p.safety.collision_check.enable_for_prepare_phase_in_intersection = getOrDeclareParameter<bool>(
       *node, parameter("collision_check.enable_for_prepare_phase.intersection"));
     p.safety.collision_check.enable_for_prepare_phase_in_turns = getOrDeclareParameter<bool>(
       *node, parameter("collision_check.enable_for_prepare_phase.turns"));
     p.safety.collision_check.check_current_lane =
       getOrDeclareParameter<bool>(*node, parameter("collision_check.check_current_lanes"));
     p.safety.collision_check.check_other_lanes =
       getOrDeclareParameter<bool>(*node, parameter("collision_check.check_other_lanes"));
     p.safety.collision_check.use_all_predicted_paths =
       getOrDeclareParameter<bool>(*node, parameter("collision_check.use_all_predicted_paths"));
     p.safety.collision_check.prediction_time_resolution =
       getOrDeclareParameter<double>(*node, parameter("collision_check.prediction_time_resolution"));
     p.safety.collision_check.th_yaw_diff =
       getOrDeclareParameter<double>(*node, parameter("collision_check.yaw_diff_threshold"));
 
     // rss check
     auto set_rss_params = [&](auto & params, const std::string & prefix) {
       params.longitudinal_distance_min_threshold = getOrDeclareParameter<double>(
         *node, parameter(prefix + ".longitudinal_distance_min_threshold"));
       params.longitudinal_velocity_delta_time = getOrDeclareParameter<double>(
         *node, parameter(prefix + ".longitudinal_velocity_delta_time"));
       params.front_vehicle_deceleration =
         getOrDeclareParameter<double>(*node, parameter(prefix + ".expected_front_deceleration"));
       params.rear_vehicle_deceleration =
         getOrDeclareParameter<double>(*node, parameter(prefix + ".expected_rear_deceleration"));
       params.rear_vehicle_reaction_time =
         getOrDeclareParameter<double>(*node, parameter(prefix + ".rear_vehicle_reaction_time"));
       params.rear_vehicle_safety_time_margin = getOrDeclareParameter<double>(
         *node, parameter(prefix + ".rear_vehicle_safety_time_margin"));
       params.lateral_distance_max_threshold =
         getOrDeclareParameter<double>(*node, parameter(prefix + ".lateral_distance_max_threshold"));
     };
     set_rss_params(p.safety.rss_params, "safety_check.execution");
     set_rss_params(p.safety.rss_params_for_parked, "safety_check.parked");
     set_rss_params(p.safety.rss_params_for_abort, "safety_check.cancel");
     set_rss_params(p.safety.rss_params_for_stuck, "safety_check.stuck");
 
     // target object types
     const std::string ns = "lane_change.target_object.";
     p.safety.target_object_types.check_car = getOrDeclareParameter<bool>(*node, ns + "car");
     p.safety.target_object_types.check_truck = getOrDeclareParameter<bool>(*node, ns + "truck");
     p.safety.target_object_types.check_bus = getOrDeclareParameter<bool>(*node, ns + "bus");
     p.safety.target_object_types.check_trailer = getOrDeclareParameter<bool>(*node, ns + "trailer");
     p.safety.target_object_types.check_unknown = getOrDeclareParameter<bool>(*node, ns + "unknown");
     p.safety.target_object_types.check_bicycle = getOrDeclareParameter<bool>(*node, ns + "bicycle");
     p.safety.target_object_types.check_motorcycle =
       getOrDeclareParameter<bool>(*node, ns + "motorcycle");
     p.safety.target_object_types.check_pedestrian =
       getOrDeclareParameter<bool>(*node, ns + "pedestrian");
   }
 
   // lane change parameters
   p.backward_lane_length = getOrDeclareParameter<double>(*node, parameter("backward_lane_length"));
   p.backward_length_buffer_for_end_of_lane =
     getOrDeclareParameter<double>(*node, parameter("backward_length_buffer_for_end_of_lane"));
   p.backward_length_buffer_for_blocking_object =
     getOrDeclareParameter<double>(*node, parameter("backward_length_buffer_for_blocking_object"));
   p.backward_length_from_intersection =
     getOrDeclareParameter<double>(*node, parameter("backward_length_from_intersection"));
 
   if (p.backward_length_buffer_for_end_of_lane < 1.0) {
     RCLCPP_WARN_STREAM(
       node->get_logger().get_child(node_name),
       "Lane change buffer must be more than 1 meter. Modifying the buffer.");
   }
 
+  // lane change delay
+  p.delay.enable = getOrDeclareParameter<bool>(*node, parameter("delay_lane_change.enable"));
+  p.delay.check_only_parked_vehicle =
+    getOrDeclareParameter<bool>(*node, parameter("delay_lane_change.check_only_parked_vehicle"));
+  p.delay.min_road_shoulder_width =
+    getOrDeclareParameter<double>(*node, parameter("delay_lane_change.min_road_shoulder_width"));
+  p.delay.th_parked_vehicle_shift_ratio = getOrDeclareParameter<double>(
+    *node, parameter("delay_lane_change.th_parked_vehicle_shift_ratio"));
+
   // lane change cancel
   p.cancel.enable_on_prepare_phase =
     getOrDeclareParameter<bool>(*node, parameter("cancel.enable_on_prepare_phase"));

planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/scene.cpp

@@ -1,4 +1,4 @@
 // Copyright 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.
@@ -1341,7 +1341,7 @@
   }
 
   if (
-    !is_stuck && !utils::lane_change::passed_parked_objects(
+    !is_stuck && utils::lane_change::is_delay_lane_change(
                    common_data_ptr_, candidate_path, filtered_objects_.target_lane_leading.stopped,
                    lane_change_debug_.collision_check_objects)) {
     throw std::logic_error(
@@ -1522,10 +1522,8 @@
     return {false, true};
   }
 
-  const auto has_passed_parked_objects = utils::lane_change::passed_parked_objects(
-    common_data_ptr_, path, filtered_objects_.target_lane_leading.stopped, debug_data);
-
-  if (!has_passed_parked_objects) {
+  if (utils::lane_change::is_delay_lane_change(
+        common_data_ptr_, path, filtered_objects_.target_lane_leading.stopped, debug_data)) {
     RCLCPP_DEBUG(logger_, "Lane change has been delayed.");
     return {false, false};
   }