fix(start_planner): remove shift pull out enough distance check (#4422)

Signed-off-by: kosuke55 <kosuke.tnp@gmail.com>
autowarefoundation · Jul 30, 2023 · 8a82fa9 · 8a82fa9
1 parent 86e0a01
commit 8a82fa9
Show file tree

Hide file tree

Showing 2 changed files with 0 additions and 31 deletions.
diff --git a/...ehavior_path_planner/include/behavior_path_planner/utils/start_planner/shift_pull_out.hpp b/...ehavior_path_planner/include/behavior_path_planner/utils/start_planner/shift_pull_out.hpp
@@ -45,10 +45,6 @@ class ShiftPullOut : public PullOutPlannerBase
     const BehaviorPathPlannerParameters & common_parameter,
     const behavior_path_planner::StartPlannerParameters & parameter);
 
-  bool hasEnoughDistance(
-    const double pull_out_total_distance, const lanelet::ConstLanelets & current_lanes,
-    const Pose & current_pose, const bool isInGoalRouteSection, const Pose & goal_pose);
-
   double calcBeforeShiftedArcLength(
     const PathWithLaneId & path, const double target_after_arc_length, const double dr);
 

diff --git a/planning/behavior_path_planner/src/utils/start_planner/shift_pull_out.cpp b/planning/behavior_path_planner/src/utils/start_planner/shift_pull_out.cpp
@@ -216,14 +216,6 @@ std::vector<PullOutPath> ShiftPullOut::calcPullOutPaths(
     const double before_shifted_pull_out_distance =
       std::max(pull_out_distance, pull_out_distance_converted);
 
-    // check has enough distance
-    const bool is_in_goal_route_section = route_handler.isInGoalRouteSection(road_lanes.back());
-    if (!hasEnoughDistance(
-          before_shifted_pull_out_distance, road_lanes, start_pose, is_in_goal_route_section,
-          goal_pose)) {
-      continue;
-    }
-
     // if before_shifted_pull_out_distance is too short, shifting path fails, so add non shifted
     if (before_shifted_pull_out_distance < RESAMPLE_INTERVAL && !has_non_shifted_path) {
       candidate_paths.push_back(non_shifted_path);
@@ -319,25 +311,6 @@ double ShiftPullOut::calcPullOutLongitudinalDistance(
   return min_pull_out_distance;
 }
 
-bool ShiftPullOut::hasEnoughDistance(
-  const double pull_out_total_distance, const lanelet::ConstLanelets & road_lanes,
-  const Pose & current_pose, const bool is_in_goal_route_section, const Pose & goal_pose)
-{
-  // the goal is far so current_lanes do not include goal's lane
-  if (pull_out_total_distance > utils::getDistanceToEndOfLane(current_pose, road_lanes)) {
-    return false;
-  }
-
-  // current_lanes include goal's lane
-  if (
-    is_in_goal_route_section &&
-    pull_out_total_distance > utils::getSignedDistance(current_pose, goal_pose, road_lanes)) {
-    return false;
-  }
-
-  return true;
-}
-
 double ShiftPullOut::calcBeforeShiftedArcLength(
   const PathWithLaneId & path, const double target_after_arc_length, const double dr)
 {