fix(lane_change): skip generating path if longitudinal distance difference is less than threshold

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

@@ -26,6 +26,11 @@
       min_longitudinal_acc: -1.0
       max_longitudinal_acc: 1.0
 
+      skip_process:
+        longitudinal_distance_diff_threshold:
+          prepare: 0.5
+          lane_changing: 0.5
+
       # 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/data_structs.hpp

@@ -124,6 +124,9 @@ struct Parameters
   double min_longitudinal_acc{-1.0};
   double max_longitudinal_acc{1.0};
 
+  double skip_process_lon_diff_th_prepare{0.5};
+  double skip_process_lon_diff_th_lane_changing{1.0};
+
   // collision check
   bool enable_collision_check_for_prepare_phase_in_general_lanes{false};
   bool enable_collision_check_for_prepare_phase_in_intersection{true};

planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/manager.cpp

@@ -354,6 +354,13 @@
     updateParam<double>(parameters, ns + "max_longitudinal_acc", p->max_longitudinal_acc);
   }
 
+  {
+    const std::string ns = "lane_change.skip_process.longitudinal_distance_diff_threshold.";
+    updateParam<double>(parameters, ns + "prepare", p->skip_process_lon_diff_th_prepare);
+    updateParam<double>(
+      parameters, ns + "lane_changing", p->skip_process_lon_diff_th_lane_changing);
+  }
+
   {
     const std::string ns = "lane_change.safety_check.lane_expansion.";
     updateParam<double>(parameters, ns + "left_offset", p->lane_expansion_left_offset);

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.
@@ -1417,77 +1417,97 @@
         continue;
       }
 
+      if (!candidate_paths->empty()) {
+        const auto prev_prep_diff = candidate_paths->back().info.length.prepare - prepare_length;
+        if (std::abs(prev_prep_diff) < lane_change_parameters_->skip_process_lon_diff_th_prepare) {
+          RCLCPP_DEBUG(logger_, "Skip: Change in prepare length is less than threshold.");
+          continue;
+        }
+      }
       auto prepare_segment = getPrepareSegment(current_lanes, backward_path_length, prepare_length);
 
       const auto debug_print = [&](const auto & s) {
         RCLCPP_DEBUG(
           logger_, "%s | prep_time: %.5f | lon_acc sampled: %.5f, actual: %.5f | prep_len: %.5f", s,
           prepare_duration, sampled_longitudinal_acc, longitudinal_acc_on_prepare, prepare_length);
       };
 
       if (prepare_segment.points.empty()) {
         debug_print("prepare segment is empty...? Unexpected.");
         continue;
       }
 
       if (!is_valid_start_point(common_data_ptr_, prepare_segment.points.back().point.pose)) {
         debug_print(
           "Reject: lane changing start point is not within the preferred lanes or its neighbors");
         continue;
       }
 
       // lane changing start getEgoPose() is at the end of prepare segment
       const auto & lane_changing_start_pose = prepare_segment.points.back().point.pose;
       const auto target_length_from_lane_change_start_pose = utils::getArcLengthToTargetLanelet(
         current_lanes, target_lanes.front(), lane_changing_start_pose);
 
       // Check if the lane changing start point is not on the lanes next to target lanes,
       if (target_length_from_lane_change_start_pose > 0.0) {
         debug_print("lane change start getEgoPose() is behind target lanelet!");
         break;
       }
 
       const auto shift_length =
         lanelet::utils::getLateralDistanceToClosestLanelet(target_lanes, lane_changing_start_pose);
 
       const auto initial_lane_changing_velocity = prepare_velocity;
       const auto max_path_velocity = prepare_segment.points.back().point.longitudinal_velocity_mps;
 
       // get lateral acceleration range
       const auto [min_lateral_acc, max_lateral_acc] =
         lane_change_parameters_->lane_change_lat_acc_map.find(initial_lane_changing_velocity);
       const auto lateral_acc_resolution =
         std::abs(max_lateral_acc - min_lateral_acc) / lateral_acc_sampling_num;
 
       std::vector<double> sample_lat_acc;
       constexpr double eps = 0.01;
       for (double a = min_lateral_acc; a < max_lateral_acc + eps; a += lateral_acc_resolution) {
         sample_lat_acc.push_back(a);
       }
       RCLCPP_DEBUG(logger_, "  -  sampling num for lat_acc: %lu", sample_lat_acc.size());
 
       debug_print("Prepare path satisfy constraints");
       const auto dist_lc_start_to_end_of_lanes = calculation::calc_dist_from_pose_to_terminal_end(
         common_data_ptr_, common_data_ptr_->lanes_ptr->target_neighbor, lane_changing_start_pose);
 
       for (const auto & lateral_acc : sample_lat_acc) {
         const auto lane_changing_time = PathShifter::calcShiftTimeFromJerk(
           shift_length, lane_change_parameters_->lane_changing_lateral_jerk, lateral_acc);
         const double longitudinal_acc_on_lane_changing =
           utils::lane_change::calcLaneChangingAcceleration(
             initial_lane_changing_velocity, max_path_velocity, lane_changing_time,
             sampled_longitudinal_acc);
         const auto lane_changing_length = utils::lane_change::calcPhaseLength(
           initial_lane_changing_velocity, getCommonParam().max_vel,
           longitudinal_acc_on_lane_changing, lane_changing_time);
 
         const auto debug_print_lat = [&](const auto & s) {
           RCLCPP_DEBUG(
             logger_,
             "    -  %s | lc_time: %.5f | lon_acc sampled: %.5f, actual: %.5f | lc_len: %.5f", s,
             lane_changing_time, sampled_longitudinal_acc, longitudinal_acc_on_lane_changing,
             lane_changing_length);
         };
+        if (!candidate_paths->empty()) {
+          const auto prev_prep_diff = candidate_paths->back().info.length.prepare - prepare_length;
+          const auto lc_length_diff =
+            candidate_paths->back().info.length.lane_changing - lane_changing_length;
+
+          if (
+            std::abs(prev_prep_diff) < eps &&
+            std::abs(lc_length_diff) <
+              lane_change_parameters_->skip_process_lon_diff_th_lane_changing) {
+            RCLCPP_DEBUG(logger_, "Skip: Change in lane changing length is less than threshold.");
+            continue;
+          }
+        }
 
         if (lane_changing_length + prepare_length > dist_to_end_of_current_lanes) {
           debug_print_lat("Reject: length of lane changing path is longer than length to goal!!");