feat(lane_departure_checker,start_planner): add check for path within lanes for bvspm

common/tier4_autoware_utils/include/tier4_autoware_utils/geometry/boost_geometry.hpp

@@ -18,6 +18,7 @@
 #include <boost/geometry/core/cs.hpp>
 #include <boost/geometry/geometries/geometries.hpp>
 #include <boost/geometry/geometries/register/point.hpp>
+#include <boost/geometry/geometries/register/ring.hpp>
 
 #define EIGEN_MPL2_ONLY
 #include <Eigen/Core>
@@ -98,5 +99,6 @@ BOOST_GEOMETRY_REGISTER_POINT_2D(                                       // NOLIN
   tier4_autoware_utils::Point2d, double, cs::cartesian, x(), y())       // NOLINT
 BOOST_GEOMETRY_REGISTER_POINT_3D(                                       // NOLINT
   tier4_autoware_utils::Point3d, double, cs::cartesian, x(), y(), z())  // NOLINT
+BOOST_GEOMETRY_REGISTER_RING(tier4_autoware_utils::LinearRing2d)        // NOLINT
 
 #endif  // TIER4_AUTOWARE_UTILS__GEOMETRY__BOOST_GEOMETRY_HPP_

control/lane_departure_checker/include/lane_departure_checker/lane_departure_checker.hpp

@@ -29,14 +29,20 @@
 #include <geometry_msgs/msg/twist_stamped.hpp>
 #include <nav_msgs/msg/odometry.hpp>
 
+#include <boost/geometry/algorithms/envelope.hpp>
+#include <boost/geometry/algorithms/union.hpp>
 #include <boost/geometry/index/rtree.hpp>
-#include <boost/optional.hpp>
 
 #include <lanelet2_core/LaneletMap.h>
+#include <lanelet2_core/geometry/BoundingBox.h>
+#include <lanelet2_core/geometry/LaneletMap.h>
+#include <lanelet2_core/geometry/LineString.h>
+#include <lanelet2_core/geometry/Polygon.h>
 
 #include <map>
 #include <memory>
 #include <string>
+#include <utility>
 #include <vector>
 
 namespace lane_departure_checker
@@ -112,6 +118,19 @@ class LaneDepartureChecker
   bool checkPathWillLeaveLane(
     const lanelet::ConstLanelets & lanelets, const PathWithLaneId & path) const;
 
+  std::vector<std::pair<double, lanelet::Lanelet>> getLaneletsFromPath(
+    const lanelet::LaneletMapPtr lanelet_map_ptr, const PathWithLaneId & path) const;
+
+  std::optional<lanelet::BasicPolygon2d> getFusedLaneletPolygonForPath(
+    const lanelet::LaneletMapPtr lanelet_map_ptr, const PathWithLaneId & path) const;
+
+  bool checkPathWillLeaveLane(
+    const lanelet::LaneletMapPtr lanelet_map_ptr, const PathWithLaneId & path) const;
+
+  PathWithLaneId cropPointsOutsideOfLanes(
+    const lanelet::LaneletMapPtr lanelet_map_ptr, const PathWithLaneId & path,
+    const size_t end_index);
+
   static bool isOutOfLane(
     const lanelet::ConstLanelets & candidate_lanelets, const LinearRing2d & vehicle_footprint);
 

control/lane_departure_checker/src/lane_departure_checker_node/lane_departure_checker.cpp

@@ -34,6 +34,7 @@
 using tier4_autoware_utils::LinearRing2d;
 using tier4_autoware_utils::LineString2d;
 using tier4_autoware_utils::MultiPoint2d;
+using tier4_autoware_utils::MultiPolygon2d;
 using tier4_autoware_utils::Point2d;
 
 namespace
@@ -92,6 +93,7 @@
 
   return candidate_lanelets;
 }
+
 }  // namespace
 
 namespace lane_departure_checker
@@ -298,6 +300,92 @@
   return false;
 }
 
