diff --git a/planning/mission_planner/config/mission_planner.param.yaml b/planning/mission_planner/config/mission_planner.param.yaml
index 9b7dcffbc687b..f86e5783845b0 100644
--- a/planning/mission_planner/config/mission_planner.param.yaml
+++ b/planning/mission_planner/config/mission_planner.param.yaml
@@ -4,9 +4,12 @@
     arrival_check_angle_deg: 45.0
     arrival_check_distance: 1.0
     arrival_check_duration: 1.0
+    start_angle_threshold_deg: 90.0
     goal_angle_threshold_deg: 45.0
     enable_correct_goal_pose: false
     reroute_time_threshold: 10.0
     minimum_reroute_length: 30.0
     consider_no_drivable_lanes: false # This flag is for considering no_drivable_lanes in planning or not.
     check_footprint_inside_lanes: true
+    prioritize_start_footprint: false # in case start checkpoint is inside multiple lanelet prioritize the one which start checkpoint footprint is within
+    prioritize_goal_footprint: false # in case goal checkpoint is inside multiple lanelet prioritize the one which goal checkpoint footprint is within
diff --git a/planning/mission_planner/src/lanelet2_plugins/default_planner.cpp b/planning/mission_planner/src/lanelet2_plugins/default_planner.cpp
index 673519b6f7a0e..a6934a74cbf08 100644
--- a/planning/mission_planner/src/lanelet2_plugins/default_planner.cpp
+++ b/planning/mission_planner/src/lanelet2_plugins/default_planner.cpp
@@ -149,11 +149,14 @@ void DefaultPlanner::initialize_common(rclcpp::Node * node)
     node_->create_publisher<MarkerArray>("debug/goal_footprint", durable_qos);
 
   vehicle_info_ = vehicle_info_util::VehicleInfoUtil(*node_).getVehicleInfo();
+  param_.start_angle_threshold_deg = node_->declare_parameter<double>("start_angle_threshold_deg");
   param_.goal_angle_threshold_deg = node_->declare_parameter<double>("goal_angle_threshold_deg");
   param_.enable_correct_goal_pose = node_->declare_parameter<bool>("enable_correct_goal_pose");
   param_.consider_no_drivable_lanes = node_->declare_parameter<bool>("consider_no_drivable_lanes");
   param_.check_footprint_inside_lanes =
     node_->declare_parameter<bool>("check_footprint_inside_lanes");
+  param_.prioritize_start_footprint = node_->declare_parameter<bool>("prioritize_start_footprint");
+  param_.prioritize_goal_footprint = node_->declare_parameter<bool>("prioritize_goal_footprint");
 }
 
 void DefaultPlanner::initialize(rclcpp::Node * node)
