diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/start_planner/shift_pull_out.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/start_planner/shift_pull_out.hpp index 7928938ac4d5b..2042593064c51 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/start_planner/shift_pull_out.hpp +++ b/planning/behavior_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 index 4561062abfa73..ef6d55a7af4c6 100644 --- 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 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) {