diff --git a/planning/behavior_path_avoidance_module/config/avoidance.param.yaml b/planning/behavior_path_avoidance_module/config/avoidance.param.yaml
index 2d38cebd591f..b300de2236fc 100644
--- a/planning/behavior_path_avoidance_module/config/avoidance.param.yaml
+++ b/planning/behavior_path_avoidance_module/config/avoidance.param.yaml
@@ -122,7 +122,8 @@
           motorcycle: true                               # [-]
           pedestrian: true                               # [-]
         # detection range
-        object_check_goal_distance: 20.0                # [m]
+        object_check_goal_distance: 20.0                 # [m]
+        object_check_return_pose_distance: 20.0          # [m]
         # filtering parking objects
         threshold_distance_object_is_on_center: 1.0     # [m]
         object_check_shiftable_ratio: 0.6               # [-]
diff --git a/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/data_structs.hpp b/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/data_structs.hpp
index 86e160850183..9b5ae3cb4db9 100644
--- a/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/data_structs.hpp
+++ b/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/data_structs.hpp
@@ -164,10 +164,14 @@ struct AvoidanceParameters
   double object_check_backward_distance{0.0};
   double object_check_yaw_deviation{0.0};
 
-  // if the distance between object and goal position is less than this parameter, the module ignore
-  // the object.
+  // if the distance between object and goal position is less than this parameter, the module do not
+  // return center line.
   double object_check_goal_distance{0.0};
 
+  // if the distance between object and return position is less than this parameter, the module do
+  // not return center line.
+  double object_check_return_pose_distance{0.0};
+
   // use in judge whether the vehicle is parking object on road shoulder
   double object_check_shiftable_ratio{0.0};
 
@@ -462,14 +466,14 @@ using AvoidLineArray = std::vector<AvoidLine>;
 
 struct AvoidOutline
 {
-  AvoidOutline(AvoidLine avoid_line, AvoidLine return_line)
+  AvoidOutline(AvoidLine avoid_line, const std::optional<AvoidLine> return_line)
   : avoid_line{std::move(avoid_line)}, return_line{std::move(return_line)}
   {
   }
 
   AvoidLine avoid_line{};
 
-  AvoidLine return_line{};
+  std::optional<AvoidLine> return_line{};
 
   AvoidLineArray middle_lines{};
 };
diff --git a/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/parameter_helper.hpp b/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/parameter_helper.hpp
index b9af50ce76eb..9f2fdf7ab96d 100644
--- a/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/parameter_helper.hpp
+++ b/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/parameter_helper.hpp
@@ -122,6 +122,8 @@ AvoidanceParameters getParameter(rclcpp::Node * node)
 
     p.object_check_goal_distance =
       getOrDeclareParameter<double>(*node, ns + "object_check_goal_distance");
+    p.object_check_return_pose_distance =
+      getOrDeclareParameter<double>(*node, ns + "object_check_return_pose_distance");
     p.threshold_distance_object_is_on_center =
       getOrDeclareParameter<double>(*node, ns + "threshold_distance_object_is_on_center");
     p.object_check_shiftable_ratio =