@@ -391,6 +394,12 @@ PlannerPlugin::LaneletRoute DefaultPlanner::plan(const RoutePoints & points)
 {
   const auto logger = node_->get_logger();
 
+  LaneletRoute route_msg;
+  if (points.size() < 2) {
+    RCLCPP_WARN_STREAM(logger, "Not enough points to plan a route!" << std::endl);
+    return route_msg;
+  }
+
   std::stringstream log_ss;
   for (const auto & point : points) {
     log_ss << "x: " << point.position.x << " "
@@ -400,16 +409,28 @@ PlannerPlugin::LaneletRoute DefaultPlanner::plan(const RoutePoints & points)
     logger, "start planning route with check points: " << std::endl
                                                        << log_ss.str());
 
-  LaneletRoute route_msg;
+  auto goal_pose = points.back();
+  if (param_.enable_correct_goal_pose) {
+    goal_pose = get_closest_centerline_pose(
+      lanelet::utils::query::laneletLayer(lanelet_map_ptr_), goal_pose, vehicle_info_);
+  }
+
   RouteSections route_sections;
 
   lanelet::ConstLanelets all_route_lanelets;
   for (std::size_t i = 1; i < points.size(); i++) {
+    bool is_start_pose = i == 0;
+    bool is_goal_pose = i == (points.size() - 1);
+
     const auto start_check_point = points.at(i - 1);
-    const auto goal_check_point = points.at(i);
+    const auto goal_check_point = is_goal_pose ? goal_pose : points.at(i);
+
     lanelet::ConstLanelets path_lanelets;
     if (!route_handler_.planPathLaneletsBetweenCheckpoints(
-          start_check_point, goal_check_point, &path_lanelets, param_.consider_no_drivable_lanes)) {
+          start_check_point, goal_check_point, &path_lanelets, is_start_pose, is_goal_pose,
+          &vehicle_info_, param_.start_angle_threshold_deg, param_.goal_angle_threshold_deg,
+          param_.prioritize_start_footprint, param_.prioritize_goal_footprint,
+          param_.consider_no_drivable_lanes)) {
       RCLCPP_WARN(logger, "Failed to plan route.");
       return route_msg;
     }
@@ -423,12 +444,6 @@ PlannerPlugin::LaneletRoute DefaultPlanner::plan(const RoutePoints & points)
   }
   route_handler_.setRouteLanelets(all_route_lanelets);
 