+std::vector<std::pair<double, lanelet::Lanelet>> LaneDepartureChecker::getLaneletsFromPath(
+  const lanelet::LaneletMapPtr lanelet_map_ptr, const PathWithLaneId & path) const
+{
+  // Get Footprint Hull basic polygon
+  std::vector<LinearRing2d> vehicle_footprints = createVehicleFootprints(path);
+  LinearRing2d footprint_hull = createHullFromFootprints(vehicle_footprints);
+  auto to_basic_polygon = [](const LinearRing2d & footprint_hull) -> lanelet::BasicPolygon2d {
+    lanelet::BasicPolygon2d basic_polygon;
+    for (const auto & point : footprint_hull) {
+      Eigen::Vector2d p(point.x(), point.y());
+      basic_polygon.push_back(p);
+    }
+    return basic_polygon;
+  };
+  lanelet::BasicPolygon2d footprint_hull_basic_polygon = to_basic_polygon(footprint_hull);
+
+  // Find all lanelets that intersect the footprint hull
+  return lanelet::geometry::findWithin2d(
+    lanelet_map_ptr->laneletLayer, footprint_hull_basic_polygon, 0.0);
+}
+
+std::optional<lanelet::BasicPolygon2d> LaneDepartureChecker::getFusedLaneletPolygonForPath(
+  const lanelet::LaneletMapPtr lanelet_map_ptr, const PathWithLaneId & path) const
+{
+  const auto lanelets_distance_pair = getLaneletsFromPath(lanelet_map_ptr, path);
+  // Fuse lanelets into a single BasicPolygon2d
+  auto fused_lanelets = [&lanelets_distance_pair]() -> std::optional<lanelet::BasicPolygon2d> {
+    if (lanelets_distance_pair.empty()) return std::nullopt;
+    if (lanelets_distance_pair.size() == 1)
+      return lanelets_distance_pair.at(0).second.polygon2d().basicPolygon();
+
+    lanelet::BasicPolygon2d merged_polygon =
+      lanelets_distance_pair.at(0).second.polygon2d().basicPolygon();
+    for (size_t i = 1; i < lanelets_distance_pair.size(); ++i) {
+      const auto & route_lanelet = lanelets_distance_pair.at(i).second;
+      const auto & poly = route_lanelet.polygon2d().basicPolygon();
+
+      std::vector<lanelet::BasicPolygon2d> lanelet_union_temp;
+      boost::geometry::union_(poly, merged_polygon, lanelet_union_temp);
+
+      // Update merged_polygon by accumulating all merged results
+      merged_polygon.clear();
+      for (const auto & temp_poly : lanelet_union_temp) {
+        merged_polygon.insert(merged_polygon.end(), temp_poly.begin(), temp_poly.end());
+      }
+    }
+    if (merged_polygon.empty()) return std::nullopt;
+    return merged_polygon;
+  }();
+
+  return fused_lanelets;
+}
+
+bool LaneDepartureChecker::checkPathWillLeaveLane(
+  const lanelet::LaneletMapPtr lanelet_map_ptr, const PathWithLaneId & path) const
+{
+  // check if the footprint is not fully contained within the fused lanelets polygon
+  const std::vector<LinearRing2d> vehicle_footprints = createVehicleFootprints(path);
+  const auto fused_lanelets_polygon = getFusedLaneletPolygonForPath(lanelet_map_ptr, path);
+  if (!fused_lanelets_polygon) return true;
+  return !std::all_of(
+    vehicle_footprints.begin(), vehicle_footprints.end(),
+    [&fused_lanelets_polygon](const auto & footprint) {
+      return boost::geometry::within(footprint, fused_lanelets_polygon.value());
+    });
+}
+
+PathWithLaneId LaneDepartureChecker::cropPointsOutsideOfLanes(
+  const lanelet::LaneletMapPtr lanelet_map_ptr, const PathWithLaneId & path, const size_t end_index)
+{
+  PathWithLaneId temp_path;
+  const auto fused_lanelets_polygon = getFusedLaneletPolygonForPath(lanelet_map_ptr, path);
+  if (path.points.empty() || !fused_lanelets_polygon) return temp_path;
+  const auto vehicle_footprints = createVehicleFootprints(path);
+  size_t idx = 0;
+  std::for_each(vehicle_footprints.begin(), vehicle_footprints.end(), [&](const auto & footprint) {
+    if (idx > end_index || boost::geometry::within(footprint, fused_lanelets_polygon.value())) {
+      temp_path.points.push_back(path.points.at(idx));
+    }
+    ++idx;
+  });
+  PathWithLaneId cropped_path = path;
+  cropped_path.points = temp_path.points;
+  return cropped_path;
+}
+
 bool LaneDepartureChecker::isOutOfLane(
   const lanelet::ConstLanelets & candidate_lanelets, const LinearRing2d & vehicle_footprint)
 {
@@ -364,4 +452,5 @@
   }
   return false;
 }
