diff --git a/planning/autoware_path_generator/config/path_generator.param.yaml b/planning/autoware_path_generator/config/path_generator.param.yaml
index 7a4ed323d6..67981033be 100644
--- a/planning/autoware_path_generator/config/path_generator.param.yaml
+++ b/planning/autoware_path_generator/config/path_generator.param.yaml
@@ -12,3 +12,5 @@
       search_distance: 30.0
       resampling_interval: 1.0
       angle_threshold_deg: 15.0
+    refine_goal_search_radius_range: 7.5  # [m]
+    search_radius_decrement: 1.0
diff --git a/planning/autoware_path_generator/include/autoware/path_generator/common_structs.hpp b/planning/autoware_path_generator/include/autoware/path_generator/common_structs.hpp
index a1b774e93a..8285e8417c 100644
--- a/planning/autoware_path_generator/include/autoware/path_generator/common_structs.hpp
+++ b/planning/autoware_path_generator/include/autoware/path_generator/common_structs.hpp
@@ -24,6 +24,20 @@
 
 namespace autoware::path_generator
 {
+
+struct PathGeneratorParameters
+{
+  /**
+   * @brief Distance on path to be modified for goal insertion
+   */
+  double refine_goal_search_radius_range{0.0};
+
+  /**
+   * @brief Decrement of the search radius range
+   */
+  double search_radius_decrement{0.0};
+};
+
 struct PlannerData
 {
   lanelet::LaneletMapPtr lanelet_map_ptr{nullptr};
@@ -31,12 +45,15 @@ struct PlannerData
   lanelet::routing::RoutingGraphPtr routing_graph_ptr{nullptr};
 
   std::string route_frame_id{};
+  lanelet::Id goal_lane_id{};
   geometry_msgs::msg::Pose goal_pose{};
 
   lanelet::ConstLanelets route_lanelets{};
   lanelet::ConstLanelets preferred_lanelets{};
   lanelet::ConstLanelets start_lanelets{};
   lanelet::ConstLanelets goal_lanelets{};
+
+  PathGeneratorParameters path_generator_parameters{};
 };
 }  // namespace autoware::path_generator
 
diff --git a/planning/autoware_path_generator/include/autoware/path_generator/utils.hpp b/planning/autoware_path_generator/include/autoware/path_generator/utils.hpp
index 0bebc1f51b..6575550202 100644
--- a/planning/autoware_path_generator/include/autoware/path_generator/utils.hpp
+++ b/planning/autoware_path_generator/include/autoware/path_generator/utils.hpp
@@ -20,6 +20,8 @@
 #include <autoware_internal_planning_msgs/msg/path_with_lane_id.hpp>
 #include <autoware_vehicle_msgs/msg/turn_indicators_command.hpp>
 
+#include <limits>
+#include <memory>
 #include <optional>
 #include <string>
 #include <unordered_map>
@@ -147,6 +149,102 @@ std::vector<geometry_msgs::msg::Point> get_path_bound(
   const lanelet::CompoundLineString2d & lanelet_centerline, const double s_start,
   const double s_end);
 
