fix(lane_change): fix abort distance enough check (autowarefoundation…

…#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]>
tier4 · Nov 21, 2024 · ecbd383 · ecbd383
1 parent ad36fc1
commit ecbd383
Show file tree

Hide file tree

Showing 4 changed files with 16 additions and 57 deletions.
diff --git a/...avior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_planner_module.cpp b/...avior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_planner_module.cpp
@@ -1542,8 +1542,7 @@ BehaviorModuleOutput GoalPlannerModule::planPullOverAsOutput()
 
   // return to lane parking if it is possible
   if (
-    is_freespace &&
-    thread_safe_data_.getPullOverPlannerType() == PullOverPlannerType::FREESPACE &&
+    is_freespace && thread_safe_data_.getPullOverPlannerType() == PullOverPlannerType::FREESPACE &&
     canReturnToLaneParking()) {
     thread_safe_data_.set_pull_over_path(thread_safe_data_.get_lane_parking_pull_over_path());
   }

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
@@ -141,17 +141,6 @@ std::vector<DrivableLanes> generateDrivableLanes(
 
 double getLateralShift(const LaneChangePath & path);
 
-bool hasEnoughLengthToLaneChangeAfterAbort(
-  const std::shared_ptr<RouteHandler> & route_handler, const lanelet::ConstLanelets & current_lanes,
-  const Pose & curent_pose, const double abort_return_dist,
-  const LaneChangeParameters & lane_change_parameters, const Direction direction);
-
-double calcLateralBufferForFiltering(const double vehicle_width, const double lateral_buffer = 0.0);
-
-double calcLateralBufferForFiltering(const double vehicle_width, const double lateral_buffer);
-
-std::string getStrDirection(const std::string & name, const Direction direction);
-
 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
@@ -2034,9 +2034,21 @@ bool NormalLaneChange::calcAbortPath()
     return false;
   }
 
-  if (!utils::lane_change::hasEnoughLengthToLaneChangeAfterAbort(
-        route_handler, reference_lanelets, current_pose, abort_return_dist,
-        *lane_change_parameters_, direction)) {
+  const auto dist_to_terminal_start = std::invoke([&]() {
+    const auto dist_to_terminal_end = calculation::calc_dist_from_pose_to_terminal_end(
+      common_data_ptr_, get_current_lanes(), common_data_ptr_->get_ego_pose());
+    const auto min_lc_length = utils::lane_change::calcMinimumLaneChangeLength(
+      route_handler, get_current_lanes().back(), *common_data_ptr_->lc_param_ptr,
+      common_data_ptr_->direction);
+    return dist_to_terminal_end - min_lc_length;
+  });
+
+  const auto enough_abort_dist =
+    abort_start_dist + abort_return_dist +
+      calculation::calc_stopping_distance(common_data_ptr_->lc_param_ptr) <=
+    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
@@ -644,47 +644,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 std::shared_ptr<RouteHandler> & route_handler, const lanelet::ConstLanelets & current_lanes,
-  const Pose & current_pose, const double abort_return_dist,
-  const LaneChangeParameters & lane_change_parameters, const Direction direction)
-{
-  if (current_lanes.empty()) {
-    return false;
-  }
-
-  const auto minimum_lane_change_length = calcMinimumLaneChangeLength(
-    route_handler, current_lanes.back(), lane_change_parameters, direction);
-  const auto abort_plus_lane_change_length = abort_return_dist + minimum_lane_change_length;
-  if (abort_plus_lane_change_length > utils::getDistanceToEndOfLane(current_pose, current_lanes)) {
-    return false;
-  }
-
-  if (
-    abort_plus_lane_change_length >
-    utils::getSignedDistance(current_pose, route_handler->getGoalPose(), current_lanes)) {
-    return false;
-  }
-
-  return true;
-}
-
-double calcLateralBufferForFiltering(const double vehicle_width, const double lateral_buffer)
-{
-  return lateral_buffer + 0.5 * vehicle_width;
-}
-
-std::string getStrDirection(const std::string & name, const Direction direction)
-{
-  if (direction == Direction::LEFT) {
-    return name + "_left";
-  }
-  if (direction == Direction::RIGHT) {
-    return name + "_right";
-  }
-  return "";
-}
-
 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)