From 09e4351033aed416f800237e8ac2b2e150116ca4 Mon Sep 17 00:00:00 2001
From: Zulfaqar Azmi <93502286+zulfaqar-azmi-t4@users.noreply.github.com>
Date: Thu, 7 Nov 2024 19:40:14 +0900
Subject: [PATCH] fix(lane_change): delay lane change cancel (#8048)  (for
 RT0-33952) (#1626)

fix(lane_change): delay lane change cancel (#8048)

RT1-6955: delay lane change cancel

Signed-off-by: Zulfaqar Azmi <zulfaqar.azmi@tier4.jp>
---
 .../src/scene.cpp                             | 20 ++++++++++++++++++-
 .../src/utils/utils.cpp                       | 10 +++++-----
 2 files changed, 24 insertions(+), 6 deletions(-)

diff --git a/planning/behavior_path_lane_change_module/src/scene.cpp b/planning/behavior_path_lane_change_module/src/scene.cpp
index 69c4a939767bb..373568549b98d 100644
--- a/planning/behavior_path_lane_change_module/src/scene.cpp
+++ b/planning/behavior_path_lane_change_module/src/scene.cpp
@@ -1405,7 +1405,7 @@ bool NormalLaneChange::getLaneChangePaths(
         std::vector<ExtendedPredictedObject> filtered_objects =
           filterObjectsInTargetLane(target_objects, target_lanes);
         if (
-          !is_stuck && utils::lane_change::passParkedObject(
+          !is_stuck && !utils::lane_change::passParkedObject(
                          route_handler, *candidate_path, filtered_objects, lane_change_buffer,
                          is_goal_in_route, *lane_change_parameters_, object_debug_)) {
           debug_print(
@@ -1453,6 +1453,24 @@ PathSafetyStatus NormalLaneChange::isApprovedPathSafe() const
   }
   CollisionCheckDebugMap debug_data;
   const bool is_stuck = isVehicleStuck(current_lanes);
+
+  const auto & route_handler = *getRouteHandler();
+  const auto & lc_param = *lane_change_parameters_;
+
+  const auto min_lc_length = utils::lane_change::calcMinimumLaneChangeLength(
+    *lane_change_parameters_,
+    route_handler.getLateralIntervalsToPreferredLane(current_lanes.back()));
+  const auto is_goal_in_route = route_handler.isInGoalRouteSection(current_lanes.back());
+
+  const auto has_passed_parked_objects = utils::lane_change::passParkedObject(
+    route_handler, path, target_objects.target_lane, min_lc_length, is_goal_in_route, lc_param,
+    debug_data);
+
+  if (!has_passed_parked_objects) {
+    RCLCPP_DEBUG(logger_, "Lane change has been delayed.");
+    return {false, false};
+  }
+
   const auto safety_status = isLaneChangePathSafe(
     path, target_objects, lane_change_parameters_->rss_params_for_abort, is_stuck, debug_data);
   {
diff --git a/planning/behavior_path_lane_change_module/src/utils/utils.cpp b/planning/behavior_path_lane_change_module/src/utils/utils.cpp
index 56cb451ce8f14..39557200eac2e 100644
--- a/planning/behavior_path_lane_change_module/src/utils/utils.cpp
+++ b/planning/behavior_path_lane_change_module/src/utils/utils.cpp
@@ -991,14 +991,14 @@ bool passParkedObject(
     route_handler.getCenterLinePath(current_lanes, 0.0, std::numeric_limits<double>::max());
 
   if (objects.empty() || path.points.empty() || current_lane_path.points.empty()) {
-    return false;
+    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 false;
+    return true;
   }
 
   const auto & leading_obj = objects.at(*leading_obj_idx);
@@ -1006,7 +1006,7 @@ bool passParkedObject(
   const auto leading_obj_poly =
     tier4_autoware_utils::toPolygon2d(leading_obj.initial_pose.pose, leading_obj.shape);
   if (leading_obj_poly.outer().empty()) {
-    return false;
+    return true;
   }
 
   const auto & current_path_end = current_lane_path.points.back().point.pose.position;
@@ -1028,10 +1028,10 @@ bool passParkedObject(
   if (min_dist_to_end_of_current_lane > minimum_lane_change_length) {
     debug.second.unsafe_reason = "delay lane change";
     utils::path_safety_checker::updateCollisionCheckDebugMap(object_debug, debug, false);
-    return true;
+    return false;
   }
 
-  return false;
+  return true;
 }
 
 std::optional<size_t> getLeadingStaticObjectIdx(