clothoid geometric parking

Signed-off-by: Takumi Ito <takumi.ito@tier4.jp>

planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/config/goal_planner.param.yaml

@@ -81,14 +81,18 @@
             forward_parking_velocity: 1.38
             forward_parking_lane_departure_margin: 0.0
             forward_parking_path_interval: 1.0
-            forward_parking_max_steer_angle: 0.4  # 22.9deg
+            forward_parking_max_steer_angle: 0.35  # 20deg
+            forward_parking_steer_rate_lim: 0.35
+            forward_parking_use_clothoid: true
           backward:
             enable_arc_backward_parking: true
             after_backward_parking_straight_distance: 2.0
             backward_parking_velocity: -1.38
             backward_parking_lane_departure_margin: 0.0
             backward_parking_path_interval: 1.0
-            backward_parking_max_steer_angle: 0.4  # 22.9deg
+            backward_parking_max_steer_angle: 0.35  # 20deg
+            backward_parking_steer_rate_lim: 0.35
+            backward_parking_use_clothoid: true
 
         # freespace parking
         freespace_parking:

planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/examples/plot_map_case2.cpp

@@ -19,6 +19,7 @@
 #include <autoware/behavior_path_goal_planner_module/manager.hpp>
 #include <autoware/behavior_path_goal_planner_module/pull_over_planner/bezier_pull_over.hpp>
 #include <autoware/behavior_path_goal_planner_module/pull_over_planner/shift_pull_over.hpp>
+#include <autoware/behavior_path_goal_planner_module/pull_over_planner/geometric_pull_over.hpp>
 #include <autoware/behavior_path_planner/behavior_path_planner_node.hpp>
 #include <autoware/behavior_path_planner_common/data_manager.hpp>
 #include <autoware/behavior_path_planner_common/utils/parking_departure/utils.hpp>

planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/manager.cpp

@@ -185,6 +185,10 @@ GoalPlannerParameters GoalPlannerModuleManager::initGoalPlannerParameters(
       node->declare_parameter<double>(ns + "forward_parking_path_interval");
     p.parallel_parking_parameters.forward_parking_max_steer_angle =
       node->declare_parameter<double>(ns + "forward_parking_max_steer_angle");  // 20deg
+    p.parallel_parking_parameters.forward_parking_steer_rate_lim =
+      node->declare_parameter<double>(ns + "forward_parking_steer_rate_lim");  // 20deg
+    p.parallel_parking_parameters.forward_parking_use_clothoid =
+      node->declare_parameter<bool>(ns + "forward_parking_use_clothoid");
   }
 
   // forward parallel parking backward
@@ -202,6 +206,10 @@ GoalPlannerParameters GoalPlannerModuleManager::initGoalPlannerParameters(
       node->declare_parameter<double>(ns + "backward_parking_path_interval");
     p.parallel_parking_parameters.backward_parking_max_steer_angle =
       node->declare_parameter<double>(ns + "backward_parking_max_steer_angle");  // 20deg
+    p.parallel_parking_parameters.backward_parking_steer_rate_lim =
+      node->declare_parameter<double>(ns + "backward_parking_steer_rate_lim");  // 20deg
+    p.parallel_parking_parameters.backward_parking_use_clothoid =
+      node->declare_parameter<bool>(ns + "backward_parking_use_clothoid");
   }
 
   // freespace parking general params
@@ -578,6 +586,12 @@ void GoalPlannerModuleManager::updateModuleParams(
     updateParam<double>(
       parameters, ns + "forward_parking_max_steer_angle",
       p->parallel_parking_parameters.forward_parking_max_steer_angle);
+    updateParam<double>(
+      parameters, ns + "forward_parking_steer_rate_lim",
+      p->parallel_parking_parameters.forward_parking_steer_rate_lim);
+    updateParam<bool>(
+      parameters, ns + "forward_parking_use_clothoid",
+      p->parallel_parking_parameters.forward_parking_use_clothoid);
   }
 
   // forward parallel parking backward
