feat(autoware_behavior_path_planner_common): add continuous curvature pullover path generation

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

@@ -59,7 +59,7 @@
         maximum_deceleration: 1.0
         maximum_jerk: 1.0
         path_priority: "efficient_path" # "efficient_path" or "close_goal"
-        efficient_path_order: ["SHIFT", "ARC_FORWARD", "ARC_BACKWARD"] # only lane based pull over(exclude freespace parking)
+        efficient_path_order: ["SHIFT", "ARC_FORWARD", "CLOTHOID_FORWARD", "ARC_BACKWARD", "CLOTHOID_BACKWARD"] # only lane based pull over(exclude freespace parking)
         lane_departure_check_expansion_margin: 0.2
 
         # shift parking
@@ -77,18 +77,22 @@
           max_steer_angle: 0.4 #22.9deg
           forward:
             enable_arc_forward_parking: true
+            enable_clothoid_forward_parking: true
             after_forward_parking_straight_distance: 2.0
             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_steer_rate_lim: 0.35
           backward:
             enable_arc_backward_parking: true
+            enable_clothoid_backward_parking: false # Not supported yet
             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_steer_rate_lim: 0.35
 
         # freespace parking
         freespace_parking:

planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/goal_planner_parameters.hpp

@@ -111,6 +111,10 @@ struct GoalPlannerParameters
   bool enable_arc_backward_parking{false};
   ParallelParkingParameters parallel_parking_parameters;
 
+  // clothoid parking
+  bool enable_clothoid_forward_parking{false};
+  bool enable_clothoid_backward_parking{false};
+
   // freespace parking
   bool enable_freespace_parking{false};
   std::string freespace_parking_algorithm;

planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/pull_over_planner/geometric_pull_over.hpp

@@ -32,7 +32,8 @@ class GeometricPullOver : public PullOverPlannerBase
 {
 public:
   GeometricPullOver(
-    rclcpp::Node & node, const GoalPlannerParameters & parameters, const bool is_forward);
+    rclcpp::Node & node, const GoalPlannerParameters & parameters, const bool is_forward,
+    const bool use_clothoid);
 
   PullOverPlannerType getPlannerType() const override
   {
@@ -61,6 +62,7 @@ class GeometricPullOver : public PullOverPlannerBase
   const ParallelParkingParameters parallel_parking_parameters_;
   const LaneDepartureChecker lane_departure_checker_;
   const bool is_forward_;
+  const bool use_clothoid_;
   const bool left_side_parking_;
 
   GeometricParallelParking planner_;

planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_planner_module.cpp

@@ -1,4 +1,4 @@
 // Copyright 2021 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.
@@ -274,11 +274,17 @@
     if (planner_type == "SHIFT" && parameters.enable_shift_parking) {
       pull_over_planners_.push_back(std::make_shared<ShiftPullOver>(node, parameters));
     } else if (planner_type == "ARC_FORWARD" && parameters.enable_arc_forward_parking) {
-      pull_over_planners_.push_back(
-        std::make_shared<GeometricPullOver>(node, parameters, /*is_forward*/ true));
+      pull_over_planners_.push_back(std::make_shared<GeometricPullOver>(
+        node, parameters, /*is_forward*/ true, /*use_clothoid*/ false));
+    } else if (planner_type == "CLOTHOID_FORWARD" && parameters.enable_clothoid_forward_parking) {
+      pull_over_planners_.push_back(std::make_shared<GeometricPullOver>(
+        node, parameters, /*is_forward*/ true, /*use_clothoid*/ true));
     } else if (planner_type == "ARC_BACKWARD" && parameters.enable_arc_backward_parking) {
-      pull_over_planners_.push_back(
-        std::make_shared<GeometricPullOver>(node, parameters, /*is_forward*/ false));
+      pull_over_planners_.push_back(std::make_shared<GeometricPullOver>(
+        node, parameters, /*is_forward*/ false, /*use_clothoid*/ false));
+    } else if (planner_type == "CLOTHOID_BACKWARD" && parameters.enable_clothoid_backward_parking) {
+      pull_over_planners_.push_back(std::make_shared<GeometricPullOver>(
+        node, parameters, /*is_forward*/ false, /*use_clothoid*/ true));
     }
   }
 

planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/manager.cpp

@@ -175,33 +175,41 @@
   {
     const std::string ns = base_ns + "pull_over.parallel_parking.forward.";
     p.enable_arc_forward_parking = node->declare_parameter<bool>(ns + "enable_arc_forward_parking");
+    p.enable_clothoid_forward_parking =
+      node->declare_parameter<bool>(ns + "enable_clothoid_forward_parking");
     p.parallel_parking_parameters.after_forward_parking_straight_distance =
       node->declare_parameter<double>(ns + "after_forward_parking_straight_distance");
     p.parallel_parking_parameters.forward_parking_velocity =
       node->declare_parameter<double>(ns + "forward_parking_velocity");
     p.parallel_parking_parameters.forward_parking_lane_departure_margin =
       node->declare_parameter<double>(ns + "forward_parking_lane_departure_margin");
     p.parallel_parking_parameters.forward_parking_path_interval =
       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");
   }
 
   // forward parallel parking backward
   {
     const std::string ns = base_ns + "pull_over.parallel_parking.backward.";
     p.enable_arc_backward_parking =
       node->declare_parameter<bool>(ns + "enable_arc_backward_parking");
+    p.enable_clothoid_backward_parking =
+      node->declare_parameter<bool>(ns + "enable_clothoid_backward_parking");
     p.parallel_parking_parameters.after_backward_parking_straight_distance =
       node->declare_parameter<double>(ns + "after_backward_parking_straight_distance");
     p.parallel_parking_parameters.backward_parking_velocity =
       node->declare_parameter<double>(ns + "backward_parking_velocity");
     p.parallel_parking_parameters.backward_parking_lane_departure_margin =
       node->declare_parameter<double>(ns + "backward_parking_lane_departure_margin");
     p.parallel_parking_parameters.backward_parking_path_interval =
       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");
   }
 
   // freespace parking general params