-  auto goal_pose = points.back();
-  if (param_.enable_correct_goal_pose) {
-    goal_pose = get_closest_centerline_pose(
-      lanelet::utils::query::laneletLayer(lanelet_map_ptr_), goal_pose, vehicle_info_);
-  }
-
   if (!is_goal_valid(goal_pose, all_route_lanelets)) {
     RCLCPP_WARN(logger, "Goal is not valid! Please check position and angle of goal_pose");
     return route_msg;
diff --git a/planning/mission_planner/src/lanelet2_plugins/default_planner.hpp b/planning/mission_planner/src/lanelet2_plugins/default_planner.hpp
index 251d0db533182..02e832e354356 100644
--- a/planning/mission_planner/src/lanelet2_plugins/default_planner.hpp
+++ b/planning/mission_planner/src/lanelet2_plugins/default_planner.hpp
@@ -35,10 +35,13 @@ namespace mission_planner::lanelet2
 
 struct DefaultPlannerParameters
 {
+  double start_angle_threshold_deg;
   double goal_angle_threshold_deg;
   bool enable_correct_goal_pose;
   bool consider_no_drivable_lanes;
   bool check_footprint_inside_lanes;
+  bool prioritize_start_footprint;
+  bool prioritize_goal_footprint;
 };
 
 class DefaultPlanner : public mission_planner::PlannerPlugin
diff --git a/planning/planning_test_utils/include/planning_interface_test_manager/planning_interface_test_manager_utils.hpp b/planning/planning_test_utils/include/planning_interface_test_manager/planning_interface_test_manager_utils.hpp
index 0af375c9d9c8a..54b67471e31cb 100644
--- a/planning/planning_test_utils/include/planning_interface_test_manager/planning_interface_test_manager_utils.hpp
+++ b/planning/planning_test_utils/include/planning_interface_test_manager/planning_interface_test_manager_utils.hpp
@@ -344,7 +344,7 @@ LaneletRoute makeBehaviorRouteFromLaneId(const int & start_lane_id, const int &
 
   // Plan the path between checkpoints (start and goal poses)
   lanelet::ConstLanelets path_lanelets;
-  if (!route_handler->planPathLaneletsBetweenCheckpoints(start_pose, goal_pose, &path_lanelets)) {
+  if (!route_handler->planPathLaneletsBetweenCheckpoints(start_pose, goal_pose, &path_lanelets, true, true)) {
     return route_msg;
   }
 
diff --git a/planning/route_handler/include/route_handler/route_handler.hpp b/planning/route_handler/include/route_handler/route_handler.hpp
index 7f7961f438284..db9d8f75186e9 100644
--- a/planning/route_handler/include/route_handler/route_handler.hpp
+++ b/planning/route_handler/include/route_handler/route_handler.hpp
@@ -16,6 +16,7 @@
 #define ROUTE_HANDLER__ROUTE_HANDLER_HPP_
 
 #include <rclcpp/logger.hpp>
+#include <vehicle_info_util/vehicle_info_util.hpp>
 
 #include <autoware_auto_mapping_msgs/msg/had_map_bin.hpp>
 #include <autoware_auto_planning_msgs/msg/path_with_lane_id.hpp>
@@ -81,7 +82,12 @@ class RouteHandler
   // for routing
   bool planPathLaneletsBetweenCheckpoints(
     const Pose & start_checkpoint, const Pose & goal_checkpoint,
-    lanelet::ConstLanelets * path_lanelets, const bool consider_no_drivable_lanes = false) const;
+    lanelet::ConstLanelets * path_lanelets, const bool is_start_pose, const bool is_goal_pose,
+    const vehicle_info_util::VehicleInfo * vehicle_info = nullptr,
+    const double start_angle_threshold_deg = std::numeric_limits<double>::max(),
+    const double goal_angle_threshold_deg = std::numeric_limits<double>::max(),
+    const bool prioritize_start_footprint = false, const bool prioritize_goal_footprint = false,
+    const bool consider_no_drivable_lanes = false) const;
   std::vector<LaneletSegment> createMapSegments(const lanelet::ConstLanelets & path_lanelets) const;
   static bool isRouteLooped(const RouteSections & route_sections);
 
diff --git a/planning/route_handler/package.xml b/planning/route_handler/package.xml
index 60adfb1531104..ee45856a6f84f 100644
--- a/planning/route_handler/package.xml
+++ b/planning/route_handler/package.xml
@@ -28,6 +28,7 @@
   <depend>rclcpp_components</depend>
   <depend>tf2_ros</depend>
   <depend>tier4_autoware_utils</depend>
+  <depend>vehicle_info_util</depend>
 
   <export>
     <build_type>ament_cmake</build_type>
diff --git a/planning/route_handler/src/route_handler.cpp b/planning/route_handler/src/route_handler.cpp
index 0e6464a81f354..9e0a908c03741 100644
--- a/planning/route_handler/src/route_handler.cpp
+++ b/planning/route_handler/src/route_handler.cpp
@@ -129,6 +129,109 @@ std::string toString(const geometry_msgs::msg::Pose & pose)
   return ss.str();
 }
 
+lanelet::ConstLanelet combine_lanelets(const lanelet::ConstLanelets & lanelets)
+{
+  lanelet::Points3d lefts;
+  lanelet::Points3d rights;
+  lanelet::Points3d centers;
+  std::vector<uint64_t> left_bound_ids;
+  std::vector<uint64_t> right_bound_ids;
+
+  for (const auto & llt : lanelets) {
+    if (llt.id() != 0) {
+      left_bound_ids.push_back(llt.leftBound().id());
+      right_bound_ids.push_back(llt.rightBound().id());
+    }
+  }
+
+  for (const auto & llt : lanelets) {
+    if (std::count(right_bound_ids.begin(), right_bound_ids.end(), llt.leftBound().id()) < 1) {
+      for (const auto & pt : llt.leftBound()) {
+        lefts.push_back(lanelet::Point3d(pt));
+      }
+    }
+    if (std::count(left_bound_ids.begin(), left_bound_ids.end(), llt.rightBound().id()) < 1) {
+      for (const auto & pt : llt.rightBound()) {
+        rights.push_back(lanelet::Point3d(pt));
+      }
+    }
+  }
+  const auto left_bound = lanelet::LineString3d(lanelet::InvalId, lefts);
+  const auto right_bound = lanelet::LineString3d(lanelet::InvalId, rights);
+  auto combined_lanelet = lanelet::Lanelet(lanelet::InvalId, left_bound, right_bound);
+  return std::move(combined_lanelet);
+}
+
+bool is_pose_footprint_inside_path_lanelets(
+  const lanelet::routing::RoutingGraphPtr routing_graph_ptr,
+  const lanelet::routing::LaneletPath & lanelet_path, const Pose & pose,
+  const vehicle_info_util::VehicleInfo * vehicle_info, const double search_margin = 2.0)
+{
+  // Redefine is_within function for an easier usage
+  const auto is_within =
+    [](const tier4_autoware_utils::LinearRing2d & ring, const lanelet::ConstLanelet & lanelet) {
+      return boost::geometry::within(ring, lanelet.polygon2d().basicPolygon());
+    };
+
+  // Create vehicle footprint projected into pose
+  const auto local_footprint = vehicle_info->createFootprint();
+  tier4_autoware_utils::LinearRing2d pose_footprint =
+    transformVector(local_footprint, tier4_autoware_utils::pose2transform(pose));
+
+  // Create combined path lanelet
+  lanelet::ConstLanelets path_lanelets;
+  path_lanelets.reserve(lanelet_path.size());
+  for (const auto & llt : lanelet_path) {
+    path_lanelets.push_back(llt);
+  }
+  auto const combined_path_lanelet = combine_lanelets(path_lanelets);
+
+  // Check if pose footprint is inside combined path lanelet
+  if (is_within(pose_footprint, combined_path_lanelet)) {
+    return true;
+  }
+
+  double min_expand_length = vehicle_info->vehicle_length_m + search_margin;
+
+  // Expand path start backwards by vehicle length + search_margin
+  double backward_expanded_path_length = 0.0;
+  for (const auto & previous_lanelet : routing_graph_ptr->previous(path_lanelets.front())) {
+    path_lanelets.insert(path_lanelets.begin(), previous_lanelet);
+    backward_expanded_path_length += lanelet::utils::getLaneletLength2d(previous_lanelet);
+
+    if (min_expand_length < backward_expanded_path_length) {
+      break;
+    }
+  }
+
+  // Expand path end forward by vehicle length + search_margin
+  double forward_expanded_path_length = 0.0;
+  for (const auto & next_lanelet : routing_graph_ptr->following(path_lanelets.back())) {
+    path_lanelets.push_back(next_lanelet);
+    forward_expanded_path_length += lanelet::utils::getLaneletLength2d(next_lanelet);
+
+    if (min_expand_length < forward_expanded_path_length) {
+      break;
+    }
+  }
+
+  if (backward_expanded_path_length > 0.0 || forward_expanded_path_length > 0.0) {
+    // Create combined expanded path lanelet
+    path_lanelets.reserve(lanelet_path.size());
+    for (const auto & llt : lanelet_path) {
+      path_lanelets.push_back(llt);
+    }
+    auto const combined_expanded_path_lanelet = combine_lanelets(path_lanelets);
+
+    // Check if pose footprint is inside combined expanded path lanelet
+    if (is_within(pose_footprint, combined_expanded_path_lanelet)) {
+      return true;
+    }
+  }
+
+  return false;
+}
+
 }  // namespace
 
 namespace route_handler
