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

…(for RT0-33952) (autowarefoundation#1626) fix(lane_change): delay lane change cancel (autowarefoundation#8048) RT1-6955: delay lane change cancel Signed-off-by: Zulfaqar Azmi <[email protected]>
saka1-s · Nov 7, 2024 · 09e4351 · 09e4351
1 parent f18e714
commit 09e4351
Show file tree

Hide file tree

Showing 2 changed files with 24 additions and 6 deletions.
diff --git 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
@@ -991,22 +991,22 @@ 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);
   auto debug = utils::path_safety_checker::createObjectDebug(leading_obj);
   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(