fix when prepare length is insufficient

Signed-off-by: Muhammad Zulfaqar Azmi <zulfaqar.azmi@tier4.jp>

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 @@ void LaneChangeModuleManager::updateModuleParams(const std::vector<rclcpp::Param
     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

@@ -1417,6 +1417,13 @@ bool NormalLaneChange::getLaneChangePaths(
         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) {
@@ -1488,6 +1495,19 @@ bool NormalLaneChange::getLaneChangePaths(
             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!!");