+/**
+ * @brief Recreate the goal pose to prevent the goal point being too far from the lanelet, which
+ *  causes the path to twist near the goal.
+ * @details Return the goal point projected on the straight line of the segment of lanelet
+ *  closest to the original goal.
+ * @param [in] goal original goal pose
+ * @param [in] goal_lanelet lanelet containing the goal pose
+ */
+const geometry_msgs::msg::Pose refine_goal(
+  const geometry_msgs::msg::Pose & goal, const lanelet::ConstLanelet & goal_lanelet);
+
+/**
+ * @brief Prepare the point before the goal point.
+ * @param goal Goal pose.
+ * @param lanes Lanelets.
+ * @return Pre-goal point.
+ */
+PathPointWithLaneId prepare_pre_goal(
+  const geometry_msgs::msg::Pose & goal, const lanelet::ConstLanelets & lanes);
+
+/**
+ * @brief Get the index of the point closest to the circumference of the circle whose center is the
+ * goal and outside of it.
+ * @param points Points to search.
+ * @param goal Goal pose.
+ * @param goal_lane_id Lane ID of the goal.
+ * @param max_dist Maximum distance to search.
+ * @return Index of the point closest to the circumference of the circle whose center is the goal
+ * and outside of it.
+ */
+std::optional<size_t> find_index_out_of_goal_search_range(
+  const std::vector<PathPointWithLaneId> & points, const geometry_msgs::msg::Pose & goal,
+  const int64_t goal_lane_id, const double max_dist);
+
+/**
+ * @brief Get the path up to just before the pre_goal.
+ * @param input Input path.
+ * @param refined_goal Goal pose.
+ * @return Recreated path
+ */
+std::optional<PathWithLaneId> path_up_to_just_before_pre_goal(
+  const PathWithLaneId & input, const geometry_msgs::msg::Pose & goal,
+  const lanelet::Id goal_lane_id, const double search_radius_range);
+
+/**
+ * @brief Recreate the path with a given goal pose.
+ * @param input Input path.
+ * @param refined_goal Goal pose.
+ * @param planner_data Planner data.
+ * @return Recreated path
+ */
+PathWithLaneId refine_path_for_goal(
+  const PathWithLaneId & input, const geometry_msgs::msg::Pose & goal,
+  const PlannerData & planner_data);
+
+/**
+ * @brief Extract lanelets from the path.
+ * @param path Input path.
+ * @param planner_data Planner data.
+ * @return Extracted lanelets
+ */
+std::optional<lanelet::ConstLanelets> extract_lanelets_from_path(
+  const PathWithLaneId & refined_path, const PlannerData & planner_data);
+
+/**
+ * @brief Get the goal lanelet.
+ * @param planner_data Planner data.
+ * @return Goal lanelet (std::nullopt if not found)
+ */
+std::optional<lanelet::ConstLanelet> get_goal_lanelet(const PlannerData & planner_data);
+
+/**
+ * @brief Check if the pose is in the lanelets.
+ * @param pose Pose.
+ * @param lanes Lanelets.
+ * @return True if the pose is in the lanelets, false otherwise
+ */
+bool is_in_lanelets(const geometry_msgs::msg::Pose & pose, const lanelet::ConstLanelets & lanes);
+
+/**
+ * @brief Check if the path is valid.
+ * @param refined_path Input path.
+ * @param planner_data Planner data.
+ * @return True if the path is valid, false otherwise
+ */
+bool is_path_valid(const PathWithLaneId & refined_path, const PlannerData & planner_data);
+
+/**
+ * @brief Modify the path to connect smoothly to the goal.
+ * @param path Input path.
+ * @param planner_data Planner data.
+ * @return Modified path
+ */
+PathWithLaneId modify_path_for_smooth_goal_connection(
+  const PathWithLaneId & path, const std::shared_ptr<const PlannerData> & planner_data);
+
 /**
  * @brief get earliest turn signal based on turn direction specified for lanelets
  * @param path target path
diff --git a/planning/autoware_path_generator/package.xml b/planning/autoware_path_generator/package.xml
index 7d54f49778..fa75059724 100644
--- a/planning/autoware_path_generator/package.xml
+++ b/planning/autoware_path_generator/package.xml
@@ -8,12 +8,14 @@
   <maintainer email="takayuki.murooka@tier4.jp">Takayuki Murooka</maintainer>
   <maintainer email="mitsuhiro.sakamoto@tier4.jp">Mitsuhiro Sakamoto</maintainer>
   <maintainer email="kosuke.takeuchi@tier4.jp">Kosuke Takeuchi</maintainer>
+  <maintainer email="junya.sasaki@tier4.jp">Junya Sasaki</maintainer>
   <license>Apache License 2.0</license>
 
   <author email="satoshi.ota@tier4.jp">Satoshi Ota</author>
   <author email="takayuki.murooka@tier4.jp">Takayuki Murooka</author>
   <author email="mitsuhiro.sakamoto@tier4.jp">Mitsuhiro Sakamoto</author>
   <author email="kosuke.takeuchi@tier4.jp">Kosuke Takeuchi</author>
+  <author email="junya.sasaki@tier4.jp">Junya Sasaki</author>
 
   <buildtool_depend>ament_cmake_auto</buildtool_depend>
   <buildtool_depend>autoware_cmake</buildtool_depend>
@@ -22,7 +24,6 @@
   <depend>autoware_lanelet2_extension</depend>
   <depend>autoware_motion_utils</depend>
   <depend>autoware_planning_test_manager</depend>
-  <depend>autoware_test_utils</depend>
   <depend>autoware_trajectory</depend>
   <depend>autoware_vehicle_info_utils</depend>
   <depend>generate_parameter_library</depend>
@@ -32,6 +33,7 @@
   <test_depend>ament_cmake_ros</test_depend>
   <test_depend>ament_lint_auto</test_depend>
   <test_depend>autoware_lint_common</test_depend>
+  <test_depend>autoware_test_utils</test_depend>
 
   <export>
     <build_type>ament_cmake</build_type>
diff --git a/planning/autoware_path_generator/src/node.cpp b/planning/autoware_path_generator/src/node.cpp
index 8712e0f0f2..becf067de9 100644
--- a/planning/autoware_path_generator/src/node.cpp
+++ b/planning/autoware_path_generator/src/node.cpp
@@ -46,6 +46,20 @@ namespace autoware::path_generator
 PathGenerator::PathGenerator(const rclcpp::NodeOptions & node_options)
 : Node("path_generator", node_options)
 {
+  // Initialize path_generator_parameters
+  planner_data_.path_generator_parameters.refine_goal_search_radius_range =
+    declare_parameter<double>("refine_goal_search_radius_range");
+  planner_data_.path_generator_parameters.search_radius_decrement =
+    declare_parameter<double>("search_radius_decrement");
+
+  // Ensure that the refine_goal_search_radius_range and search_radius_decrement must be positive
+  if (
+    planner_data_.path_generator_parameters.refine_goal_search_radius_range <= 0 ||
+    planner_data_.path_generator_parameters.search_radius_decrement <= 0) {
+    throw std::runtime_error(
+      "refine_goal_search_radius_range and search_radius_decrement must be positive");
+  }
+
   param_listener_ =
     std::make_shared<::path_generator::ParamListener>(this->get_node_parameters_interface());
 
@@ -155,6 +169,8 @@ void PathGenerator::set_route(const LaneletRoute::ConstSharedPtr & route_ptr)
     }
   }
 
+  planner_data_.goal_lane_id = route_ptr->segments.back().preferred_primitive.id;
+
   const auto set_lanelets_from_segment =
     [&](
       const autoware_planning_msgs::msg::LaneletSegment & segment,
@@ -364,7 +380,10 @@ std::optional<PathWithLaneId> PathGenerator::generate_path(
     return std::nullopt;
   }
 
+  // Attach orientation for all the points
   trajectory->align_orientation_with_trajectory_direction();
+
+  // Refine the trajectory by cropping
   trajectory->crop(
     s_offset + s_start -
       get_arc_length_along_centerline(
@@ -372,10 +391,32 @@ std::optional<PathWithLaneId> PathGenerator::generate_path(
                                      path_points_with_lane_id.front().point.pose.position)),
     s_end - s_start);
 
-  PathWithLaneId path{};
-  path.header.frame_id = planner_data_.route_frame_id;
-  path.header.stamp = now();
-  path.points = trajectory->restore();
+  // Compose the polished path
+  PathWithLaneId preprocessed_path{};
+  preprocessed_path.header.frame_id = planner_data_.route_frame_id;
+  preprocessed_path.header.stamp = now();
+  preprocessed_path.points = trajectory->restore();
+
+  PathWithLaneId finalized_path_with_lane_id{};
+
+  // Check if the goal point is in the search range
+  // Note: We assume the current position is the first point of the path
+  const auto distance_to_goal = autoware_utils::calc_distance2d(
+    preprocessed_path.points.front().point.pose, planner_data_.goal_pose);
+
+  if (distance_to_goal < planner_data_.path_generator_parameters.refine_goal_search_radius_range) {
+    // Perform smooth goal connection
+    const auto planner_data_ptr = std::make_shared<const PlannerData>(planner_data_);
+    finalized_path_with_lane_id =
+      utils::modify_path_for_smooth_goal_connection(std::move(preprocessed_path), planner_data_ptr);
+  } else {
+    finalized_path_with_lane_id = std::move(preprocessed_path);
+  }
+
+  // check if the path is empty
+  if (finalized_path_with_lane_id.points.empty()) {
+    return std::nullopt;
+  }
 
   const auto get_path_bound = [&](const lanelet::CompoundLineString2d & lanelet_bound) {
     const auto s_bound_start =
@@ -385,10 +426,11 @@ std::optional<PathWithLaneId> PathGenerator::generate_path(
     return utils::get_path_bound(
       lanelet_bound, extended_lanelet_sequence.centerline2d(), s_bound_start, s_bound_end);
   };
-  path.left_bound = get_path_bound(extended_lanelet_sequence.leftBound2d());
-  path.right_bound = get_path_bound(extended_lanelet_sequence.rightBound2d());
+  finalized_path_with_lane_id.left_bound = get_path_bound(extended_lanelet_sequence.leftBound2d());
+  finalized_path_with_lane_id.right_bound =
+    get_path_bound(extended_lanelet_sequence.rightBound2d());
 
-  return path;
+  return finalized_path_with_lane_id;
 }
 }  // namespace autoware::path_generator
 
diff --git a/planning/autoware_path_generator/src/utils.cpp b/planning/autoware_path_generator/src/utils.cpp
index 1e1c7a81fe..e8247f41b5 100644
--- a/planning/autoware_path_generator/src/utils.cpp
+++ b/planning/autoware_path_generator/src/utils.cpp
@@ -28,6 +28,8 @@
 #include <lanelet2_core/geometry/Lanelet.h>
 
 #include <algorithm>
+#include <limits>
+#include <memory>
 #include <utility>
 #include <vector>
 
@@ -302,6 +304,311 @@ std::vector<geometry_msgs::msg::Point> get_path_bound(
   return path_bound;
 }
 
+const geometry_msgs::msg::Pose refine_goal(
+  const geometry_msgs::msg::Pose & goal, const lanelet::ConstLanelet & goal_lanelet)
+{
+  const auto goal_point_on_lanelet = lanelet::utils::conversion::toLaneletPoint(goal.position);
+  const double distance = boost::geometry::distance(
+    goal_lanelet.polygon2d().basicPolygon(),
+    lanelet::utils::to2D(goal_point_on_lanelet).basicPoint());
+
+  // You are almost at the goal
+  if (distance < std::numeric_limits<double>::epsilon()) {
+    return goal;
+  }
+
+  // Get the closest segment to the goal
+  const auto segment = lanelet::utils::getClosestSegment(
+    lanelet::utils::to2D(goal_point_on_lanelet), goal_lanelet.centerline());
+
+  // If the segment is empty, return the original goal.
+  if (segment.empty()) {
+    return goal;
+  }
+
+  geometry_msgs::msg::Pose refined_goal;
+  {
+    // find position
+    const auto p1 = segment.front().basicPoint();
+    const auto p2 = segment.back().basicPoint();
+    const auto direction_vector = (p2 - p1).normalized();
+    const auto p1_to_goal = goal_point_on_lanelet.basicPoint() - p1;
+    const double s = direction_vector.dot(p1_to_goal);
+    const auto refined_point = p1 + direction_vector * s;
+
+    refined_goal.position.x = refined_point.x();
+    refined_goal.position.y = refined_point.y();
+    refined_goal.position.z = refined_point.z();
+
+    // find orientation
+    const double yaw = std::atan2(direction_vector.y(), direction_vector.x());
+    tf2::Quaternion tf_quat;
+    tf_quat.setRPY(0, 0, yaw);
+    refined_goal.orientation = tf2::toMsg(tf_quat);
+  }
+  return refined_goal;
+}
+
+// To perform smooth goal connection, we need to prepare the point before the goal point.
+// This function prepares the point before the goal point.
+// See the link below for more details:
+//   -
+//   https://autowarefoundation.github.io/autoware.universe/main/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/#fixed_goal_planner
+PathPointWithLaneId prepare_pre_goal(
+  const geometry_msgs::msg::Pose & goal, const lanelet::ConstLanelets & lanes)
+{
+  PathPointWithLaneId pre_refined_goal{};
+
+  // -1.0 is to prepare the point before the goal point. See the link above for more details.
+  constexpr double goal_to_pre_goal_distance = -1.0;
+
+  // First, calculate the pose of the pre_goal point
+  pre_refined_goal.point.pose =
+    autoware_utils::calc_offset_pose(goal, goal_to_pre_goal_distance, 0.0, 0.0);
+
+  // Second, find and set the lane_id that the pre_goal point belongs to
+  for (const auto & lane : lanes) {
+    if (lanelet::utils::isInLanelet(pre_refined_goal.point.pose, lane)) {
+      // Prevent from duplication
+      if (exists(pre_refined_goal.lane_ids, lane.id())) {
+        continue;
+      }
+      pre_refined_goal.lane_ids.push_back(lane.id());
+    }
+  }
+
+  return pre_refined_goal;
+}
+
+// A function that assumes a circle with radius max_dist centered at the goal and returns
+// the index of the point closest to the circumference of the circle and outside of it.
+std::optional<size_t> find_index_out_of_goal_search_range(
+  const std::vector<autoware_internal_planning_msgs::msg::PathPointWithLaneId> & points,
+  const geometry_msgs::msg::Pose & goal, const int64_t goal_lane_id, const double max_dist)
+{
+  if (points.empty()) {
+    return std::nullopt;
+  }
+
+  // find goal index
+  size_t min_dist_index;
+  {
+    bool found = false;
+    double min_dist = std::numeric_limits<double>::max();
+    for (size_t i = 0; i < points.size(); ++i) {
+      const auto & lane_ids = points.at(i).lane_ids;
+
+      const double dist_to_goal = autoware_utils::calc_distance2d(points.at(i).point.pose, goal);
+      const bool is_goal_lane_id_in_point = exists(lane_ids, goal_lane_id);
+      if (dist_to_goal < max_dist && dist_to_goal < min_dist && is_goal_lane_id_in_point) {
+        min_dist_index = i;
+        min_dist = dist_to_goal;
+        found = true;
+      }
+    }
+    if (!found) {
+      return std::nullopt;
+    }
+  }
+
+  // Find index out of goal search range
+  size_t min_dist_out_of_range_index = min_dist_index;
+  for (int i = min_dist_index; 0 <= i; --i) {
+    const double dist = autoware_utils::calc_distance2d(points.at(i).point, goal);
+    min_dist_out_of_range_index = i;
+    if (max_dist < dist) {
+      break;
+    }
+  }
+
+  return min_dist_out_of_range_index;
+}
+
+// Clean up points around the goal for smooth goal connection
+// This function returns the cleaned up path. You need to add pre_goal and goal to the returned
+// path. This is because we'll do spline interpolation between the tail of the returned path and the
+// pre_goal later at this file.
+//   -
+//   https://github.com/tier4/autoware.universe/blob/908cb7ee5cca01c367f03caf6db4562a620504fb/planning/behavior_path_planner/autoware_behavior_path_planner/src/behavior_path_planner_node.cpp#L724-L725
+std::optional<PathWithLaneId> path_up_to_just_before_pre_goal(
+  const PathWithLaneId & input, const geometry_msgs::msg::Pose & goal,
+  const lanelet::Id goal_lane_id, const double search_radius_range)
+{
+  // Find min_dist_out_of_circle_index whose distance to goal is longer than search_radius_range
+  const auto min_dist_out_of_circle_index_opt =
+    find_index_out_of_goal_search_range(input.points, goal, goal_lane_id, search_radius_range);
+
+  // It seems we are almost at the goal as no point is found outside of the circle whose center is
+  // the goal
+  if (!min_dist_out_of_circle_index_opt) {
+    return std::nullopt;
+  }
+
+  // It seems we have a point outside of the circle whose center is the goal
+  const auto min_dist_out_of_circle_index = min_dist_out_of_circle_index_opt.value();
+
+  // Fill all the points up to just before the point outside of the circle
+  PathWithLaneId output;
+  output.points.reserve(output.points.size() + min_dist_out_of_circle_index + 3);
+  for (size_t i = 0; i <= min_dist_out_of_circle_index; ++i) {
+    output.points.push_back(input.points.at(i));
+  }
+
+  return output;
+}
+
+// Function to refine the path for the goal
+PathWithLaneId refine_path_for_goal(
+  const PathWithLaneId & input, const geometry_msgs::msg::Pose & goal,
+  const PlannerData & planner_data)
+{
+  PathWithLaneId filtered_path = input;
+
+  filtered_path.points = autoware::motion_utils::removeOverlapPoints(filtered_path.points);
+
+  // Always set zero velocity at the end of path for safety
+  if (!filtered_path.points.empty()) {
+    filtered_path.points.back().point.longitudinal_velocity_mps = 0.0;
+  }
+
+  // Clean up points around the goal for smooth goal connection
+  auto path_up_to_just_before_pre_goal_opt = path_up_to_just_before_pre_goal(
+    filtered_path, goal, planner_data.goal_lane_id,
+    planner_data.path_generator_parameters.refine_goal_search_radius_range);
+  if (!path_up_to_just_before_pre_goal_opt) {
+    // It seems we are almost at the goal and no need to clean up. Lets use the original path.
+    return input;
+  }
+
+  // Get the value from the optional
+  auto path_up_to_just_before_pre_goal = path_up_to_just_before_pre_goal_opt.value();
+
+  // Add pre_goal to the path
+  const auto lanes_opt = extract_lanelets_from_path(filtered_path, planner_data);
+  if (!lanes_opt) {
+    // It might be better to use the original path when the lanelets are not found
+    return input;
+  }
+  const auto lanes = lanes_opt.value();
+
+  // Reserve the size of the path + pre_goal + goal
+  path_up_to_just_before_pre_goal.points.reserve(path_up_to_just_before_pre_goal.points.size() + 2);
+
+  // Insert pre_goal to the path
+  path_up_to_just_before_pre_goal.points.push_back(prepare_pre_goal(goal, lanes));
+
+  // Insert goal (obtained from the tail of input path) to the cleaned up path
+  path_up_to_just_before_pre_goal.points.push_back(input.points.back());
+
+  // Finally, replace the goal point with the refined one
+  path_up_to_just_before_pre_goal.points.back().point.pose = goal;
+
+  // If necessary, do more fine tuning for goal connection here
+
+  return path_up_to_just_before_pre_goal;
+}
+
+std::optional<lanelet::ConstLanelets> extract_lanelets_from_path(
+  const PathWithLaneId & refined_path, const PlannerData & planner_data)
+{
+  lanelet::ConstLanelets refined_path_lanelets;
+  for (size_t i = 0; i < refined_path.points.size(); ++i) {
+    try {
+      const auto & path_point = refined_path.points.at(i);
+      int64_t lane_id = path_point.lane_ids.at(0);
+      lanelet::ConstLanelet lanelet = planner_data.lanelet_map_ptr->laneletLayer.get(lane_id);
+
+      const bool is_unique =
+        std::find(refined_path_lanelets.begin(), refined_path_lanelets.end(), lanelet) ==
+        refined_path_lanelets.end();
+
+      if (is_unique) {
+        refined_path_lanelets.push_back(lanelet);
+      }
+    } catch (const std::out_of_range & e) {
+      RCLCPP_ERROR(rclcpp::get_logger("path_generator"), "Out of range error: %s", e.what());
+      return std::nullopt;
+    }
+  }
+  return refined_path_lanelets;
+}
+
+std::optional<lanelet::ConstLanelet> get_goal_lanelet(const PlannerData & planner_data)
+{
+  const lanelet::Id goal_lane_id = planner_data.goal_lane_id;
+  for (const auto & llt : planner_data.route_lanelets) {
+    if (llt.id() == goal_lane_id) {
+      return llt;
+    }
+  }
+  return std::nullopt;
+}
+
+bool is_in_lanelets(const geometry_msgs::msg::Pose & pose, const lanelet::ConstLanelets & lanes)
+{
+  for (const auto & lane : lanes) {
+    if (lanelet::utils::isInLanelet(pose, lane)) {
+      return true;
+    }
+  }
+  return false;
+}
+
+bool is_path_valid(const PathWithLaneId & refined_path, const PlannerData & planner_data)
+{
+  // Extract lanelets from the refined path
+  const auto lanelets_opt = extract_lanelets_from_path(refined_path, planner_data);
+  if (!lanelets_opt) {
+    RCLCPP_ERROR(rclcpp::get_logger("path_generator"), "Failed to extract lanelets from path");
+    return false;
+  }
+  const auto & lanelets = lanelets_opt.value();
+
+  // std::any_of detects whether any point lies outside lanelets
+  const bool has_points_outside_lanelet = std::any_of(
+    refined_path.points.begin(), refined_path.points.end(),
+    [&lanelets](const auto & refined_path_point) {
+      return !is_in_lanelets(refined_path_point.point.pose, lanelets);
+    });
+
+  // Return true if no points lie outside the extracted lanelets
+  return !has_points_outside_lanelet;
+}
+
+PathWithLaneId modify_path_for_smooth_goal_connection(
+  const PathWithLaneId & path, const std::shared_ptr<const PlannerData> & planner_data)
+{
+  const auto goal = planner_data->goal_pose;
+
+  geometry_msgs::msg::Pose refined_goal{};
+  {
+    // Prevent from shadowVariable
+    const auto goal_lanelet = get_goal_lanelet(*planner_data);
+
+    // First, polish up the goal pose if possible
+    if (goal_lanelet) {
+      refined_goal = refine_goal(goal, *goal_lanelet);
+    } else {
+      refined_goal = goal;
+    }
+  }
+
+  bool is_valid_path{false};
+  PathWithLaneId refined_path;
+
+  // Then, refine the path for the goal
+  refined_path = refine_path_for_goal(path, refined_goal, *planner_data);
+  if (is_path_valid(refined_path, *planner_data)) {
+    is_valid_path = true;
+  }
+
+  // It is better to return the original path if the refined path is not valid
+  if (!is_valid_path) {
+    return path;
+  }
+  return refined_path;
+}
+
 TurnIndicatorsCommand get_turn_signal(
   const PathWithLaneId & path, const PlannerData & planner_data,
   const geometry_msgs::msg::Pose & current_pose, const double current_vel,