fix(lane_change): fix abort distance enough check (#8979)

* RT1-7991 fix abort distance enough check Signed-off-by: Zulfaqar Azmi <[email protected]> * RT-7991 remove unused function Signed-off-by: Zulfaqar Azmi <[email protected]> --------- Signed-off-by: Zulfaqar Azmi <[email protected]>
autowarefoundation · Sep 27, 2024 · 21f547d · 21f547d
1 parent f77acfe
commit 21f547d
Show file tree

Hide file tree

Showing 3 changed files with 6 additions and 25 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
@@ -121,9 +121,6 @@ std::vector<DrivableLanes> generateDrivableLanes(
 
 double getLateralShift(const LaneChangePath & path);
 
-bool hasEnoughLengthToLaneChangeAfterAbort(
-  const CommonDataPtr & common_data_ptr, const double abort_return_dist);
-
 CandidateOutput assignToCandidate(
   const LaneChangePath & lane_change_path, const Point & ego_position);
 std::optional<lanelet::ConstLanelet> getLaneChangeTargetLane(

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
@@ -1963,8 +1963,12 @@ bool NormalLaneChange::calcAbortPath()
     return false;
   }
 
-  if (!utils::lane_change::hasEnoughLengthToLaneChangeAfterAbort(
-        common_data_ptr_, abort_return_dist)) {
+  const auto enough_abort_dist =
+    abort_start_dist + abort_return_dist +
+      calculation::calc_stopping_distance(common_data_ptr_->lc_param_ptr) <=
+    common_data_ptr_->transient_data.dist_to_terminal_start;
+
+  if (!enough_abort_dist) {
     RCLCPP_ERROR(logger_, "insufficient distance to abort.");
     return false;
   }

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
@@ -548,26 +548,6 @@ double getLateralShift(const LaneChangePath & path)
   return path.shifted_path.shift_length.at(end_idx) - path.shifted_path.shift_length.at(start_idx);
 }
 
-bool hasEnoughLengthToLaneChangeAfterAbort(
-  const CommonDataPtr & common_data_ptr, const double abort_return_dist)
-{
-  const auto & current_lanes = common_data_ptr->lanes_ptr->current;
-  if (current_lanes.empty()) {
-    return false;
-  }
-
-  const auto & current_pose = common_data_ptr->get_ego_pose();
-  const auto abort_plus_lane_change_length =
-    abort_return_dist + common_data_ptr->transient_data.current_dist_buffer.min;
-  if (abort_plus_lane_change_length > utils::getDistanceToEndOfLane(current_pose, current_lanes)) {
-    return false;
-  }
-
-  const auto goal_pose = common_data_ptr->route_handler_ptr->getGoalPose();
-  return abort_plus_lane_change_length <=
-         utils::getSignedDistance(current_pose, goal_pose, current_lanes);
-}
-
 std::vector<std::vector<int64_t>> getSortedLaneIds(
   const RouteHandler & route_handler, const Pose & current_pose,
   const lanelet::ConstLanelets & current_lanes, const lanelet::ConstLanelets & target_lanes)