add lane departure check to geometric pullout

Signed-off-by: Daniel Sanchez <danielsanchezaran@gmail.com>

Author: danielsanchezaran

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

@@ -116,7 +116,7 @@ std::vector<PathWithLaneId> GeometricParallelParking::generatePullOverPaths(
                                               : 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,7 +222,8 @@ bool GeometricParallelParking::planPullOver(
 
 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
@@ -242,7 +243,7 @@ bool GeometricParallelParking::planPullOut(
     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,7 +363,8 @@ std::vector<PathWithLaneId> GeometricParallelParking::planOneTrial(
   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();
 
@@ -496,6 +498,24 @@ std::vector<PathWithLaneId> GeometricParallelParking::planOneTrial(
   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/src/geometric_pull_out.cpp

@@ -30,9 +30,12 @@ namespace behavior_path_planner
 {
 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 @@ std::optional<PullOutPath> GeometricPullOut::plan(const Pose & start_pose, const
   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 {};
   }

planning/behavior_path_start_planner_module/src/start_planner_module.cpp

@@ -69,7 +69,8 @@ StartPlannerModule::StartPlannerModule(
       std::make_shared<ShiftPullOut>(node, *parameters, lane_departure_checker_));
   }
   if (parameters_->enable_geometric_pull_out) {
-    start_planners_.push_back(std::make_shared<GeometricPullOut>(node, *parameters));
+    start_planners_.push_back(
+      std::make_shared<GeometricPullOut>(node, *parameters, lane_departure_checker_));
   }
   if (start_planners_.empty()) {
     RCLCPP_ERROR(getLogger(), "Not found enabled planner");