diff --git a/planning/behavior_path_avoidance_module/src/shift_line_generator.cpp b/planning/behavior_path_avoidance_module/src/shift_line_generator.cpp
index d16cfe2e15c8..96e22152a2f9 100644
--- a/planning/behavior_path_avoidance_module/src/shift_line_generator.cpp
+++ b/planning/behavior_path_avoidance_module/src/shift_line_generator.cpp
@@ -81,7 +81,9 @@ AvoidLineArray toArray(const AvoidOutlines & outlines)
   AvoidLineArray ret{};
   for (const auto & outline : outlines) {
     ret.push_back(outline.avoid_line);
-    ret.push_back(outline.return_line);
+    if (outline.return_line.has_value()) {
+      ret.push_back(outline.return_line.value());
+    }
 
     std::for_each(
       outline.middle_lines.begin(), outline.middle_lines.end(),
@@ -357,7 +359,30 @@ AvoidOutlines ShiftLineGenerator::generateAvoidOutline(
       al_return.object_on_right = utils::avoidance::isOnRight(o);
     }
 
-    if (is_valid_shift_line(al_avoid) && is_valid_shift_line(al_return)) {
+    const bool skip_return_shift = [&]() {
+      if (!utils::isAllowedGoalModification(data_->route_handler)) {
+        return false;
+      }
+      const auto goal_pose = data_->route_handler->getGoalPose();
+      const double goal_longitudinal_distance =
+        motion_utils::calcSignedArcLength(data.reference_path.points, 0, goal_pose.position);
+      const bool is_return_shift_to_goal =
+        std::abs(al_return.end_longitudinal - goal_longitudinal_distance) <
+        parameters_->object_check_return_pose_distance;
+      if (is_return_shift_to_goal) {
+        return true;
+      }
+      const auto & object_pos = o.object.kinematics.initial_pose_with_covariance.pose.position;
+      const bool has_object_near_goal =
+        tier4_autoware_utils::calcDistance2d(goal_pose.position, object_pos) <
+        parameters_->object_check_goal_distance;
+      return has_object_near_goal;
+    }();
+
+    if (skip_return_shift) {
+      // if the object is close to goal or ego is about to return near GOAL, do not return
+      outlines.emplace_back(al_avoid, std::nullopt);
+    } else if (is_valid_shift_line(al_avoid) && is_valid_shift_line(al_return)) {
       outlines.emplace_back(al_avoid, al_return);
     } else {
       o.reason = "InvalidShiftLine";
@@ -653,35 +678,43 @@ AvoidOutlines ShiftLineGenerator::applyMergeProcess(
     auto & next_outline = outlines.at(i);
 
     const auto & return_line = last_outline.return_line;
-    const auto & avoid_line = next_outline.avoid_line;
+    if (!return_line.has_value()) {
+      ret.push_back(outlines.at(i));
+      break;
+    }
 
-    if (no_conflict(return_line, avoid_line)) {
+    const auto & avoid_line = next_outline.avoid_line;
+    if (no_conflict(return_line.value(), avoid_line)) {
       ret.push_back(outlines.at(i));
       continue;
     }
 
-    const auto merged_shift_line = merge(return_line, avoid_line, generateUUID());
+    const auto merged_shift_line = merge(return_line.value(), avoid_line, generateUUID());
 
     if (!helper_->isComfortable(AvoidLineArray{merged_shift_line})) {
       ret.push_back(outlines.at(i));
       continue;
     }
 
-    if (same_side_shift(return_line, avoid_line)) {
+    if (same_side_shift(return_line.value(), avoid_line)) {
       last_outline.middle_lines.push_back(merged_shift_line);
       last_outline.return_line = next_outline.return_line;
       debug.step1_merged_shift_line.push_back(merged_shift_line);
       continue;
     }
 
-    if (within(return_line, avoid_line.end_idx) && within(avoid_line, return_line.start_idx)) {
+    if (
+      within(return_line.value(), avoid_line.end_idx) &&
+      within(avoid_line, return_line->start_idx)) {
       last_outline.middle_lines.push_back(merged_shift_line);
       last_outline.return_line = next_outline.return_line;
       debug.step1_merged_shift_line.push_back(merged_shift_line);
       continue;
     }
 
-    if (within(return_line, avoid_line.start_idx) && within(avoid_line, return_line.end_idx)) {
+    if (
+      within(return_line.value(), avoid_line.start_idx) &&
+      within(avoid_line, return_line->end_idx)) {
       last_outline.middle_lines.push_back(merged_shift_line);
       last_outline.return_line = next_outline.return_line;
       debug.step1_merged_shift_line.push_back(merged_shift_line);
@@ -702,7 +735,10 @@ AvoidOutlines ShiftLineGenerator::applyFillGapProcess(
 
   for (auto & outline : ret) {
     if (outline.middle_lines.empty()) {
-      const auto new_line = fill(outline.avoid_line, outline.return_line, generateUUID());
+      const auto new_line =
+        outline.return_line.has_value()
+          ? fill(outline.avoid_line, outline.return_line.value(), generateUUID())
+          : outline.avoid_line;
       outline.middle_lines.push_back(new_line);
       debug.step1_filled_shift_line.push_back(new_line);
     }
@@ -717,8 +753,11 @@ AvoidOutlines ShiftLineGenerator::applyFillGapProcess(
 
     helper_->alignShiftLinesOrder(outline.middle_lines, false);
 
-    if (outline.middle_lines.back().end_longitudinal < outline.return_line.start_longitudinal) {
-      const auto new_line = fill(outline.middle_lines.back(), outline.return_line, generateUUID());
+    if (
+      outline.return_line.has_value() &&
+      outline.middle_lines.back().end_longitudinal < outline.return_line->start_longitudinal) {
+      const auto new_line =
+        fill(outline.middle_lines.back(), outline.return_line.value(), generateUUID());
       outline.middle_lines.push_back(new_line);
       debug.step1_filled_shift_line.push_back(new_line);
     }
@@ -989,6 +1028,20 @@ AvoidLineArray ShiftLineGenerator::addReturnShiftLine(
   const bool has_candidate_point = !ret.empty();
   const bool has_registered_point = last_.has_value();
 
+  if (utils::isAllowedGoalModification(data_->route_handler)) {
+    const auto has_object_near_goal =
+      std::any_of(data.target_objects.begin(), data.target_objects.end(), [&](const auto & o) {
+        return tier4_autoware_utils::calcDistance2d(
+                 data_->route_handler->getGoalPose().position,
+                 o.object.kinematics.initial_pose_with_covariance.pose.position) <
+               parameters_->object_check_goal_distance;
+      });
+    if (has_object_near_goal) {
+      RCLCPP_DEBUG(rclcpp::get_logger(""), "object near goal exists so skip adding return shift");
+      return ret;
+    }
+  }
+
   const auto exist_unavoidable_object = std::any_of(
     data.target_objects.begin(), data.target_objects.end(),
     [](const auto & o) { return !o.is_avoidable && o.longitudinal > 0.0; });
@@ -1043,6 +1096,21 @@ AvoidLineArray ShiftLineGenerator::addReturnShiftLine(
     }
   }
 
+  // if last shift line is near the objects, do not add return shift.
+  if (utils::isAllowedGoalModification(data_->route_handler)) {
+    const bool has_last_shift_near_goal =
+      std::any_of(data.target_objects.begin(), data.target_objects.end(), [&](const auto & o) {
+        return tier4_autoware_utils::calcDistance2d(
+                 last_sl.end.position,
+                 o.object.kinematics.initial_pose_with_covariance.pose.position) <
+               parameters_->object_check_goal_distance;
+      });
+    if (has_last_shift_near_goal) {
+      RCLCPP_DEBUG(rclcpp::get_logger(""), "last shift line is near the objects");
+      return ret;
+    }
+  }
+
   // There already is a shift point candidates to go back to center line, but it could be too sharp
   // due to detection noise or timing.
   // Here the return-shift from ego is added for the in case.
@@ -1086,8 +1154,11 @@ AvoidLineArray ShiftLineGenerator::addReturnShiftLine(
     return ret;
   }
 
-  const auto remaining_distance = std::min(
-    arclength_from_ego.back() - parameters_->dead_line_buffer_for_goal, data.to_return_point);
+  const auto remaining_distance =
+    utils::isAllowedGoalModification(data_->route_handler)
+      ? data.to_return_point
+      : std::min(
+          arclength_from_ego.back() - parameters_->dead_line_buffer_for_goal, data.to_return_point);
 
   // If the avoidance point has already been set, the return shift must be set after the point.
   const auto last_sl_distance = data.arclength_from_ego.at(last_sl.end_idx);
diff --git a/planning/behavior_path_avoidance_module/src/utils.cpp b/planning/behavior_path_avoidance_module/src/utils.cpp
index 206c990e6e54..5114bd39c938 100644
--- a/planning/behavior_path_avoidance_module/src/utils.cpp
+++ b/planning/behavior_path_avoidance_module/src/utils.cpp
@@ -710,11 +710,13 @@ bool isSatisfiedWithCommonCondition(
     return false;
   }
 
-  if (
-    object.longitudinal + object.length / 2 + parameters->object_check_goal_distance >
-    to_goal_distance) {
-    object.reason = "TooNearToGoal";
-    return false;
+  if (!utils::isAllowedGoalModification(planner_data->route_handler)) {
+    if (
+      object.longitudinal + object.length / 2 + parameters->object_check_goal_distance >
+      to_goal_distance) {
+      object.reason = "TooNearToGoal";
+      return false;
+    }
   }
 
   return true;
@@ -1718,7 +1720,9 @@ void fillAdditionalInfoFromLongitudinal(
 {
   for (auto & outline : outlines) {
     fillAdditionalInfoFromLongitudinal(data, outline.avoid_line);
-    fillAdditionalInfoFromLongitudinal(data, outline.return_line);
+    if (outline.return_line.has_value()) {
+      fillAdditionalInfoFromLongitudinal(data, outline.return_line.value());
+    }
 
     std::for_each(outline.middle_lines.begin(), outline.middle_lines.end(), [&](auto & line) {
       fillAdditionalInfoFromLongitudinal(data, line);
@@ -2210,7 +2214,9 @@ double calcDistanceToReturnDeadLine(
   }
 
   // dead line for goal
-  if (parameters->enable_dead_line_for_goal) {
+  if (
+    !utils::isAllowedGoalModification(planner_data->route_handler) &&
+    parameters->enable_dead_line_for_goal) {
     if (planner_data->route_handler->isInGoalRouteSection(lanelets.back())) {
       const auto & ego_pos = planner_data->self_odometry->pose.pose.position;
       const auto to_goal_distance =
diff --git a/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/goal_planner_module.hpp b/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/goal_planner_module.hpp
index e90162c958be..9fb9c17f1ca7 100644
--- a/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/goal_planner_module.hpp
+++ b/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/goal_planner_module.hpp
@@ -472,6 +472,10 @@ class GoalPlannerModule : public SceneModuleInterface
   // new turn signal
   TurnSignalInfo calcTurnSignalInfo() const;
 
+  std::optional<BehaviorModuleOutput> last_previous_module_output_{};
+  bool hasPreviousModulePathShapeChanged() const;
+  bool hasDeviatedFromLastPreviousModulePath() const;
+
   // timer for generating pull over path candidates in a separate thread
   void onTimer();
   void onFreespaceParkingTimer();
diff --git a/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/pull_over_planner_base.hpp b/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/pull_over_planner_base.hpp
index ddd7c9399865..9c51623bbf24 100644
--- a/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/pull_over_planner_base.hpp
+++ b/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/pull_over_planner_base.hpp
@@ -119,6 +119,11 @@ class PullOverPlannerBase
   }
   virtual ~PullOverPlannerBase() = default;
 
+  void setPreviousModuleOutput(const BehaviorModuleOutput & previous_module_output)
+  {
+    previous_module_output_ = previous_module_output;
+  }
+
   void setPlannerData(const std::shared_ptr<const PlannerData> planner_data)
   {
     planner_data_ = planner_data;
@@ -132,6 +137,8 @@ class PullOverPlannerBase
   vehicle_info_util::VehicleInfo vehicle_info_;
   LinearRing2d vehicle_footprint_;
   GoalPlannerParameters parameters_;
+
+  BehaviorModuleOutput previous_module_output_;
 };
 }  // namespace behavior_path_planner
 
diff --git a/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/shift_pull_over.hpp b/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/shift_pull_over.hpp
index bd19dc2e87de..892ef7d5dd30 100644
--- a/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/shift_pull_over.hpp
+++ b/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/shift_pull_over.hpp
@@ -43,6 +43,8 @@ class ShiftPullOver : public PullOverPlannerBase
 protected:
   PathWithLaneId generateReferencePath(
     const lanelet::ConstLanelets & road_lanes, const Pose & end_pose) const;
+  std::optional<PathWithLaneId> cropPrevModulePath(
+    const PathWithLaneId & prev_module_path, const Pose & shift_end_pose) const;
   std::optional<PullOverPath> generatePullOverPath(
     const lanelet::ConstLanelets & road_lanes, const lanelet::ConstLanelets & shoulder_lanes,
     const Pose & goal_pose, const double lateral_jerk) const;
diff --git a/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/util.hpp b/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/util.hpp
index 791c35f3afca..a1fd063bd411 100644
--- a/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/util.hpp
+++ b/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/util.hpp
@@ -52,8 +52,21 @@ PredictedObjects filterObjectsByLateralDistance(
   const Pose & ego_pose, const double vehicle_width, const PredictedObjects & objects,
   const double distance_thresh, const bool filter_inside);
 
-bool isAllowedGoalModification(const std::shared_ptr<RouteHandler> & route_handler);
-bool checkOriginalGoalIsInShoulder(const std::shared_ptr<RouteHandler> & route_handler);
+double calcLateralDeviationBetweenPaths(
+  const PathWithLaneId & reference_path, const PathWithLaneId & target_path);
+bool isReferencePath(
+  const PathWithLaneId & reference_path, const PathWithLaneId & target_path,
+  const double lateral_deviation_thresh);
+
+std::optional<PathWithLaneId> cropPath(const PathWithLaneId & path, const Pose & end_pose);
+PathWithLaneId cropForwardPoints(
+  const PathWithLaneId & path, const size_t target_seg_idx, const double forward_length);
+PathWithLaneId extendPath(
+  const PathWithLaneId & prev_module_path, const PathWithLaneId & reference_path,
+  const double extend_length);
+PathWithLaneId extendPath(
+  const PathWithLaneId & prev_module_path, const PathWithLaneId & reference_path,
+  const Pose & extend_pose);
 
 // debug
 MarkerArray createPullOverAreaMarkerArray(
diff --git a/planning/behavior_path_goal_planner_module/src/goal_planner_module.cpp b/planning/behavior_path_goal_planner_module/src/goal_planner_module.cpp
index 2ea6f1e100e9..894577f51b79 100644
--- a/planning/behavior_path_goal_planner_module/src/goal_planner_module.cpp
+++ b/planning/behavior_path_goal_planner_module/src/goal_planner_module.cpp
@@ -136,11 +136,34 @@ void GoalPlannerModule::updateOccupancyGrid()
   occupancy_grid_map_->setMap(*(planner_data_->occupancy_grid));
 }
 
+bool GoalPlannerModule::hasPreviousModulePathShapeChanged() const
+{
+  if (!last_previous_module_output_) {
+    return true;
+  }
+
+  const auto current_path = getPreviousModuleOutput().path;
+
+  // the terminal distance is far
+  return calcDistance2d(
+           last_previous_module_output_->path.points.back().point,
+           current_path.points.back().point) > 0.3;
+}
+
+bool GoalPlannerModule::hasDeviatedFromLastPreviousModulePath() const
+{
+  if (!last_previous_module_output_) {
+    return true;
+  }
+  return std::abs(motion_utils::calcLateralOffset(
+           last_previous_module_output_->path.points,
+           planner_data_->self_odometry->pose.pose.position)) > 0.3;
+}
+
 // generate pull over candidate paths
 void GoalPlannerModule::onTimer()
 {
-  // already generated pull over candidate paths
-  if (!thread_safe_data_.get_pull_over_path_candidates().empty()) {
+  if (getCurrentStatus() == ModuleStatus::IDLE) {
     return;
   }
 
@@ -149,16 +172,30 @@ void GoalPlannerModule::onTimer()
     return;
   }
 
-  if (
-    !planner_data_ ||
-    !goal_planner_utils::isAllowedGoalModification(planner_data_->route_handler)) {
+  if (!planner_data_ || !utils::isAllowedGoalModification(planner_data_->route_handler)) {
     return;
   }
 
-  if (getCurrentStatus() == ModuleStatus::IDLE) {
+  // check if new pull over path candidates are needed to be generated
+  const bool need_update = std::invoke([&]() {
+    if (thread_safe_data_.get_pull_over_path_candidates().empty()) {
+      return true;
+    }
+    if (hasPreviousModulePathShapeChanged()) {
+      RCLCPP_ERROR(getLogger(), "has previous module path shape changed");
+      return true;
+    }
+    if (hasDeviatedFromLastPreviousModulePath() && !hasDecidedPath()) {
+      RCLCPP_ERROR(getLogger(), "has deviated from last previous module path");
+      return true;
+    }
+    return false;
+  });
+  if (!need_update) {
     return;
   }
 
+  const auto previous_module_output = getPreviousModuleOutput();
   const auto goal_candidates = thread_safe_data_.get_goal_candidates();
 
   // generate valid pull over path candidates and calculate closest start pose
@@ -173,8 +210,9 @@ void GoalPlannerModule::onTimer()
                                     const std::shared_ptr<PullOverPlannerBase> & planner,
                                     const GoalCandidate & goal_candidate) {
     planner->setPlannerData(planner_data_);
+    planner->setPreviousModuleOutput(previous_module_output);
     auto pull_over_path = planner->plan(goal_candidate.goal_pose);
-    if (pull_over_path && isCrossingPossible(*pull_over_path)) {
+    if (pull_over_path) {
       pull_over_path->goal_id = goal_candidate.id;
       pull_over_path->id = path_candidates.size();
       path_candidates.push_back(*pull_over_path);
@@ -188,9 +226,21 @@ void GoalPlannerModule::onTimer()
       }
     }
   };
+
+  // todo: currently non centerline input path is supported only by shift pull over
+  const bool is_center_line_input_path = goal_planner_utils::isReferencePath(
+    previous_module_output.reference_path, previous_module_output.path, 0.1);
+  RCLCPP_DEBUG(
+    getLogger(), "the input path of pull over planner is center line: %d",
+    is_center_line_input_path);
+
   // plan candidate paths and set them to the member variable
   if (parameters_->path_priority == "efficient_path") {
     for (const auto & planner : pull_over_planners_) {
+      // todo: temporary skip NON SHIFT planner when input path is not center line
+      if (!is_center_line_input_path && planner->getPlannerType() != PullOverPlannerType::SHIFT) {
+        continue;
+      }
       for (const auto & goal_candidate : goal_candidates) {
         planCandidatePaths(planner, goal_candidate);
       }
@@ -198,6 +248,10 @@ void GoalPlannerModule::onTimer()
   } else if (parameters_->path_priority == "close_goal") {
     for (const auto & goal_candidate : goal_candidates) {
       for (const auto & planner : pull_over_planners_) {
+        // todo: temporary skip NON SHIFT planner when input path is not center line
+        if (!is_center_line_input_path && planner->getPlannerType() != PullOverPlannerType::SHIFT) {
+          continue;
+        }
         planCandidatePaths(planner, goal_candidate);
       }
     }
@@ -213,7 +267,10 @@ void GoalPlannerModule::onTimer()
     const std::lock_guard<std::recursive_mutex> lock(mutex_);
     thread_safe_data_.set_pull_over_path_candidates(path_candidates);
     thread_safe_data_.set_closest_start_pose(closest_start_pose);
+    RCLCPP_INFO(getLogger(), "generated %lu pull over path candidates", path_candidates.size());
   }
+
+  last_previous_module_output_ = previous_module_output;
 }
 
 void GoalPlannerModule::onFreespaceParkingTimer()
@@ -225,7 +282,7 @@ void GoalPlannerModule::onFreespaceParkingTimer()
     return;
   }
   // fixed goal planner do not use freespace planner
-  if (!goal_planner_utils::isAllowedGoalModification(planner_data_->route_handler)) {
+  if (!utils::isAllowedGoalModification(planner_data_->route_handler)) {
     return;
   }
 
@@ -354,7 +411,9 @@ bool GoalPlannerModule::isExecutionRequested() const
 
   // check if goal_pose is in current_lanes.
   lanelet::ConstLanelet current_lane{};
-  const lanelet::ConstLanelets current_lanes = utils::getCurrentLanes(planner_data_);
+  // const lanelet::ConstLanelets current_lanes = utils::getCurrentLanes(planner_data_);
+  const lanelet::ConstLanelets current_lanes =
+    utils::getCurrentLanesFromPath(getPreviousModuleOutput().reference_path, planner_data_);
   lanelet::utils::query::getClosestLanelet(current_lanes, current_pose, &current_lane);
   const bool goal_is_in_current_lanes = std::any_of(
     current_lanes.begin(), current_lanes.end(), [&](const lanelet::ConstLanelet & current_lane) {
@@ -384,14 +443,14 @@ bool GoalPlannerModule::isExecutionRequested() const
   // if goal modification is not allowed
   // 1) goal_pose is in current_lanes, plan path to the original fixed goal
   // 2) goal_pose is NOT in current_lanes, do not execute goal_planner
-  if (!goal_planner_utils::isAllowedGoalModification(route_handler)) {
+  if (!utils::isAllowedGoalModification(route_handler)) {
     return goal_is_in_current_lanes;
   }
 
   // if goal arc coordinates can be calculated, check if goal is in request_length
   const double self_to_goal_arc_length =
     utils::getSignedDistance(current_pose, goal_pose, current_lanes);
-  const double request_length = goal_planner_utils::isAllowedGoalModification(route_handler)
+  const double request_length = utils::isAllowedGoalModification(route_handler)
                                   ? calcModuleRequestLength()
                                   : parameters_->pull_over_minimum_request_length;
   if (self_to_goal_arc_length < 0.0 || self_to_goal_arc_length > request_length) {
@@ -402,7 +461,7 @@ bool GoalPlannerModule::isExecutionRequested() const
   // if goal modification is not allowed
   // 1) goal_pose is in current_lanes, plan path to the original fixed goal
   // 2) goal_pose is NOT in current_lanes, do not execute goal_planner
-  if (!goal_planner_utils::isAllowedGoalModification(route_handler)) {
+  if (!utils::isAllowedGoalModification(route_handler)) {
     return goal_is_in_current_lanes;
   }
 
@@ -585,7 +644,7 @@ GoalCandidates GoalPlannerModule::generateGoalCandidates() const
 {
   // calculate goal candidates
   const auto & route_handler = planner_data_->route_handler;
-  if (goal_planner_utils::isAllowedGoalModification(route_handler)) {
+  if (utils::isAllowedGoalModification(route_handler)) {
     return goal_searcher_->search();
   }
 
@@ -603,7 +662,7 @@ GoalCandidates GoalPlannerModule::generateGoalCandidates() const
 
 BehaviorModuleOutput GoalPlannerModule::plan()
 {
-  if (goal_planner_utils::isAllowedGoalModification(planner_data_->route_handler)) {
+  if (utils::isAllowedGoalModification(planner_data_->route_handler)) {
     return planPullOver();
   }
 
@@ -1094,7 +1153,7 @@ void GoalPlannerModule::updatePreviousData(const BehaviorModuleOutput & output)
 
 BehaviorModuleOutput GoalPlannerModule::planWaitingApproval()
 {
-  if (goal_planner_utils::isAllowedGoalModification(planner_data_->route_handler)) {
+  if (utils::isAllowedGoalModification(planner_data_->route_handler)) {
     return planPullOverAsCandidate();
   }
 
@@ -1153,11 +1212,15 @@ PathWithLaneId GoalPlannerModule::generateStopPath() const
     return PathWithLaneId{};
   }
 
-  // generate reference path
-  const auto s_current = lanelet::utils::getArcCoordinates(current_lanes, current_pose).length;
-  const double s_start = std::max(0.0, s_current - common_parameters.backward_path_length);
-  const double s_end = s_current + common_parameters.forward_path_length;
-  auto reference_path = route_handler->getCenterLinePath(current_lanes, s_start, s_end, true);
+  // extend previous module path to generate reference path for stop path
+  const auto reference_path = std::invoke([&]() -> PathWithLaneId {
+    const auto s_current = lanelet::utils::getArcCoordinates(current_lanes, current_pose).length;
+    const double s_start = std::max(0.0, s_current - common_parameters.backward_path_length);
+    const double s_end = s_current + common_parameters.forward_path_length;
+    return route_handler->getCenterLinePath(current_lanes, s_start, s_end, true);
+  });
+  const auto extended_prev_path = goal_planner_utils::extendPath(
+    getPreviousModuleOutput().path, reference_path, common_parameters.forward_path_length);
 
   // calculate search start offset pose from the closest goal candidate pose with
   // approximate_pull_over_distance_ ego vehicle decelerates to this position. or if no feasible
@@ -1165,7 +1228,7 @@ PathWithLaneId GoalPlannerModule::generateStopPath() const
   const auto closest_goal_candidate =
     goal_searcher_->getClosetGoalCandidateAlongLanes(thread_safe_data_.get_goal_candidates());
   const auto decel_pose = calcLongitudinalOffsetPose(
-    reference_path.points, closest_goal_candidate.goal_pose.position,
+    extended_prev_path.points, closest_goal_candidate.goal_pose.position,
     -approximate_pull_over_distance_);
 
   // if not approved stop road lane.
@@ -1193,7 +1256,7 @@ PathWithLaneId GoalPlannerModule::generateStopPath() const
   }
 
   // if stop pose is closer than min_stop_distance, stop as soon as possible
-  const double ego_to_stop_distance = calcSignedArcLengthFromEgo(reference_path, *stop_pose);
+  const double ego_to_stop_distance = calcSignedArcLengthFromEgo(extended_prev_path, *stop_pose);
   const auto min_stop_distance = calcFeasibleDecelDistance(
     planner_data_, parameters_->maximum_deceleration, parameters_->maximum_jerk, 0.0);
   const double eps_vel = 0.01;
@@ -1204,55 +1267,43 @@ PathWithLaneId GoalPlannerModule::generateStopPath() const
   }
 
   // slow down for turn signal, insert stop point to stop_pose
-  decelerateForTurnSignal(*stop_pose, reference_path);
+  auto stop_path = extended_prev_path;
+  decelerateForTurnSignal(*stop_pose, stop_path);
   stop_pose_ = *stop_pose;  // for debug wall marker
 
   // slow down before the search area.
   if (decel_pose) {
-    decelerateBeforeSearchStart(*decel_pose, reference_path);
-    return reference_path;
+    decelerateBeforeSearchStart(*decel_pose, stop_path);
+    return stop_path;
   }
 
-  // if already passed the decel pose, set pull_over_velocity to reference_path.
+  // if already passed the decel pose, set pull_over_velocity to stop_path.
   const auto min_decel_distance = calcFeasibleDecelDistance(
     planner_data_, parameters_->maximum_deceleration, parameters_->maximum_jerk,
     pull_over_velocity);
-  for (auto & p : reference_path.points) {
-    const double distance_from_ego = calcSignedArcLengthFromEgo(reference_path, p.point.pose);
+  for (auto & p : stop_path.points) {
+    const double distance_from_ego = calcSignedArcLengthFromEgo(stop_path, p.point.pose);
     if (min_decel_distance && distance_from_ego < *min_decel_distance) {
       continue;
     }
     p.point.longitudinal_velocity_mps =
       std::min(p.point.longitudinal_velocity_mps, static_cast<float>(pull_over_velocity));
   }
-  return reference_path;
+  return stop_path;
 }
 
 PathWithLaneId GoalPlannerModule::generateFeasibleStopPath() const
 {
-  const auto & route_handler = planner_data_->route_handler;
-  const auto & current_pose = planner_data_->self_odometry->pose.pose;
-  const auto & common_parameters = planner_data_->parameters;
-
-  // generate stop reference path
-  const lanelet::ConstLanelets current_lanes = utils::getExtendedCurrentLanes(
-    planner_data_, parameters_->backward_goal_search_length,
-    parameters_->forward_goal_search_length,
-    /*forward_only_in_route*/ false);
-
-  const auto s_current = lanelet::utils::getArcCoordinates(current_lanes, current_pose).length;
-  const double s_start = std::max(0.0, s_current - common_parameters.backward_path_length);
-  const double s_end = s_current + common_parameters.forward_path_length;
-  auto stop_path = route_handler->getCenterLinePath(current_lanes, s_start, s_end, true);
-
   // calc minimum stop distance under maximum deceleration
   const auto min_stop_distance = calcFeasibleDecelDistance(
     planner_data_, parameters_->maximum_deceleration, parameters_->maximum_jerk, 0.0);
   if (!min_stop_distance) {
-    return stop_path;
+    return getPreviousModuleOutput().path;
   }
 
   // set stop point
+  auto stop_path = getPreviousModuleOutput().path;
+  const auto & current_pose = planner_data_->self_odometry->pose.pose;
   const auto stop_idx =
     motion_utils::insertStopPoint(current_pose, *min_stop_distance, stop_path.points);
   if (stop_idx) {
@@ -1881,7 +1932,7 @@ void GoalPlannerModule::setDebugData()
     }
     tier4_autoware_utils::appendMarkerArray(added, &debug_marker_);
   };
-  if (goal_planner_utils::isAllowedGoalModification(planner_data_->route_handler)) {
+  if (utils::isAllowedGoalModification(planner_data_->route_handler)) {
     // Visualize pull over areas
     const auto color = hasDecidedPath() ? createMarkerColor(1.0, 1.0, 0.0, 0.999)   // yellow
                                         : createMarkerColor(0.0, 1.0, 0.0, 0.999);  // green
@@ -1894,6 +1945,14 @@ void GoalPlannerModule::setDebugData()
     add(goal_planner_utils::createGoalCandidatesMarkerArray(goal_candidates, color));
   }
 
+  // Visualize previous module output
+  add(createPathMarkerArray(
+    getPreviousModuleOutput().path, "previous_module_path", 0, 1.0, 0.0, 0.0));
+  if (last_previous_module_output_.has_value()) {
+    add(createPathMarkerArray(
+      last_previous_module_output_.value().path, "last_previous_module_path", 0, 0.0, 1.0, 1.0));
+  }
+
   // Visualize path and related pose
   if (thread_safe_data_.foundPullOverPath()) {
     add(createPoseMarkerArray(
diff --git a/planning/behavior_path_goal_planner_module/src/manager.cpp b/planning/behavior_path_goal_planner_module/src/manager.cpp
index 3079361253c3..8608d6cce2c8 100644
--- a/planning/behavior_path_goal_planner_module/src/manager.cpp
+++ b/planning/behavior_path_goal_planner_module/src/manager.cpp
@@ -419,7 +419,7 @@ bool GoalPlannerModuleManager::isAlwaysExecutableModule() const
 {
   // enable AlwaysExecutable whenever goal modification is not allowed
   // because only minor path refinements are made for fixed goals
-  if (!goal_planner_utils::isAllowedGoalModification(planner_data_->route_handler)) {
+  if (!utils::isAllowedGoalModification(planner_data_->route_handler)) {
     return true;
   }
 
@@ -434,7 +434,7 @@ bool GoalPlannerModuleManager::isSimultaneousExecutableAsApprovedModule() const
 
   // enable SimultaneousExecutable whenever goal modification is not allowed
   // because only minor path refinements are made for fixed goals
-  if (!goal_planner_utils::isAllowedGoalModification(planner_data_->route_handler)) {
+  if (!utils::isAllowedGoalModification(planner_data_->route_handler)) {
     return true;
   }
 
@@ -449,7 +449,7 @@ bool GoalPlannerModuleManager::isSimultaneousExecutableAsCandidateModule() const
 
   // enable SimultaneousExecutable whenever goal modification is not allowed
   // because only minor path refinements are made for fixed goals
-  if (!goal_planner_utils::isAllowedGoalModification(planner_data_->route_handler)) {
+  if (!utils::isAllowedGoalModification(planner_data_->route_handler)) {
     return true;
   }
 
diff --git a/planning/behavior_path_goal_planner_module/src/shift_pull_over.cpp b/planning/behavior_path_goal_planner_module/src/shift_pull_over.cpp
index 712da5aa03a4..b168da61e623 100644
--- a/planning/behavior_path_goal_planner_module/src/shift_pull_over.cpp
+++ b/planning/behavior_path_goal_planner_module/src/shift_pull_over.cpp
@@ -46,10 +46,10 @@ std::optional<PullOverPath> ShiftPullOver::plan(const Pose & goal_pose)
   const int shift_sampling_num = parameters_.shift_sampling_num;
   const double jerk_resolution = std::abs(max_jerk - min_jerk) / shift_sampling_num;
 
-  // get road and shoulder lanes
-  const auto road_lanes = utils::getExtendedCurrentLanes(
-    planner_data_, backward_search_length, forward_search_length,
+  const auto road_lanes = utils::getExtendedCurrentLanesFromPath(
+    previous_module_output_.path, planner_data_, backward_search_length, forward_search_length,
     /*forward_only_in_route*/ false);
+
   const auto pull_over_lanes = goal_planner_utils::getPullOverLanes(
     *route_handler, left_side_parking_, backward_search_length, forward_search_length);
   if (road_lanes.empty() || pull_over_lanes.empty()) {
@@ -99,6 +99,31 @@ PathWithLaneId ShiftPullOver::generateReferencePath(
   return road_lane_reference_path;
 }
 
+std::optional<PathWithLaneId> ShiftPullOver::cropPrevModulePath(
+  const PathWithLaneId & prev_module_path, const Pose & shift_end_pose) const
+{
+  // clip previous module path to shift end pose nearest segment index
+  const size_t shift_end_idx =
+    motion_utils::findNearestSegmentIndex(prev_module_path.points, shift_end_pose.position);
+  std::vector<PathPointWithLaneId> clipped_points{
+    prev_module_path.points.begin(), prev_module_path.points.begin() + shift_end_idx};
+  if (clipped_points.empty()) {
+    return std::nullopt;
+  }
+
+  // add projected shift end pose to clipped points
+  PathPointWithLaneId projected_point = clipped_points.back();
+  const double offset = motion_utils::calcSignedArcLength(
+    prev_module_path.points, shift_end_idx, shift_end_pose.position);
+  projected_point.point.pose =
+    tier4_autoware_utils::calcOffsetPose(clipped_points.back().point.pose, offset, 0, 0);
+  clipped_points.push_back(projected_point);
+  auto clipped_prev_module_path = prev_module_path;
+  clipped_prev_module_path.points = clipped_points;
+
+  return clipped_prev_module_path;
+}
+
 std::optional<PullOverPath> ShiftPullOver::generatePullOverPath(
   const lanelet::ConstLanelets & road_lanes, const lanelet::ConstLanelets & shoulder_lanes,
   const Pose & goal_pose, const double lateral_jerk) const
@@ -106,33 +131,55 @@ std::optional<PullOverPath> ShiftPullOver::generatePullOverPath(
   const double pull_over_velocity = parameters_.pull_over_velocity;
   const double after_shift_straight_distance = parameters_.after_shift_straight_distance;
 
+  // shift end pose is longitudinal offset from goal pose to improve parking angle accuracy
   const Pose shift_end_pose =
     tier4_autoware_utils::calcOffsetPose(goal_pose, -after_shift_straight_distance, 0, 0);
 
-  // generate road lane reference path to shift end
+  // calculate lateral shift of previous module path terminal pose from road lane reference path
   const auto road_lane_reference_path_to_shift_end = utils::resamplePathWithSpline(
     generateReferencePath(road_lanes, shift_end_pose), parameters_.center_line_path_interval);
+  const auto prev_module_path = utils::resamplePathWithSpline(
+    previous_module_output_.path, parameters_.center_line_path_interval);
+  const auto prev_module_path_terminal_pose = prev_module_path.points.back().point.pose;
+
+  // process previous module path for path shifter input path
+  // case1) extend path if shift end pose is behind of previous module path terminal pose
+  // case2) crop path if shift end pose is ahead of previous module path terminal pose
+  const auto processed_prev_module_path = std::invoke([&]() -> std::optional<PathWithLaneId> {
+    const bool extend_previous_module_path =
+      lanelet::utils::getArcCoordinates(road_lanes, shift_end_pose).length >
+      lanelet::utils::getArcCoordinates(road_lanes, prev_module_path_terminal_pose).length;
+    if (extend_previous_module_path) {  // case1
+      return goal_planner_utils::extendPath(
+        prev_module_path, road_lane_reference_path_to_shift_end, shift_end_pose);
+    } else {  // case2
+      return goal_planner_utils::cropPath(prev_module_path, shift_end_pose);
+    }
+  });
+  if (!processed_prev_module_path || processed_prev_module_path->points.empty()) {
+    return {};
+  }
 
   // calculate shift length
-  const Pose & shift_end_pose_road_lane =
-    road_lane_reference_path_to_shift_end.points.back().point.pose;
+  const Pose & shift_end_pose_prev_module_path =
+    processed_prev_module_path->points.back().point.pose;
   const double shift_end_road_to_target_distance =
-    tier4_autoware_utils::inverseTransformPoint(shift_end_pose.position, shift_end_pose_road_lane)
+    tier4_autoware_utils::inverseTransformPoint(
+      shift_end_pose.position, shift_end_pose_prev_module_path)
       .y;
 
   // calculate shift start pose on road lane
   const double pull_over_distance = PathShifter::calcLongitudinalDistFromJerk(
     shift_end_road_to_target_distance, lateral_jerk, pull_over_velocity);
   const double before_shifted_pull_over_distance = calcBeforeShiftedArcLength(
-    road_lane_reference_path_to_shift_end, pull_over_distance, shift_end_road_to_target_distance);
+    processed_prev_module_path.value(), pull_over_distance, shift_end_road_to_target_distance);
   const auto shift_start_pose = motion_utils::calcLongitudinalOffsetPose(
-    road_lane_reference_path_to_shift_end.points, shift_end_pose_road_lane.position,
+    processed_prev_module_path->points, shift_end_pose_prev_module_path.position,
     -before_shifted_pull_over_distance);
-  if (!shift_start_pose) return {};
 
   // set path shifter and generate shifted path
   PathShifter path_shifter{};
-  path_shifter.setPath(road_lane_reference_path_to_shift_end);
+  path_shifter.setPath(processed_prev_module_path.value());
   ShiftLine shift_line{};
   shift_line.start = *shift_start_pose;
   shift_line.end = shift_end_pose;
@@ -140,7 +187,9 @@ std::optional<PullOverPath> ShiftPullOver::generatePullOverPath(
   path_shifter.addShiftLine(shift_line);
   ShiftedPath shifted_path{};
   const bool offset_back = true;  // offset front side from reference path
-  if (!path_shifter.generate(&shifted_path, offset_back)) return {};
+  if (!path_shifter.generate(&shifted_path, offset_back)) {
+    return {};
+  }
   shifted_path.path.points = motion_utils::removeOverlapPoints(shifted_path.path.points);
   motion_utils::insertOrientation(shifted_path.path.points, true);
 
@@ -208,8 +257,13 @@ std::optional<PullOverPath> ShiftPullOver::generatePullOverPath(
   pull_over_path.pairs_terminal_velocity_and_accel.push_back(std::make_pair(pull_over_velocity, 0));
   pull_over_path.start_pose = path_shifter.getShiftLines().front().start;
   pull_over_path.end_pose = path_shifter.getShiftLines().front().end;
-  pull_over_path.debug_poses.push_back(shift_end_pose_road_lane);
+  pull_over_path.debug_poses.push_back(shift_end_pose_prev_module_path);
   pull_over_path.debug_poses.push_back(actual_shift_end_pose);
+  pull_over_path.debug_poses.push_back(goal_pose);
+  pull_over_path.debug_poses.push_back(shift_end_pose);
+  pull_over_path.debug_poses.push_back(
+    road_lane_reference_path_to_shift_end.points.back().point.pose);
+  pull_over_path.debug_poses.push_back(prev_module_path_terminal_pose);
 
   // check if the parking path will leave lanes
   const auto drivable_lanes =
@@ -218,8 +272,10 @@ std::optional<PullOverPath> ShiftPullOver::generatePullOverPath(
   const auto expanded_lanes = utils::expandLanelets(
     drivable_lanes, dp.drivable_area_left_bound_offset, dp.drivable_area_right_bound_offset,
     dp.drivable_area_types_to_skip);
+  const auto combined_drivable = utils::combineDrivableLanes(
+    expanded_lanes, previous_module_output_.drivable_area_info.drivable_lanes);
   if (lane_departure_checker_.checkPathWillLeaveLane(
-        utils::transformToLanelets(expanded_lanes), pull_over_path.getParkingPath())) {
+        utils::transformToLanelets(combined_drivable), pull_over_path.getParkingPath())) {
     return {};
   }
 
diff --git a/planning/behavior_path_goal_planner_module/src/util.cpp b/planning/behavior_path_goal_planner_module/src/util.cpp
index 23e9c53efae7..1610c76c11d5 100644
--- a/planning/behavior_path_goal_planner_module/src/util.cpp
+++ b/planning/behavior_path_goal_planner_module/src/util.cpp
@@ -196,22 +196,138 @@ MarkerArray createGoalCandidatesMarkerArray(
   return marker_array;
 }
 
-bool isAllowedGoalModification(const std::shared_ptr<RouteHandler> & route_handler)
+double calcLateralDeviationBetweenPaths(
+  const PathWithLaneId & reference_path, const PathWithLaneId & target_path)
 {
-  return route_handler->isAllowedGoalModification() || checkOriginalGoalIsInShoulder(route_handler);
+  double lateral_deviation = 0.0;
+  for (const auto & target_point : target_path.points) {
+    const size_t nearest_index =
+      motion_utils::findNearestIndex(reference_path.points, target_point.point.pose.position);
+    lateral_deviation = std::max(
+      lateral_deviation,
+      std::abs(tier4_autoware_utils::calcLateralDeviation(
+        reference_path.points[nearest_index].point.pose, target_point.point.pose.position)));
+  }
+  return lateral_deviation;
+}
+
+bool isReferencePath(
+  const PathWithLaneId & reference_path, const PathWithLaneId & target_path,
+  const double lateral_deviation_threshold)
+{
+  return calcLateralDeviationBetweenPaths(reference_path, target_path) <
+         lateral_deviation_threshold;
+}
+
+std::optional<PathWithLaneId> cropPath(const PathWithLaneId & path, const Pose & end_pose)
+{
+  const size_t end_idx = motion_utils::findNearestSegmentIndex(path.points, end_pose.position);
+  std::vector<PathPointWithLaneId> clipped_points{
+    path.points.begin(), path.points.begin() + end_idx};
+  if (clipped_points.empty()) {
+    return std::nullopt;
+  }
+
+  // add projected end pose to clipped points
+  PathPointWithLaneId projected_point = clipped_points.back();
+  const double offset = motion_utils::calcSignedArcLength(path.points, end_idx, end_pose.position);
+  projected_point.point.pose =
+    tier4_autoware_utils::calcOffsetPose(clipped_points.back().point.pose, offset, 0, 0);
+  clipped_points.push_back(projected_point);
+  auto clipped_path = path;
+  clipped_path.points = clipped_points;
+
+  return clipped_path;
+}
+
+PathWithLaneId cropForwardPoints(
+  const PathWithLaneId & path, const size_t target_seg_idx, const double forward_length)
+{
+  const auto & points = path.points;
+
+  double sum_length = 0;
+  for (size_t i = target_seg_idx + 1; i < points.size(); ++i) {
+    const double seg_length = tier4_autoware_utils::calcDistance2d(points.at(i), points.at(i - 1));
+    if (forward_length < sum_length + seg_length) {
+      const auto cropped_points =
+        std::vector<PathPointWithLaneId>{points.begin() + target_seg_idx, points.begin() + i};
+      PathWithLaneId cropped_path = path;
+      cropped_path.points = cropped_points;
+
+      // add precise end pose to cropped points
+      const double remaining_length = forward_length - sum_length;
+      const Pose precise_end_pose =
+        calcOffsetPose(cropped_path.points.back().point.pose, remaining_length, 0, 0);
+      if (remaining_length < 0.1) {
+        // if precise_end_pose is too close, replace the last point
+        cropped_path.points.back().point.pose = precise_end_pose;
+      } else {
+        auto precise_end_point = cropped_path.points.back();
+        precise_end_point.point.pose = precise_end_pose;
+        cropped_path.points.push_back(precise_end_point);
+      }
+      return cropped_path;
+    }
+    sum_length += seg_length;
+  }
+
+  // if forward_length is too long, return points after target_seg_idx
+  const auto cropped_points =
+    std::vector<PathPointWithLaneId>{points.begin() + target_seg_idx, points.end()};
+  PathWithLaneId cropped_path = path;
+  cropped_path.points = cropped_points;
+  return cropped_path;
 }
 
-bool checkOriginalGoalIsInShoulder(const std::shared_ptr<RouteHandler> & route_handler)
+PathWithLaneId extendPath(
+  const PathWithLaneId & target_path, const PathWithLaneId & reference_path,
+  const double extend_length)
 {
-  const Pose & goal_pose = route_handler->getOriginalGoalPose();
-  const auto shoulder_lanes = route_handler->getShoulderLanelets();
+  const auto & target_terminal_pose = target_path.points.back().point.pose;
+
+  // generate clipped road lane reference path from previous module path terminal pose to shift end
+  const size_t target_path_terminal_idx =
+    motion_utils::findNearestSegmentIndex(reference_path.points, target_terminal_pose.position);
 
-  lanelet::ConstLanelet closest_shoulder_lane{};
-  if (lanelet::utils::query::getClosestLanelet(shoulder_lanes, goal_pose, &closest_shoulder_lane)) {
-    return lanelet::utils::isInLanelet(goal_pose, closest_shoulder_lane, 0.1);
+  PathWithLaneId clipped_path =
+    cropForwardPoints(reference_path, target_path_terminal_idx, extend_length);
+
+  // shift clipped path to previous module path terminal pose
+  const double lateral_shift_from_reference_path =
+    motion_utils::calcLateralOffset(reference_path.points, target_terminal_pose.position);
+  for (auto & p : clipped_path.points) {
+    p.point.pose =
+      tier4_autoware_utils::calcOffsetPose(p.point.pose, 0, lateral_shift_from_reference_path, 0);
   }
 
-  return false;
+  auto extended_path = target_path;
+  const auto start_point =
+    std::find_if(clipped_path.points.begin(), clipped_path.points.end(), [&](const auto & p) {
+      const bool is_forward =
+        tier4_autoware_utils::inverseTransformPoint(p.point.pose.position, target_terminal_pose).x >
+        0.0;
+      const bool is_close = tier4_autoware_utils::calcDistance2d(
+                              p.point.pose.position, target_terminal_pose.position) < 0.1;
+      return is_forward && !is_close;
+    });
+  std::copy(start_point, clipped_path.points.end(), std::back_inserter(extended_path.points));
+
+  extended_path.points = motion_utils::removeOverlapPoints(extended_path.points);
+
+  return extended_path;
+}
+
+PathWithLaneId extendPath(
+  const PathWithLaneId & target_path, const PathWithLaneId & reference_path,
+  const Pose & extend_pose)
+{
+  const auto & target_terminal_pose = target_path.points.back().point.pose;
+  const size_t target_path_terminal_idx =
+    motion_utils::findNearestSegmentIndex(reference_path.points, target_terminal_pose.position);
+  const double extend_distance = motion_utils::calcSignedArcLength(
+    reference_path.points, target_path_terminal_idx, extend_pose.position);
+
+  return extendPath(target_path, reference_path, extend_distance);
 }
 
 }  // namespace goal_planner_utils
diff --git a/planning/behavior_path_planner/docs/behavior_path_planner_avoidance_design.md b/planning/behavior_path_planner/docs/behavior_path_planner_avoidance_design.md
index c4580bb1e82c..5fc1105967b1 100644
--- a/planning/behavior_path_planner/docs/behavior_path_planner_avoidance_design.md
+++ b/planning/behavior_path_planner/docs/behavior_path_planner_avoidance_design.md
@@ -833,7 +833,8 @@ namespace: `avoidance.target_filtering.`
 | object_ignore_section_crosswalk_behind_distance       | [m]  | double | If the back distance between crosswalk and vehicle is less than this parameter, this module will ignore it.                                                                                                                            | 30.0          |
 | object_check_forward_distance                         | [m]  | double | Forward distance to search the avoidance target.                                                                                                                                                                                       | 150.0         |
 | object_check_backward_distance                        | [m]  | double | Backward distance to search the avoidance target.                                                                                                                                                                                      | 2.0           |
-| object_check_goal_distance                            | [m]  | double | Backward distance to search the avoidance target.                                                                                                                                                                                      | 20.0          |
+| object_check_goal_distance                            | [m]  | double | If the distance between object and goal position is less than this parameter, the module do not return center line.                                                                                                                    | 20.0          |
+| object_check_return_pose_distance                     | [m]  | double | If the distance between object and return position is less than this parameter, the module do not return center line.                                                                                                                  | 20.0          |
 | object_check_shiftable_ratio                          | [m]  | double | Vehicles around the center line within this distance will be excluded from avoidance target.                                                                                                                                           | 0.6           |
 | object_check_min_road_shoulder_width                  | [m]  | double | Width considered as a road shoulder if the lane does not have a road shoulder target.                                                                                                                                                  | 0.5           |
 | object_last_seen_threshold                            | [s]  | double | For the compensation of the detection lost. The object is registered once it is observed as an avoidance target. When the detection loses, the timer will start and the object will be un-registered when the time exceeds this limit. | 2.0           |
diff --git a/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/utils.hpp b/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/utils.hpp
index e2c9fd1e332a..6a04590d6f6d 100644
--- a/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/utils.hpp
+++ b/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/utils.hpp
@@ -243,6 +243,9 @@ PathWithLaneId refinePathForGoal(
 
 bool containsGoal(const lanelet::ConstLanelets & lanes, const lanelet::Id & goal_id);
 
+bool isAllowedGoalModification(const std::shared_ptr<RouteHandler> & route_handler);
+bool checkOriginalGoalIsInShoulder(const std::shared_ptr<RouteHandler> & route_handler);
+
 bool isInLanelets(const Pose & pose, const lanelet::ConstLanelets & lanes);
 
 bool isInLaneletWithYawThreshold(
@@ -325,6 +328,10 @@ lanelet::ConstLanelets getExtendedCurrentLanes(
   const std::shared_ptr<const PlannerData> & planner_data, const double backward_length,
   const double forward_length, const bool forward_only_in_route);
 
+lanelet::ConstLanelets getExtendedCurrentLanesFromPath(
+  const PathWithLaneId & path, const std::shared_ptr<const PlannerData> & planner_data,
+  const double backward_length, const double forward_length, const bool forward_only_in_route);
+
 lanelet::ConstLanelets calcLaneAroundPose(
   const std::shared_ptr<RouteHandler> route_handler, const geometry_msgs::msg::Pose & pose,
   const double forward_length, const double backward_length,
diff --git a/planning/behavior_path_planner_common/src/utils/utils.cpp b/planning/behavior_path_planner_common/src/utils/utils.cpp
index 7c3a5883989f..a528b2232520 100644
--- a/planning/behavior_path_planner_common/src/utils/utils.cpp
+++ b/planning/behavior_path_planner_common/src/utils/utils.cpp
@@ -1450,6 +1450,70 @@ lanelet::ConstLanelets getExtendedCurrentLanes(
   return extendLanes(planner_data->route_handler, getCurrentLanes(planner_data));
 }
 
+lanelet::ConstLanelets getExtendedCurrentLanesFromPath(
+  const PathWithLaneId & path, const std::shared_ptr<const PlannerData> & planner_data,
+  const double backward_length, const double forward_length, const bool forward_only_in_route)
+{
+  auto lanes = getCurrentLanesFromPath(path, planner_data);
+
+  if (lanes.empty()) return lanes;
+  const auto start_lane = lanes.front();
+  double forward_length_sum = 0.0;
+  double backward_length_sum = 0.0;
+
+  while (backward_length_sum < backward_length) {
+    auto extended_lanes = extendPrevLane(planner_data->route_handler, lanes);
+    if (extended_lanes.empty()) {
+      return lanes;
+    }
+    // loop check
+    // if current map lanes is looping and has a very large value for backward_length,
+    // the extending process will not finish.
+    if (extended_lanes.front().id() == start_lane.id()) {
+      return lanes;
+    }
+
+    if (extended_lanes.size() > lanes.size()) {
+      backward_length_sum += lanelet::utils::getLaneletLength2d(extended_lanes.front());
+    } else {
+      break;  // no more previous lanes to add
+    }
+    lanes = extended_lanes;
+  }
+
+  while (forward_length_sum < forward_length) {
+    auto extended_lanes = extendNextLane(planner_data->route_handler, lanes);
+    if (extended_lanes.empty()) {
+      return lanes;
+    }
+    // loop check
+    // if current map lanes is looping and has a very large value for forward_length,
+    // the extending process will not finish.
+    if (extended_lanes.back().id() == start_lane.id()) {
+      return lanes;
+    }
+
+    if (extended_lanes.size() > lanes.size()) {
+      forward_length_sum += lanelet::utils::getLaneletLength2d(extended_lanes.back());
+    } else {
+      break;  // no more next lanes to add
+    }
+
+    // stop extending when the lane outside of the route is reached
+    // if forward_length is a very large value, set it to true,
+    // as it may continue to extend forever.
+    if (forward_only_in_route) {
+      if (!planner_data->route_handler->isRouteLanelet(extended_lanes.back())) {
+        return lanes;
+      }
+    }
+
+    lanes = extended_lanes;
+  }
+
+  return lanes;
+}
+
 lanelet::ConstLanelets calcLaneAroundPose(
   const std::shared_ptr<RouteHandler> route_handler, const Pose & pose, const double forward_length,
   const double backward_length, const double dist_threshold, const double yaw_threshold)
@@ -1551,4 +1615,22 @@ std::string convertToSnakeCase(const std::string & input_str)
   }
   return output_str;
 }
+
+bool isAllowedGoalModification(const std::shared_ptr<RouteHandler> & route_handler)
+{
+  return route_handler->isAllowedGoalModification() || checkOriginalGoalIsInShoulder(route_handler);
+}
+
+bool checkOriginalGoalIsInShoulder(const std::shared_ptr<RouteHandler> & route_handler)
+{
+  const Pose & goal_pose = route_handler->getOriginalGoalPose();
+  const auto shoulder_lanes = route_handler->getShoulderLanelets();
+
+  lanelet::ConstLanelet closest_shoulder_lane{};
+  if (lanelet::utils::query::getClosestLanelet(shoulder_lanes, goal_pose, &closest_shoulder_lane)) {
+    return lanelet::utils::isInLanelet(goal_pose, closest_shoulder_lane, 0.1);
+  }
+
+  return false;
+}
 }  // namespace behavior_path_planner::utils