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

RT1-6955: delay lane change cancel Signed-off-by: Zulfaqar Azmi <[email protected]> Signed-off-by: Muhammad Zulfaqar Azmi <[email protected]>
tier4 · Aug 13, 2024 · ab60a5b · ab60a5b
1 parent 3e4d2d0
commit ab60a5b
Show file tree

Hide file tree

Showing 3 changed files with 28 additions and 15 deletions.
diff --git a/...path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/utils.hpp b/...path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/utils.hpp
@@ -178,10 +178,10 @@ bool isParkedObject(
   const ExtendedPredictedObject & object, const double buffer_to_bound,
   const double ratio_threshold);
 
-bool passParkedObject(
+bool passed_parked_objects(
   const CommonDataPtr & common_data_ptr, const LaneChangePath & lane_change_path,
   const std::vector<ExtendedPredictedObject> & objects, const double minimum_lane_change_length,
-  const bool is_goal_in_route, CollisionCheckDebugMap & object_debug);
+  CollisionCheckDebugMap & object_debug);
 
 std::optional<size_t> getLeadingStaticObjectIdx(
   const RouteHandler & route_handler, const LaneChangePath & lane_change_path,

diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/scene.cpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/scene.cpp
@@ -1628,10 +1628,9 @@ bool NormalLaneChange::getLaneChangePaths(
         candidate_paths->push_back(*candidate_path);
 
         if (
-          !is_stuck &&
-          utils::lane_change::passParkedObject(
-            common_data_ptr_, *candidate_path, filtered_objects.target_lane, lane_change_buffer,
-            is_goal_in_route, lane_change_debug_.collision_check_objects)) {
+          !is_stuck && !utils::lane_change::passed_parked_objects(
+                         common_data_ptr_, *candidate_path, filtered_objects.target_lane,
+                         lane_change_buffer, lane_change_debug_.collision_check_objects)) {
           debug_print_lat(
             "Reject: parking vehicle exists in the target lane, and the ego is not in stuck. Skip "
             "lane change.");
@@ -1815,6 +1814,19 @@ PathSafetyStatus NormalLaneChange::isApprovedPathSafe() const
   const auto target_objects = getTargetObjects(filtered_objects, current_lanes);
 
   CollisionCheckDebugMap debug_data;
+
+  const auto min_lc_length = utils::lane_change::calcMinimumLaneChangeLength(
+    *lane_change_parameters_,
+    common_data_ptr_->route_handler_ptr->getLateralIntervalsToPreferredLane(current_lanes.back()));
+
+  const auto has_passed_parked_objects = utils::lane_change::passed_parked_objects(
+    common_data_ptr_, path, filtered_objects.target_lane, min_lc_length, 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, debug_data);
   {

diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/utils.cpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/utils.cpp
@@ -968,43 +968,44 @@ bool isParkedObject(
   return clamped_ratio > ratio_threshold;
 }
 
-bool passParkedObject(
+bool passed_parked_objects(
   const CommonDataPtr & common_data_ptr, const LaneChangePath & lane_change_path,
   const std::vector<ExtendedPredictedObject> & objects, const double minimum_lane_change_length,
-  const bool is_goal_in_route, CollisionCheckDebugMap & object_debug)
+  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.object_shiftable_ratio_threshold;
-  const auto & path = lane_change_path.path;
   const auto & current_lanes = common_data_ptr->lanes_ptr->current;
   const auto current_lane_path =
     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;
+  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 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 =
     autoware::universe_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;
   double min_dist_to_end_of_current_lane = std::numeric_limits<double>::max();
+  const auto & target_lanes = common_data_ptr->lanes_ptr->target;
+  const auto is_goal_in_route = route_handler.isInGoalRouteSection(target_lanes.back());
   for (const auto & point : leading_obj_poly.outer()) {
     const auto obj_p = autoware::universe_utils::createPoint(point.x(), point.y(), 0.0);
     const double dist = autoware::motion_utils::calcSignedArcLength(
@@ -1022,10 +1023,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(