feat(lane_change): improve delay lane change logic

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};
   }

planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/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.
@@ -667,71 +667,34 @@
   // --------------------------------------------
   // +: object position
   // o: nearest point on centerline
 
-  using lanelet::geometry::distance2d;
-  using lanelet::geometry::toArcCoordinates;
-
   const double object_vel_norm =
     std::hypot(object.initial_twist.linear.x, object.initial_twist.linear.y);
   if (object_vel_norm > static_object_velocity_threshold) {
     return false;
   }
 
   const auto & object_pose = object.initial_pose;
   const auto object_closest_index =
     autoware::motion_utils::findNearestIndex(path.points, object_pose.position);
   const auto object_closest_pose = path.points.at(object_closest_index).point.pose;
 
   lanelet::ConstLanelet closest_lanelet;
   if (!route_handler.getClosestLaneletWithinRoute(object_closest_pose, &closest_lanelet)) {
     return false;
   }
 
   const double lat_dist =
     autoware::motion_utils::calcLateralOffset(path.points, object_pose.position);
-  lanelet::BasicLineString2d bound;
-  double center_to_bound_buffer = 0.0;
-  if (lat_dist > 0.0) {
-    // left side vehicle
-    const auto most_left_road_lanelet = route_handler.getMostLeftLanelet(closest_lanelet);
-    const auto most_left_lanelet_candidates =
-      route_handler.getLaneletMapPtr()->laneletLayer.findUsages(most_left_road_lanelet.leftBound());
-    lanelet::ConstLanelet most_left_lanelet = most_left_road_lanelet;
-    const lanelet::Attribute most_left_sub_type =
-      most_left_lanelet.attribute(lanelet::AttributeName::Subtype);
-
-    for (const auto & ll : most_left_lanelet_candidates) {
-      const auto & sub_type = ll.attribute(lanelet::AttributeName::Subtype);
-      if (sub_type.value() == "road_shoulder") {
-        most_left_lanelet = ll;
-      }
-    }
-    bound = most_left_lanelet.leftBound2d().basicLineString();
-    if (most_left_sub_type.value() != "road_shoulder") {
-      center_to_bound_buffer = object_check_min_road_shoulder_width;
-    }
-  } else {
-    // right side vehicle
-    const auto most_right_road_lanelet = route_handler.getMostRightLanelet(closest_lanelet);
-    const auto most_right_lanelet_candidates =
-      route_handler.getLaneletMapPtr()->laneletLayer.findUsages(
-        most_right_road_lanelet.rightBound());
-
-    lanelet::ConstLanelet most_right_lanelet = most_right_road_lanelet;
-    const lanelet::Attribute most_right_sub_type =
-      most_right_lanelet.attribute(lanelet::AttributeName::Subtype);
-
-    for (const auto & ll : most_right_lanelet_candidates) {
-      const auto & sub_type = ll.attribute(lanelet::AttributeName::Subtype);
-      if (sub_type.value() == "road_shoulder") {
-        most_right_lanelet = ll;
-      }
-    }
-    bound = most_right_lanelet.rightBound2d().basicLineString();
-    if (most_right_sub_type.value() != "road_shoulder") {
-      center_to_bound_buffer = object_check_min_road_shoulder_width;
-    }
-  }
+  const auto most_side_lanelet =
+    lat_dist > 0.0 ? route_handler.getMostLeftLanelet(closest_lanelet, false, true)
+                   : route_handler.getMostRightLanelet(closest_lanelet, false, true);
+  const auto bound = lat_dist > 0.0 ? most_side_lanelet.leftBound2d().basicLineString()
+                                    : most_side_lanelet.rightBound2d().basicLineString();
+  const lanelet::Attribute lanelet_sub_type =
+    most_side_lanelet.attribute(lanelet::AttributeName::Subtype);
+  const auto center_to_bound_buffer =
+    lanelet_sub_type.value() == "road_shoulder" ? 0.0 : object_check_min_road_shoulder_width;
 
   return isParkedObject(
     closest_lanelet, bound, object, center_to_bound_buffer, object_shiftable_ratio_threshold);
@@ -776,129 +739,65 @@
   return clamped_ratio > ratio_threshold;
 }
 