+
 }  // namespace lane_departure_checker

planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/parking_departure/geometric_parallel_parking.hpp

@@ -18,6 +18,8 @@
 #include "behavior_path_planner_common/data_manager.hpp"
 #include "behavior_path_planner_common/parameters.hpp"
 
+#include <lane_departure_checker/lane_departure_checker.hpp>
+
 #include <autoware_auto_perception_msgs/msg/predicted_objects.hpp>
 #include <autoware_auto_planning_msgs/msg/path_with_lane_id.hpp>
 #include <geometry_msgs/msg/point.hpp>
@@ -72,7 +74,8 @@ class GeometricParallelParking
     const bool left_side_parking);
   bool planPullOut(
     const Pose & start_pose, const Pose & goal_pose, const lanelet::ConstLanelets & road_lanes,
-    const lanelet::ConstLanelets & shoulder_lanes, const bool left_side_start);
+    const lanelet::ConstLanelets & shoulder_lanes, const bool left_side_start,
+    const std::shared_ptr<lane_departure_checker::LaneDepartureChecker> lane_departure_checker);
   void setParameters(const ParallelParkingParameters & parameters) { parameters_ = parameters; }
   void setPlannerData(const std::shared_ptr<const PlannerData> & planner_data)
   {
@@ -115,7 +118,8 @@ class GeometricParallelParking
     const Pose & start_pose, const Pose & goal_pose, const double R_E_far,
     const lanelet::ConstLanelets & road_lanes, const lanelet::ConstLanelets & shoulder_lanes,
     const bool is_forward, const bool left_side_parking, const double end_pose_offset,
-    const double lane_departure_margin, const double arc_path_interval);
+    const double lane_departure_margin, const double arc_path_interval,
+    const std::shared_ptr<lane_departure_checker::LaneDepartureChecker> lane_departure_checker);
   PathWithLaneId generateArcPath(
     const Pose & center, const double radius, const double start_yaw, double end_yaw,
     const double arc_path_interval, const bool is_left_turn, const bool is_forward);

planning/behavior_path_planner_common/package.xml

@@ -49,6 +49,7 @@
   <depend>freespace_planning_algorithms</depend>
   <depend>geometry_msgs</depend>
   <depend>interpolation</depend>
+  <depend>lane_departure_checker</depend>
   <depend>lanelet2_extension</depend>
   <depend>magic_enum</depend>
   <depend>motion_utils</depend>

planning/behavior_path_planner_common/src/utils/parking_departure/geometric_parallel_parking.cpp

@@ -1,4 +1,4 @@
 // Copyright 2022 TIER IV, Inc.
 //
 // Licensed under the Apache License, Version 2.0 (the "License");
 // you may not use this file except in compliance with the License.
@@ -116,7 +116,7 @@
                                               : parameters_.backward_parking_path_interval;
   auto arc_paths = planOneTrial(
     start_pose, goal_pose, R_E_far, road_lanes, shoulder_lanes, is_forward, left_side_parking,
-    end_pose_offset, lane_departure_margin, arc_path_interval);
+    end_pose_offset, lane_departure_margin, arc_path_interval, {});
   if (arc_paths.empty()) {
     return std::vector<PathWithLaneId>{};
   }