@@ -563,43 +571,53 @@
   {
     const std::string ns = base_ns + "pull_over.parallel_parking.forward.";
     updateParam<bool>(parameters, ns + "enable_arc_forward_parking", p->enable_arc_forward_parking);
+    updateParam<bool>(
+      parameters, ns + "enable_clothoid_forward_parking", p->enable_clothoid_forward_parking);
     updateParam<double>(
       parameters, ns + "after_forward_parking_straight_distance",
       p->parallel_parking_parameters.after_forward_parking_straight_distance);
     updateParam<double>(
       parameters, ns + "forward_parking_velocity",
       p->parallel_parking_parameters.forward_parking_velocity);
     updateParam<double>(
       parameters, ns + "forward_parking_lane_departure_margin",
       p->parallel_parking_parameters.forward_parking_lane_departure_margin);
     updateParam<double>(
       parameters, ns + "forward_parking_path_interval",
       p->parallel_parking_parameters.forward_parking_path_interval);
     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);
   }
 
   // forward parallel parking backward
   {
     const std::string ns = base_ns + "pull_over.parallel_parking.backward.";
     updateParam<bool>(
       parameters, ns + "enable_arc_backward_parking", p->enable_arc_backward_parking);
+    updateParam<bool>(
+      parameters, ns + "enable_clothoid_backward_parking", p->enable_clothoid_backward_parking);
     updateParam<double>(
       parameters, ns + "after_backward_parking_straight_distance",
       p->parallel_parking_parameters.after_backward_parking_straight_distance);
     updateParam<double>(
       parameters, ns + "backward_parking_velocity",
       p->parallel_parking_parameters.backward_parking_velocity);
     updateParam<double>(
       parameters, ns + "backward_parking_lane_departure_margin",
       p->parallel_parking_parameters.backward_parking_lane_departure_margin);
     updateParam<double>(
       parameters, ns + "backward_parking_path_interval",
       p->parallel_parking_parameters.backward_parking_path_interval);
     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);
   }
 
   // freespace parking general params

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

@@ -26,7 +26,8 @@
 namespace autoware::behavior_path_planner
 {
 GeometricPullOver::GeometricPullOver(
-  rclcpp::Node & node, const GoalPlannerParameters & parameters, const bool is_forward)
+  rclcpp::Node & node, const GoalPlannerParameters & parameters, const bool is_forward,
+  const bool use_clothoid)
 : PullOverPlannerBase{node, parameters},
   parallel_parking_parameters_{parameters.parallel_parking_parameters},
   lane_departure_checker_{[&]() {
@@ -36,6 +37,7 @@ GeometricPullOver::GeometricPullOver(
     return LaneDepartureChecker{lane_departure_checker_params, vehicle_info_};
   }()},
   is_forward_{is_forward},
+  use_clothoid_{use_clothoid},
   left_side_parking_{parameters.parking_policy == ParkingPolicy::LEFT_SIDE}
 {
   planner_.setParameters(parallel_parking_parameters_);
@@ -66,8 +68,8 @@ std::optional<PullOverPath> GeometricPullOver::plan(
   planner_.setTurningRadius(planner_data->parameters, max_steer_angle);
   planner_.setPlannerData(planner_data);
 
-  const bool found_valid_path =
-    planner_.planPullOver(goal_pose, road_lanes, pull_over_lanes, is_forward_, left_side_parking_);
+  const bool found_valid_path = planner_.planPullOver(
+    goal_pose, road_lanes, pull_over_lanes, is_forward_, left_side_parking_, use_clothoid_);
   if (!found_valid_path) {
     return {};
   }

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,22 @@ 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};
 
   // 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};
 
   // 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};
 };
 
 class GeometricParallelParking
@@ -71,10 +74,11 @@ class GeometricParallelParking
   bool planPullOver(
     const Pose & goal_pose, const lanelet::ConstLanelets & road_lanes,
     const lanelet::ConstLanelets & pull_over_lanes, const bool is_forward,
-    const bool left_side_parking);
+    const bool left_side_parking, const bool use_clothoid);
   bool planPullOut(
     const Pose & start_pose, const Pose & goal_pose, const lanelet::ConstLanelets & road_lanes,
     const lanelet::ConstLanelets & pull_over_lanes, const bool left_side_start,
+    const bool use_clothoid,
     const std::shared_ptr<autoware::lane_departure_checker::LaneDepartureChecker>
       autoware_lane_departure_checker);
   void setParameters(const ParallelParkingParameters & parameters) { parameters_ = parameters; }
@@ -102,6 +106,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_{};
@@ -135,13 +147,26 @@ class GeometricParallelParking
   std::vector<PathWithLaneId> generatePullOverPaths(
     const Pose & start_pose, const Pose & goal_pose, const double R_E_far,
     const lanelet::ConstLanelets & road_lanes, const lanelet::ConstLanelets & pull_over_lanes,
-    const bool is_forward, const bool left_side_parking, const double end_pose_offset,
-    const double velocity);
+    const bool is_forward, const bool left_side_parking, const bool use_clothoid,
+    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_{};