add docstring for function is_delay_lane_change

Signed-off-by: mohammad alqudah <[email protected]>

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

@@ -27,7 +27,7 @@
         longitudinal_distance_diff_threshold:
           prepare: 1.0
           lane_changing: 1.0
-      
+
       # delay lane change
       delay_lane_change:
         enable: true

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

@@ -132,9 +132,28 @@ bool isParkedObject(
   const ExtendedPredictedObject & object, const double buffer_to_bound,
   const double ratio_threshold);
 
+/**
+ * @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 Cadidate 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 traget 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> & target_lane_static_objects,
+  const std::vector<ExtendedPredictedObject> & target_objects,
   CollisionCheckDebugMap & object_debug);
 
 bool passed_parked_objects(

planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/manager.cpp

@@ -224,13 +224,13 @@ LCParamPtr LaneChangeModuleManager::set_params(rclcpp::Node * node, const std::s
   }
 
   // lane change delay
-  p.delay.enable = getOrDeclareParameter<bool>(*node, parameter("delay.enable"));
+  p.delay.enable = getOrDeclareParameter<bool>(*node, parameter("delay_lane_change.enable"));
   p.delay.check_only_parked_vehicle =
-    getOrDeclareParameter<bool>(*node, parameter("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.min_road_shoulder_width"));
-  p.delay.th_parked_vehicle_shift_ratio =
-    getOrDeclareParameter<double>(*node, parameter("delay.th_parked_vehicle_shift_ratio"));
+    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 =

planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/scene.cpp

@@ -1338,7 +1338,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(
@@ -1519,10 +1519,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};
   }

planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/utils.cpp

@@ -741,48 +741,44 @@ bool isParkedObject(
 
 bool is_delay_lane_change(
   const CommonDataPtr & common_data_ptr, const LaneChangePath & lane_change_path,
-  const std::vector<ExtendedPredictedObject> & target_lane_objects,
+  const std::vector<ExtendedPredictedObject> & target_objects,
   CollisionCheckDebugMap & object_debug)
 {
   const auto & current_lane_path = common_data_ptr->current_lanes_path;
 
   if (
-    target_lane_objects.empty() || lane_change_path.path.points.empty() ||
+    target_objects.empty() || lane_change_path.path.points.empty() ||
     current_lane_path.points.empty()) {
     return false;
   }
 
-  const auto vel_threshold = common_data_ptr->lc_param_ptr->stopped_object_velocity_threshold;
-  auto is_static = [&vel_threshold](const ExtendedPredictedObject & obj) {
-    const auto obj_vel_norm = std::hypot(obj.initial_twist.linear.x, obj.initial_twist.linear.y);
-    return obj_vel_norm < vel_threshold;
-  };
-
   const auto dist_to_end = common_data_ptr->transient_data.dist_to_terminal_end;
   const auto dist_buffer = common_data_ptr->transient_data.current_dist_buffer.min;
   auto is_near_end = [&dist_to_end, &dist_buffer](const ExtendedPredictedObject & obj) {
     const auto dist_obj_to_end = dist_to_end - obj.dist_from_ego;
     return dist_obj_to_end <= dist_buffer;
   };
 
-  const auto lc_length = lane_change_path.info.length.lane_changing;
-  auto is_sufficient_gap = [&lc_length](
+  const auto ego_vel = common_data_ptr->get_ego_speed();
+  const auto min_lon_acc = common_data_ptr->lc_param_ptr->min_longitudinal_acc;
+  const auto gap_threshold = std::abs((ego_vel * ego_vel) / (2 * min_lon_acc));
+  auto is_sufficient_gap = [&gap_threshold](
                              const ExtendedPredictedObject & current_obj,
                              const ExtendedPredictedObject & next_obj) {
     const auto curr_obj_half_length = current_obj.shape.dimensions.x;
     const auto next_obj_half_length = next_obj.shape.dimensions.x;
     const auto dist_current_to_next = next_obj.dist_from_ego - current_obj.dist_from_ego;
     const auto gap_length = dist_current_to_next - curr_obj_half_length - next_obj_half_length;
-    return gap_length > lc_length;
+    return gap_length > gap_threshold;
   };
 
   const auto & delay_lc_param = common_data_ptr->lc_param_ptr->delay;
 
-  auto it = target_lane_objects.begin();
-  for (; it < target_lane_objects.end(); ++it) {
+  auto it = target_objects.begin();
+  for (; it < target_objects.end(); ++it) {
     if (is_near_end(*it)) break;
 
-    if (!is_static(*it) || it->dist_from_ego < lc_length) continue;
+    if (it->dist_from_ego < lane_change_path.info.length.lane_changing) continue;
 
     if (
       delay_lc_param.check_only_parked_vehicle &&
@@ -793,7 +789,7 @@ bool is_delay_lane_change(
     }
 
     auto next_it = std::next(it);
-    if (next_it == target_lane_objects.end() || is_sufficient_gap(*it, *next_it)) {
+    if (next_it == target_objects.end() || is_sufficient_gap(*it, *next_it)) {
       auto debug = utils::path_safety_checker::createObjectDebug(*it);
       debug.second.unsafe_reason = "delay lane change";
       utils::path_safety_checker::updateCollisionCheckDebugMap(object_debug, debug, false);