@@ -222,27 +222,28 @@
 
 bool GeometricParallelParking::planPullOut(
   const Pose & start_pose, const Pose & goal_pose, const lanelet::ConstLanelets & road_lanes,
-  const lanelet::ConstLanelets & shoulder_lanes, const bool left_side_start)
+  const lanelet::ConstLanelets & shoulder_lanes, const bool left_side_start,
+  const std::shared_ptr<lane_departure_checker::LaneDepartureChecker> lane_departure_checker)
 {
   constexpr bool is_forward = false;         // parking backward means pull_out forward
   constexpr double start_pose_offset = 0.0;  // start_pose is current_pose
   constexpr double max_offset = 10.0;
   constexpr double offset_interval = 1.0;
 
   for (double end_pose_offset = 0; end_pose_offset < max_offset;
        end_pose_offset += offset_interval) {
     // pull_out end pose which is the second arc path end
     const auto end_pose =
       calcStartPose(start_pose, road_lanes, end_pose_offset, R_E_min_, is_forward, left_side_start);
     if (!end_pose) {
       continue;
     }
 
     // plan reverse path of parking. end_pose <-> start_pose
     auto arc_paths = planOneTrial(
       *end_pose, start_pose, R_E_min_, road_lanes, shoulder_lanes, is_forward, left_side_start,
       start_pose_offset, parameters_.pull_out_lane_departure_margin,
-      parameters_.pull_out_path_interval);
+      parameters_.pull_out_path_interval, lane_departure_checker);
     if (arc_paths.empty()) {
       // not found path
       continue;
@@ -362,140 +363,159 @@
   const Pose & start_pose, const Pose & goal_pose, const double R_E_far,
   const lanelet::ConstLanelets & road_lanes, const lanelet::ConstLanelets & shoulder_lanes,
   const bool is_forward, const bool left_side_parking, const double end_pose_offset,
-  const double lane_departure_margin, const double arc_path_interval)
+  const double lane_departure_margin, const double arc_path_interval,
+  const std::shared_ptr<lane_departure_checker::LaneDepartureChecker> lane_departure_checker)
 {
   clearPaths();
 
   const auto & common_params = planner_data_->parameters;
   const auto & route_handler = planner_data_->route_handler;
 
   const Pose arc_end_pose = calcOffsetPose(goal_pose, end_pose_offset, 0, 0);
   const double self_yaw = tf2::getYaw(start_pose.orientation);
   const double goal_yaw = tf2::getYaw(arc_end_pose.orientation);
   const double psi = normalizeRadian(self_yaw - goal_yaw);
 
   const Pose C_far = left_side_parking ? calcOffsetPose(arc_end_pose, 0, -R_E_far, 0)
                                        : calcOffsetPose(arc_end_pose, 0, R_E_far, 0);
   const double d_C_far_Einit = calcDistance2d(C_far, start_pose);
 
   const Point C_far_goal_coords = inverseTransformPoint(C_far.position, arc_end_pose);
   const Point self_point_goal_coords = inverseTransformPoint(start_pose.position, arc_end_pose);
 
   const double alpha =
     left_side_parking
       ? M_PI_2 - psi + std::asin((self_point_goal_coords.y - C_far_goal_coords.y) / d_C_far_Einit)
       : M_PI_2 + psi - std::asin((self_point_goal_coords.y - C_far_goal_coords.y) / d_C_far_Einit);
 
   const double R_E_near = (std::pow(d_C_far_Einit, 2) - std::pow(R_E_far, 2)) /
                           (2 * (R_E_far + d_C_far_Einit * std::cos(alpha)));
   if (R_E_near <= 0) {
     return std::vector<PathWithLaneId>{};
   }
 
   // combine road and shoulder lanes
   // cut the road lanes up to start_pose to prevent unintended processing for overlapped lane
   lanelet::ConstLanelets lanes{};
   tier4_autoware_utils::Point2d start_point2d(start_pose.position.x, start_pose.position.y);
   for (const auto & lane : road_lanes) {
     if (boost::geometry::within(start_point2d, lane.polygon2d().basicPolygon())) {
       lanes.push_back(lane);
       break;
     }
     lanes.push_back(lane);
   }
   lanes.insert(lanes.end(), shoulder_lanes.begin(), shoulder_lanes.end());
 
   // If start_pose is parallel to goal_pose, we can know lateral deviation of edges of vehicle,
   // and detect lane departure.
   if (is_forward) {  // Check near bound
     const double R_front_near =
       std::hypot(R_E_far + common_params.vehicle_width / 2, common_params.base_link2front);
     const double distance_to_near_bound =
       utils::getSignedDistanceFromBoundary(shoulder_lanes, arc_end_pose, left_side_parking);
     const double near_deviation = R_front_near - R_E_far;
     if (std::abs(distance_to_near_bound) - near_deviation < lane_departure_margin) {
       return std::vector<PathWithLaneId>{};
     }
   } else {  // Check far bound
     const double R_front_far =
       std::hypot(R_E_near + common_params.vehicle_width / 2, common_params.base_link2front);
     const double far_deviation = R_front_far - R_E_near;
     const double distance_to_far_bound =
       utils::getSignedDistanceFromBoundary(lanes, start_pose, !left_side_parking);
     if (std::abs(distance_to_far_bound) - far_deviation < lane_departure_margin) {
       return std::vector<PathWithLaneId>{};
     }
   }
 
   // Generate arc path(first turn -> second turn)
   const Pose C_near = left_side_parking ? calcOffsetPose(start_pose, 0, R_E_near, 0)
                                         : calcOffsetPose(start_pose, 0, -R_E_near, 0);
   const double theta_near =
     std::acos(
       (std::pow(R_E_near, 2) + std::pow(R_E_near + R_E_far, 2) - std::pow(d_C_far_Einit, 2)) /
       (2 * R_E_near * (R_E_near + R_E_far))) *
     (is_forward == left_side_parking ? 1 : -1);
 
   const auto generateArcPathWithHeader =
     [&](
       const auto & C, const auto & R_E, const auto & start_angle, const auto & end_angle,
       bool is_forward_first, bool is_forward_second) -> PathWithLaneId {
     auto path = generateArcPath(
       C, R_E, start_angle, end_angle, arc_path_interval, is_forward_first, is_forward_second);
     path.header = route_handler->getRouteHeader();
     return path;
   };
 
   PathWithLaneId path_turn_first =
     left_side_parking
       ? generateArcPathWithHeader(
           C_near, R_E_near, -M_PI_2, normalizeRadian(-M_PI_2 + theta_near), is_forward, is_forward)
       : generateArcPathWithHeader(
           C_near, R_E_near, M_PI_2, normalizeRadian(M_PI_2 + theta_near), !is_forward, is_forward);
 
   PathWithLaneId path_turn_second =
     left_side_parking ? generateArcPathWithHeader(
                           C_far, R_E_far, normalizeRadian(psi + M_PI_2 + theta_near), M_PI_2,
                           !is_forward, is_forward)
                       : generateArcPathWithHeader(
                           C_far, R_E_far, normalizeRadian(psi - M_PI_2 + theta_near), -M_PI_2,
                           is_forward, is_forward);
 
   // Need to add straight path to last right_turning for parking in parallel
   if (std::abs(end_pose_offset) > 0) {
     PathPointWithLaneId straight_point{};
     straight_point.point.pose = goal_pose;
     path_turn_second.points.push_back(straight_point);
   }
 
   // Populate lane ids for a given path.
   // It checks if each point in the path is within a lane
   // and if its ID hasn't been added yet, it appends the ID to the container.
   std::vector<lanelet::Id> path_lane_ids;
   const auto populateLaneIds = [&](const auto & path) {
     for (const auto & p : path.points) {
       for (const auto & lane : lanes) {
         if (
           lanelet::utils::isInLanelet(p.point.pose, lane) &&
           std::find(path_lane_ids.begin(), path_lane_ids.end(), lane.id()) == path_lane_ids.end()) {
           path_lane_ids.push_back(lane.id());
         }
       }
     }
   };
   populateLaneIds(path_turn_first);
   populateLaneIds(path_turn_second);
 
   // Set lane ids to each point in a given path.
   // It assigns the accumulated lane ids from path_lane_ids to each point's lane_ids member.
   const auto setLaneIdsToPath = [&](PathWithLaneId & path) {
     for (auto & p : path.points) {
       p.lane_ids = path_lane_ids;
     }
   };
   setLaneIdsToPath(path_turn_first);
   setLaneIdsToPath(path_turn_second);
 
+  if (lane_departure_checker) {
+    const auto lanelet_map_ptr = planner_data_->route_handler->getLaneletMapPtr();
+
+    const bool is_path_turn_first_outside_lanes =
+      lane_departure_checker->checkPathWillLeaveLane(lanelet_map_ptr, path_turn_first);
+
+    if (is_path_turn_first_outside_lanes) {
+      return std::vector<PathWithLaneId>{};
+    }
+
+    const bool is_path_turn_second_outside_lanes =
+      lane_departure_checker->checkPathWillLeaveLane(lanelet_map_ptr, path_turn_second);
+
+    if (is_path_turn_second_outside_lanes) {
+      return std::vector<PathWithLaneId>{};
+    }
+  }
+
   // generate arc path vector
   paths_.push_back(path_turn_first);
   paths_.push_back(path_turn_second);

planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/geometric_pull_out.hpp

@@ -19,20 +19,27 @@
 #include "behavior_path_start_planner_module/pull_out_path.hpp"
 #include "behavior_path_start_planner_module/pull_out_planner_base.hpp"
 
+#include <lane_departure_checker/lane_departure_checker.hpp>
+
 #include <autoware_auto_planning_msgs/msg/path_with_lane_id.hpp>
 
+#include <memory>
+
 namespace behavior_path_planner
 {
 class GeometricPullOut : public PullOutPlannerBase
 {
 public:
-  explicit GeometricPullOut(rclcpp::Node & node, const StartPlannerParameters & parameters);
+  explicit GeometricPullOut(
+    rclcpp::Node & node, const StartPlannerParameters & parameters,
+    const std::shared_ptr<lane_departure_checker::LaneDepartureChecker> lane_departure_checker);
 
   PlannerType getPlannerType() override { return PlannerType::GEOMETRIC; };
   std::optional<PullOutPath> plan(const Pose & start_pose, const Pose & goal_pose) override;
 
   GeometricParallelParking planner_;
   ParallelParkingParameters parallel_parking_parameters_;
+  std::shared_ptr<lane_departure_checker::LaneDepartureChecker> lane_departure_checker_;
 };
 }  // namespace behavior_path_planner
 

planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/shift_pull_out.hpp

@@ -50,11 +50,6 @@ class ShiftPullOut : public PullOutPlannerBase
     ShiftedPath & shifted_path, const Pose & start_pose, const Pose & end_pose,
     const double longitudinal_acc, const double lateral_acc);
 