@@ -600,6 +614,12 @@ void GoalPlannerModuleManager::updateModuleParams(
     updateParam<double>(
       parameters, ns + "backward_parking_max_steer_angle",
       p->parallel_parking_parameters.backward_parking_max_steer_angle);
+    updateParam<double>(
+      parameters, ns + "backward_parking_steer_rate_lim",
+      p->parallel_parking_parameters.backward_parking_steer_rate_lim);
+    updateParam<bool>(
+      parameters, ns + "backward_parking_use_clothoid",
+      p->parallel_parking_parameters.backward_parking_use_clothoid);
   }
 
   // freespace parking general params

planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/pull_over_planner/geometric_pull_over.cpp

@@ -82,6 +82,9 @@ std::optional<PullOverPath> GeometricPullOver::plan(
   auto pull_over_path_opt = PullOverPath::create(
     getPlannerType(), id, planner_.getPaths(), planner_.getStartPose(), modified_goal_pose,
     planner_.getPairsTerminalVelocityAndAccel());
-  return pull_over_path_opt;
+  if (!pull_over_path_opt) {
+    return {};
+  }
+  return pull_over_path_opt.value();
 }
 }  // namespace autoware::behavior_path_planner

planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/parking_departure/geometric_parallel_parking.hpp

@@ -49,19 +49,25 @@ struct ParallelParkingParameters
   double forward_parking_lane_departure_margin{0.0};
   double forward_parking_path_interval{0.0};
   double forward_parking_max_steer_angle{0.0};
+  double forward_parking_steer_rate_lim{0.0};
+  bool forward_parking_use_clothoid{false};
 
   // backward parking
   double after_backward_parking_straight_distance{0.0};
   double backward_parking_velocity{0.0};
   double backward_parking_lane_departure_margin{0.0};
   double backward_parking_path_interval{0.0};
   double backward_parking_max_steer_angle{0.0};
+  double backward_parking_steer_rate_lim{0.0};
+  bool backward_parking_use_clothoid{false};
 
   // pull_out
   double pull_out_velocity{0.0};
   double pull_out_lane_departure_margin{0.0};
   double pull_out_arc_path_interval{0.0};
   double pull_out_max_steer_angle{0.0};
+  double pull_out_steer_rate_lim{0.0};
+  bool pull_out_use_clothoid{false};
 };
 
 class GeometricParallelParking
@@ -102,6 +108,14 @@ class GeometricParallelParking
   Pose getStartPose() const { return start_pose_; }
   Pose getArcEndPose() const { return arc_end_pose_; }
 
+  std::vector<PathWithLaneId> planOneTrialClothoid(
+    const Pose & start_pose, const Pose & goal_pose, const double R_E_far, const double L_min,
+    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 std::shared_ptr<autoware::lane_departure_checker::LaneDepartureChecker>
+      lane_departure_checker);
+
 private:
   std::shared_ptr<const PlannerData> planner_data_{nullptr};
   ParallelParkingParameters parameters_{};
@@ -138,10 +152,23 @@ class GeometricParallelParking
     const bool is_forward, const bool left_side_parking, const double end_pose_offset,
     const double velocity);
   PathWithLaneId generateStraightPath(
-    const Pose & start_pose, const lanelet::ConstLanelets & road_lanes);
+    const Pose & start_pose, const lanelet::ConstLanelets & road_lanes, const bool set_stop_end);
   void setVelocityToArcPaths(
     std::vector<PathWithLaneId> & arc_paths, const double velocity, const bool set_stop_end);
 
+  std::vector<PathWithLaneId> generateClothoidalSequence(
+    const double A, const double L, const double theta, const Pose & start_pose,
+    const Pose & end_pose, const double arc_path_interval, const bool is_left_steering,
+    const bool is_forward);
+  PathWithLaneId generateArcPathFromTwoPoses(
+    const Pose & start_pose, const Pose & goal_pose, const double arc_path_interval,
+    const bool is_left_turn, const bool is_forward);
+  PathWithLaneId generateClothoidPath(
+    const double A, const double L, const Pose & start_pose, const double arc_path_interval,
+    const bool is_left_steering, const bool is_forward);
+
+  const double clothoid_integral_interval_{0.001};
+
   // debug
   Pose Cr_{};
   Pose Cl_{};