@@ -2090,20 +2193,23 @@ lanelet::ConstLanelets RouteHandler::getNextLaneSequence(
 
 bool RouteHandler::planPathLaneletsBetweenCheckpoints(
   const Pose & start_checkpoint, const Pose & goal_checkpoint,
-  lanelet::ConstLanelets * path_lanelets, const bool consider_no_drivable_lanes) const
+  lanelet::ConstLanelets * path_lanelets, const bool is_start_pose, const bool is_goal_pose,
+  const vehicle_info_util::VehicleInfo * vehicle_info, const double start_angle_threshold_deg,
+  const double goal_angle_threshold_deg, const bool prioritize_start_footprint,
+  const bool prioritize_goal_footprint, const bool consider_no_drivable_lanes) const
 {
   // Find lanelets for start point. First, find all lanelets containing the start point to calculate
   // all possible route later. It fails when the point is not located on any road_lanelet (e.g. the
   // start point is located out of any lanelets or road_shoulder lanelet which is not contained in
   // road_lanelet). In that case, find the closest lanelet instead.
-  lanelet::ConstLanelet start_lanelet;
   lanelet::ConstLanelets start_lanelets;
   if (!lanelet::utils::query::getCurrentLanelets(
         road_lanelets_, start_checkpoint, &start_lanelets)) {
+    lanelet::ConstLanelet start_lanelet;
     if (!lanelet::utils::query::getClosestLanelet(
           road_lanelets_, start_checkpoint, &start_lanelet)) {
       RCLCPP_WARN_STREAM(
-        logger_, "Failed to find current lanelet."
+        logger_, "Failed to find start lanelet."
                    << std::endl
                    << " - start checkpoint: " << toString(start_checkpoint) << std::endl
                    << " - goal checkpoint: " << toString(goal_checkpoint) << std::endl);
@@ -2112,83 +2218,166 @@ bool RouteHandler::planPathLaneletsBetweenCheckpoints(
     start_lanelets = {start_lanelet};
   }
 
-  // Find lanelets for goal point.
-  lanelet::ConstLanelet goal_lanelet;
-  if (!lanelet::utils::query::getClosestLanelet(road_lanelets_, goal_checkpoint, &goal_lanelet)) {
-    RCLCPP_WARN_STREAM(
-      logger_, "Failed to find closest lanelet."
-                 << std::endl
-                 << " - start checkpoint: " << toString(start_checkpoint) << std::endl
-                 << " - goal checkpoint: " << toString(goal_checkpoint) << std::endl);
-    return false;
+  // Find lanelets for goal point. First, find all lanelets containing the goal point to calculate
+  // all possible route later. It fails when the point is not located on any road_lanelet (e.g. the
+  // goal point is located out of any lanelets or road_shoulder lanelet which is not contained in
+  // road_lanelet). In that case, find the closest lanelet instead.
+  lanelet::ConstLanelets goal_lanelets;
+  if (!lanelet::utils::query::getCurrentLanelets(road_lanelets_, goal_checkpoint, &goal_lanelets)) {
+    lanelet::ConstLanelet goal_lanelet;
+    if (!lanelet::utils::query::getClosestLanelet(road_lanelets_, goal_checkpoint, &goal_lanelet)) {
+      RCLCPP_WARN_STREAM(
+        logger_, "Failed to find goal lanelet."
+                   << std::endl
+                   << " - start checkpoint: " << toString(start_checkpoint) << std::endl
+                   << " - goal checkpoint: " << toString(goal_checkpoint) << std::endl);
+      return false;
+    }
+    goal_lanelets = {goal_lanelet};
   }
 
-  lanelet::Optional<lanelet::routing::Route> optional_route;
-  std::vector<lanelet::ConstLanelets> candidate_paths;
-  lanelet::routing::LaneletPath shortest_path;
-  bool is_route_found = false;
+  struct PathInfo
+  {
+    lanelet::ConstLanelet start_lanelet;
+    lanelet::ConstLanelet goal_lanelet;
+    lanelet::routing::LaneletPath lanelet_path;
+    double path_length2d = std::numeric_limits<double>::max();
+    bool is_start_pose_footprint_inside_path_lanelets = false;
+    bool is_goal_pose_footprint_inside_path_lanelets = false;
+  };
+  std::vector<PathInfo> candidate_paths;
 
-  lanelet::routing::LaneletPath drivable_lane_path;
-  bool drivable_lane_path_found = false;
-  double shortest_path_length2d = std::numeric_limits<double>::max();
-
-  for (const auto & st_llt : start_lanelets) {
-    // check if the angle difference between start_checkpoint and start lanelet center line
-    // orientation is in yaw_threshold range
-    double yaw_threshold = M_PI / 2.0;
-    bool is_proper_angle = false;
-    {
-      double lanelet_angle = lanelet::utils::getLaneletAngle(st_llt, start_checkpoint.position);
-      double pose_yaw = tf2::getYaw(start_checkpoint.orientation);
-      double angle_diff = std::abs(autoware_utils::normalize_radian(lanelet_angle - pose_yaw));
-
-      if (angle_diff <= std::abs(yaw_threshold)) {
-        is_proper_angle = true;
+  bool is_route_found = false;
+  for (const auto & gl_llt : goal_lanelets) {
+    for (const auto & st_llt : start_lanelets) {
+      // check if the angle difference between start_pose and start lanelet center line
+      // orientation is in yaw_threshold range
+      bool is_start_proper_angle = false;
+      if (is_start_pose) {
+        double yaw_threshold_rad = start_angle_threshold_deg * M_PI / 180.0;
+        {
+          double lanelet_angle = lanelet::utils::getLaneletAngle(st_llt, start_checkpoint.position);
+          double pose_yaw = tf2::getYaw(start_checkpoint.orientation);
+          double angle_diff = std::abs(autoware_utils::normalize_radian(lanelet_angle - pose_yaw));
+
+          if (angle_diff <= std::abs(yaw_threshold_rad)) {
+            is_start_proper_angle = true;
+          }
+        }
+      } else {
+        is_start_proper_angle = true;
       }
-    }
 
-    optional_route = routing_graph_ptr_->getRoute(st_llt, goal_lanelet, 0);
-    if (!optional_route || !is_proper_angle) {
-      RCLCPP_ERROR_STREAM(
-        logger_, "Failed to find a proper route!"
-                   << std::endl
-                   << " - start checkpoint: " << toString(start_checkpoint) << std::endl
-                   << " - goal checkpoint: " << toString(goal_checkpoint) << std::endl
-                   << " - start lane id: " << st_llt.id() << std::endl
-                   << " - goal lane id: " << goal_lanelet.id() << std::endl);
-    } else {
-      is_route_found = true;
+      // check if the angle difference between goal_pose and goal lanelet center line
+      // orientation is in yaw_threshold range
+      bool is_goal_proper_angle = false;
+      if (is_goal_pose) {
+        double yaw_threshold_rad = goal_angle_threshold_deg * M_PI / 180.0;
+        {
+          double lanelet_angle = lanelet::utils::getLaneletAngle(gl_llt, goal_checkpoint.position);
+          double pose_yaw = tf2::getYaw(goal_checkpoint.orientation);
+          double angle_diff = std::abs(autoware_utils::normalize_radian(lanelet_angle - pose_yaw));
+
+          if (angle_diff <= std::abs(yaw_threshold_rad)) {
+            is_goal_proper_angle = true;
+          }
+        }
+      } else {
+        is_goal_proper_angle = true;
+      }
 
-      if (optional_route->length2d() < shortest_path_length2d) {
-        shortest_path_length2d = optional_route->length2d();
-        shortest_path = optional_route->shortestPath();
-        start_lanelet = st_llt;
+      // Find the shortest route available from start lanelet to goal lanelet
+      auto optional_route = routing_graph_ptr_->getRoute(st_llt, gl_llt, 0);
+      if (!optional_route || !is_start_proper_angle || !is_goal_proper_angle) {
+        RCLCPP_WARN_STREAM(
+          logger_, "Failed to find a proper route!"
+                     << std::endl
+                     << " - start checkpoint: " << toString(start_checkpoint) << std::endl
+                     << " - goal checkpoint: " << toString(goal_checkpoint) << std::endl
+                     << " - start lane id: " << st_llt.id() << std::endl
+                     << " - goal lane id: " << gl_llt.id() << std::endl);
+      } else {
+        is_route_found = true;
+        PathInfo candidate_path{
+          st_llt, gl_llt, optional_route->shortestPath(), optional_route->length2d()};
+        candidate_paths.push_back(candidate_path);
       }
     }
   }
 
   if (is_route_found) {
-    lanelet::routing::LaneletPath path;
+    // sort by shortest path length
+    std::sort(
+      candidate_paths.begin(), candidate_paths.end(),
+      [](const PathInfo & x, const PathInfo & y) { return x.path_length2d < y.path_length2d; });
+
+    if (prioritize_start_footprint || prioritize_goal_footprint) {
+      // Get if checkpoint pose footprints are inside of path lanelets
+      if (vehicle_info != nullptr) {
+        for (auto & candidate_path : candidate_paths) {
+          candidate_path.is_start_pose_footprint_inside_path_lanelets =
+            is_pose_footprint_inside_path_lanelets(
+              routing_graph_ptr_, candidate_path.lanelet_path, start_checkpoint, vehicle_info);
+
+          candidate_path.is_goal_pose_footprint_inside_path_lanelets =
+            is_pose_footprint_inside_path_lanelets(
+              routing_graph_ptr_, candidate_path.lanelet_path, goal_checkpoint, vehicle_info);
+        }
+
+        if (prioritize_start_footprint) {
+          // sort by whether start pose footprint is inside path lanelets
+          std::sort(
+            candidate_paths.begin(), candidate_paths.end(),
+            [](const PathInfo & x, const PathInfo & y) {
+              return x.is_start_pose_footprint_inside_path_lanelets &&
+                     !y.is_start_pose_footprint_inside_path_lanelets;
+            });
+        }
+
+        if (prioritize_goal_footprint) {
+          // sort by whether goal pose footprint is inside path lanelets
+          std::sort(
+            candidate_paths.begin(), candidate_paths.end(),
+            [](const PathInfo & x, const PathInfo & y) {
+              return x.is_goal_pose_footprint_inside_path_lanelets &&
+                     !y.is_goal_pose_footprint_inside_path_lanelets;
+            });
+        }
+      } else {
+        RCLCPP_WARN_STREAM(
+          logger_,
+          "Vehicle information is not retrieved. Bypassing prioritizing start & goal footprint."
+            << std::endl);
+      }
+    }
+
+    PathInfo preferred_path = candidate_paths.front();
+
     if (consider_no_drivable_lanes) {
-      bool shortest_path_has_no_drivable_lane = hasNoDrivableLaneInPath(shortest_path);
+      lanelet::routing::LaneletPath drivable_lane_path;
+      auto drivable_lane_path_found = false;
+      auto shortest_path_has_no_drivable_lane =
+        hasNoDrivableLaneInPath(preferred_path.lanelet_path);
       if (shortest_path_has_no_drivable_lane) {
-        drivable_lane_path_found =
-          findDrivableLanePath(start_lanelet, goal_lanelet, drivable_lane_path);
+        drivable_lane_path_found = findDrivableLanePath(
+          preferred_path.start_lanelet, preferred_path.goal_lanelet, drivable_lane_path);
       }
 
       if (drivable_lane_path_found) {
-        path = drivable_lane_path;
-      } else {
-        path = shortest_path;
+        preferred_path.lanelet_path = drivable_lane_path;
       }
-    } else {
-      path = shortest_path;
     }
 
-    path_lanelets->reserve(path.size());
-    for (const auto & llt : path) {
+    path_lanelets->reserve(preferred_path.lanelet_path.size());
+    for (const auto & llt : preferred_path.lanelet_path) {
       path_lanelets->push_back(llt);
     }
+  } else {
+    RCLCPP_ERROR_STREAM(
+      logger_, "Failed to find a proper route!"
+                 << std::endl
+                 << " - start checkpoint: " << toString(start_checkpoint) << std::endl
+                 << " - goal checkpoint: " << toString(goal_checkpoint) << std::endl);
   }
 
   return is_route_found;