-bool passed_parked_objects(
+bool is_delay_lane_change(
   const CommonDataPtr & common_data_ptr, const LaneChangePath & lane_change_path,
-  const std::vector<ExtendedPredictedObject> & objects, CollisionCheckDebugMap & object_debug)
+  const std::vector<ExtendedPredictedObject> & target_objects,
+  CollisionCheckDebugMap & object_debug)
 {
-  const auto route_handler = *common_data_ptr->route_handler_ptr;
-  const auto lane_change_parameters = *common_data_ptr->lc_param_ptr;
-  const auto & object_check_min_road_shoulder_width =
-    lane_change_parameters.object_check_min_road_shoulder_width;
-  const auto & object_shiftable_ratio_threshold = lane_change_parameters.th_object_shiftable_ratio;
   const auto & current_lane_path = common_data_ptr->current_lanes_path;
 
-  if (objects.empty() || lane_change_path.path.points.empty() || current_lane_path.points.empty()) {
-    return true;
-  }
-
-  const auto leading_obj_idx = getLeadingStaticObjectIdx(
-    route_handler, lane_change_path, objects, object_check_min_road_shoulder_width,
-    object_shiftable_ratio_threshold);
-  if (!leading_obj_idx) {
-    return true;
-  }
-
-  const auto & leading_obj = objects.at(*leading_obj_idx);
-  auto debug = utils::path_safety_checker::createObjectDebug(leading_obj);
-  const auto leading_obj_poly =
-    autoware::universe_utils::toPolygon2d(leading_obj.initial_pose, leading_obj.shape);
-  if (leading_obj_poly.outer().empty()) {
-    return true;
+  if (
+    target_objects.empty() || lane_change_path.path.points.empty() ||
+    current_lane_path.points.empty()) {
+    return false;
   }
 
-  const auto & current_path_end = current_lane_path.points.back().point.pose.position;
-  const auto dist_to_path_end = [&](const auto & src_point) {
-    if (common_data_ptr->lanes_ptr->current_lane_in_goal_section) {
-      const auto goal_pose = route_handler.getGoalPose();
-      return motion_utils::calcSignedArcLength(
-        current_lane_path.points, src_point, goal_pose.position);
-    }
-    return motion_utils::calcSignedArcLength(current_lane_path.points, src_point, current_path_end);
+  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 min_dist_to_end_of_current_lane = std::invoke([&]() {
-    auto min_dist_to_end_of_current_lane = std::numeric_limits<double>::max();
-    for (const auto & point : leading_obj_poly.outer()) {
-      const auto obj_p = universe_utils::createPoint(point.x(), point.y(), 0.0);
-      const auto dist = dist_to_path_end(obj_p);
-      min_dist_to_end_of_current_lane = std::min(dist, min_dist_to_end_of_current_lane);
-    }
-    return min_dist_to_end_of_current_lane;
-  });
-
-  // If there are still enough length after the target object, we delay the lane change
-  if (min_dist_to_end_of_current_lane <= common_data_ptr->transient_data.current_dist_buffer.min) {
-    return true;
-  }
-
-  const auto current_pose = common_data_ptr->get_ego_pose();
-  const auto dist_ego_to_obj = motion_utils::calcSignedArcLength(
-    current_lane_path.points, current_pose.position, leading_obj.initial_pose.position);
-
-  if (dist_ego_to_obj < lane_change_path.info.length.lane_changing) {
-    return true;
-  }
-
-  debug.second.unsafe_reason = "delay lane change";
-  utils::path_safety_checker::updateCollisionCheckDebugMap(object_debug, debug, false);
-  return false;
-}
-
-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 auto & path = lane_change_path.path;
-
-  if (path.points.empty() || objects.empty()) {
-    return {};
-  }
-
-  const auto & lane_change_start = lane_change_path.info.lane_changing_start;
-  const auto & path_end = path.points.back();
-
-  double dist_lc_start_to_leading_obj = 0.0;
-  std::optional<size_t> leading_obj_idx = std::nullopt;
-  for (size_t obj_idx = 0; obj_idx < objects.size(); ++obj_idx) {
-    const auto & obj = objects.at(obj_idx);
-    const auto & obj_pose = obj.initial_pose;
+  const auto ego_vel = common_data_ptr->get_ego_speed();
+  const auto min_lon_acc = common_data_ptr->lc_param_ptr->trajectory.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 > gap_threshold;
+  };
 
-    // ignore non-static object
-    // TODO(shimizu): parametrize threshold
-    const double obj_vel_norm = std::hypot(obj.initial_twist.linear.x, obj.initial_twist.linear.y);
-    if (obj_vel_norm > 1.0) {
-      continue;
-    }
+  const auto & delay_lc_param = common_data_ptr->lc_param_ptr->delay;
 
-    // ignore non-parked object
-    if (!isParkedObject(
-          path, route_handler, obj, object_check_min_road_shoulder_width,
-          object_shiftable_ratio_threshold)) {
-      continue;
-    }
+  auto it = target_objects.begin();
+  for (; it < target_objects.end(); ++it) {
+    if (is_near_end(*it)) break;
 
-    const double dist_back_to_obj = autoware::motion_utils::calcSignedArcLength(
-      path.points, path_end.point.pose.position, obj_pose.position);
-    if (dist_back_to_obj > 0.0) {
-      // object is not on the lane change path
-      continue;
-    }
+    if (it->dist_from_ego < lane_change_path.info.length.lane_changing) continue;
 
-    const double dist_lc_start_to_obj = autoware::motion_utils::calcSignedArcLength(
-      path.points, lane_change_start.position, obj_pose.position);
-    if (dist_lc_start_to_obj < 0.0) {
-      // object is on the lane changing path or behind it. It will be detected in safety check
+    if (
+      delay_lc_param.check_only_parked_vehicle &&
+      !isParkedObject(
+        lane_change_path.path, *common_data_ptr->route_handler_ptr, *it,
+        delay_lc_param.min_road_shoulder_width, delay_lc_param.th_parked_vehicle_shift_ratio)) {
       continue;
     }
 
-    if (dist_lc_start_to_obj > dist_lc_start_to_leading_obj) {
-      dist_lc_start_to_leading_obj = dist_lc_start_to_obj;
-      leading_obj_idx = obj_idx;
+    auto next_it = std::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);
+      return true;
     }
   }
 
-  return leading_obj_idx;
+  return false;
 }
 
 lanelet::BasicPolygon2d create_polygon(