-  void setDepartureCheckLanes(const lanelet::ConstLanelets & departure_check_lanes)
-  {
-    departure_check_lanes_ = departure_check_lanes;
-  }
-
   std::shared_ptr<LaneDepartureChecker> lane_departure_checker_;
 
 private:
@@ -63,8 +58,6 @@ class ShiftPullOut : public PullOutPlannerBase
   double calcPullOutLongitudinalDistance(
     const double lon_acc, const double shift_time, const double shift_length,
     const double max_curvature, const double min_distance) const;
-
-  lanelet::ConstLanelets departure_check_lanes_;
 };
 }  // namespace behavior_path_planner
 

planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/start_planner_module.hpp

@@ -275,7 +275,6 @@ class StartPlannerModule : public SceneModuleInterface
     const std::vector<PoseWithVelocityStamped> & ego_predicted_path) const;
   bool isSafePath() const;
   void setDrivableAreaInfo(BehaviorModuleOutput & output) const;
-  void updateDepartureCheckLanes();
   lanelet::ConstLanelets createDepartureCheckLanes() const;
 
   // check if the goal is located behind the ego in the same route segment.

planning/behavior_path_start_planner_module/src/geometric_pull_out.cpp

@@ -30,9 +30,12 @@
 {
 using start_planner_utils::getPullOutLanes;
 
-GeometricPullOut::GeometricPullOut(rclcpp::Node & node, const StartPlannerParameters & parameters)
+GeometricPullOut::GeometricPullOut(
+  rclcpp::Node & node, const StartPlannerParameters & parameters,
+  const std::shared_ptr<lane_departure_checker::LaneDepartureChecker> lane_departure_checker)
 : PullOutPlannerBase{node, parameters},
-  parallel_parking_parameters_{parameters.parallel_parking_parameters}
+  parallel_parking_parameters_{parameters.parallel_parking_parameters},
+  lane_departure_checker_(lane_departure_checker)
 {
   planner_.setParameters(parallel_parking_parameters_);
 }
@@ -55,8 +58,8 @@
   planner_.setTurningRadius(
     planner_data_->parameters, parallel_parking_parameters_.pull_out_max_steer_angle);
   planner_.setPlannerData(planner_data_);
-  const bool found_valid_path =
-    planner_.planPullOut(start_pose, goal_pose, road_lanes, pull_out_lanes, left_side_start);
+  const bool found_valid_path = planner_.planPullOut(
+    start_pose, goal_pose, road_lanes, pull_out_lanes, left_side_start, lane_departure_checker_);
   if (!found_valid_path) {
     return {};
   }