feat(lane_change): improve delay lane change logic (#9480)

* implement function to check if lane change delay is required Signed-off-by: mohammad alqudah <[email protected]> * refactor function isParkedObject Signed-off-by: mohammad alqudah <[email protected]> * refactor delay lane change parameters Signed-off-by: mohammad alqudah <[email protected]> * update lc param yaml Signed-off-by: mohammad alqudah <[email protected]> * separate target lane leading objects based on behavior (RT1-8532) Signed-off-by: Zulfaqar Azmi <[email protected]> * fixed overlapped filtering and lanes debug marker Signed-off-by: Zulfaqar Azmi <[email protected]> * combine filteredObjects function Signed-off-by: Zulfaqar Azmi <[email protected]> * renaming functions and type Signed-off-by: Zulfaqar Azmi <[email protected]> * update some logic to check is stopped Signed-off-by: Zulfaqar Azmi <[email protected]> * rename expanded to stopped_outside_boundary Signed-off-by: Zulfaqar Azmi <[email protected]> * Include docstring Signed-off-by: Zulfaqar Azmi <[email protected]> * rename stopped_outside_boundary → stopped_at_bound Signed-off-by: Zulfaqar Azmi <[email protected]> * Update planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/path_safety_checker/objects_filtering.hpp Co-authored-by: mkquda <[email protected]> Signed-off-by: Zulfaqar Azmi <[email protected]> * Update planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/path_safety_checker/objects_filtering.hpp Co-authored-by: mkquda <[email protected]> Signed-off-by: Zulfaqar Azmi <[email protected]> * spell-check Signed-off-by: Zulfaqar Azmi <[email protected]> * add docstring for function is_delay_lane_change Signed-off-by: mohammad alqudah <[email protected]> * remove unused functions Signed-off-by: mohammad alqudah <[email protected]> * fix spelling Signed-off-by: mohammad alqudah <[email protected]> * add delay parameters to README Signed-off-by: mohammad alqudah <[email protected]> * add documentation for delay lane change behavior Signed-off-by: mohammad alqudah <[email protected]> * Update planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/utils.cpp Co-authored-by: Zulfaqar Azmi <[email protected]> * Update planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/utils.cpp Co-authored-by: Zulfaqar Azmi <[email protected]> * Update planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/utils.cpp Co-authored-by: Zulfaqar Azmi <[email protected]> * run pre-commit checks Signed-off-by: mohammad alqudah <[email protected]> * only check for delay lc if feature is enabled Signed-off-by: mohammad alqudah <[email protected]> --------- Signed-off-by: mohammad alqudah <[email protected]> Signed-off-by: Zulfaqar Azmi <[email protected]> Co-authored-by: Zulfaqar Azmi <[email protected]> Co-authored-by: Zulfaqar Azmi <[email protected]>
autowarefoundation · Nov 28, 2024 · c4608ad · c4608ad
1 parent 08621e6
commit c4608ad
Show file tree

Hide file tree

Showing 12 changed files with 3,495 additions and 176 deletions.
diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/README.md b/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

diff --git a/...vior_path_planner/autoware_behavior_path_lane_change_module/config/lane_change.param.yaml b/...vior_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

diff --git a/...autoware_behavior_path_lane_change_module/images/delay_lane_change_1.drawio.svg b/...autoware_behavior_path_lane_change_module/images/delay_lane_change_1.drawio.svg
diff --git a/...autoware_behavior_path_lane_change_module/images/delay_lane_change_2.drawio.svg b/...autoware_behavior_path_lane_change_module/images/delay_lane_change_2.drawio.svg
diff --git a/...autoware_behavior_path_lane_change_module/images/delay_lane_change_3.drawio.svg b/...autoware_behavior_path_lane_change_module/images/delay_lane_change_3.drawio.svg
diff --git a/...autoware_behavior_path_lane_change_module/images/delay_lane_change_4.drawio.svg b/...autoware_behavior_path_lane_change_module/images/delay_lane_change_4.drawio.svg
diff --git a/...autoware_behavior_path_lane_change_module/images/delay_lane_change_5.drawio.svg b/...autoware_behavior_path_lane_change_module/images/delay_lane_change_5.drawio.svg
diff --git a/...lane_change_module/include/autoware/behavior_path_lane_change_module/utils/parameters.hpp b/...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};

diff --git a/...path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/utils.hpp b/...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);

diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/manager.cpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/manager.cpp
@@ -99,12 +99,6 @@ LCParamPtr LaneChangeModuleManager::set_params(rclcpp::Node * node, const std::s
     }
   }
 
-  // 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"));
@@ -203,6 +197,15 @@ LCParamPtr LaneChangeModuleManager::set_params(rclcpp::Node * node, const std::s
       "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"));

diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/scene.cpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/scene.cpp
@@ -1341,7 +1341,7 @@ bool NormalLaneChange::check_candidate_path_safety(
   }
 
   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 @@ PathSafetyStatus NormalLaneChange::isApprovedPathSafe() const
     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};
   }