append enable_clothoid_* param instead of *_use_clothoid

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

Author: Takumi Ito

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,22 +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
-            forward_parking_use_clothoid: true
           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
-            backward_parking_use_clothoid: false # Not supported yet
 
         # 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,7 @@ 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 +61,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

@@ -275,10 +275,16 @@ LaneParkingPlanner::LaneParkingPlanner(
       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));
+        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));
+        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

@@ -174,7 +174,10 @@ GoalPlannerParameters GoalPlannerModuleManager::initGoalPlannerParameters(
   // forward parallel parking forward
   {
     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_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 =
@@ -187,15 +190,15 @@ GoalPlannerParameters GoalPlannerModuleManager::initGoalPlannerParameters(
       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");
-    p.parallel_parking_parameters.forward_parking_use_clothoid =
-      node->declare_parameter<bool>(ns + "forward_parking_use_clothoid");
   }
 
   // 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 =
@@ -208,8 +211,6 @@ GoalPlannerParameters GoalPlannerModuleManager::initGoalPlannerParameters(
       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");
-    p.parallel_parking_parameters.backward_parking_use_clothoid =
-      node->declare_parameter<bool>(ns + "backward_parking_use_clothoid");
   }
 
   // freespace parking general params
@@ -571,6 +572,7 @@ void GoalPlannerModuleManager::updateModuleParams(
   {
     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);
@@ -589,16 +591,15 @@ void GoalPlannerModuleManager::updateModuleParams(
     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
   {
     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);
@@ -617,9 +618,6 @@ void GoalPlannerModuleManager::updateModuleParams(
     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

@@ -26,7 +26,7 @@
 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 +36,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_);
@@ -67,7 +68,7 @@ std::optional<PullOverPath> GeometricPullOver::plan(
   planner_.setPlannerData(planner_data);
 
   const bool found_valid_path =
-    planner_.planPullOver(goal_pose, road_lanes, pull_over_lanes, is_forward_, left_side_parking_);
+    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

@@ -50,7 +50,6 @@ struct ParallelParkingParameters
   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};
@@ -59,15 +58,13 @@ struct ParallelParkingParameters
   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
@@ -77,10 +74,10 @@ 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 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; }
@@ -149,7 +146,7 @@ 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 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 bool set_stop_end);

planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/parking_departure/geometric_parallel_parking.cpp

@@ -110,7 +110,7 @@ void GeometricParallelParking::setVelocityToArcPaths(
 std::vector<PathWithLaneId> GeometricParallelParking::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 bool is_forward, const bool left_side_parking, const bool use_clothoid, const double end_pose_offset,
   const double velocity)
 {
   const double lane_departure_margin = is_forward
@@ -119,7 +119,7 @@ std::vector<PathWithLaneId> GeometricParallelParking::generatePullOverPaths(
   const double arc_path_interval = is_forward ? parameters_.forward_parking_path_interval
                                               : parameters_.backward_parking_path_interval;
   std::vector<PathWithLaneId> arc_paths;
-  if (is_forward && parameters_.forward_parking_use_clothoid) {  // clothoid parking only supports
+  if (is_forward && use_clothoid) {  // clothoid parking only supports
                                                                  // forward for now
     const double L_min =
       is_forward
@@ -148,7 +148,7 @@ std::vector<PathWithLaneId> GeometricParallelParking::generatePullOverPaths(
   setVelocityToArcPaths(arc_paths, velocity, set_stop_end);
 
   // straight path from current to parking start
-  const bool set_stop_straight_end = !(is_forward && parameters_.forward_parking_use_clothoid);
+  const bool set_stop_straight_end = !(is_forward && use_clothoid);
   const auto straight_path = generateStraightPath(start_pose, road_lanes, set_stop_straight_end);
 
   // check the continuity of straight path and arc path
@@ -163,7 +163,7 @@ std::vector<PathWithLaneId> GeometricParallelParking::generatePullOverPaths(
 
   // combine straight_path -> arc_path*2
   std::vector<PathWithLaneId> paths;
-  if (is_forward && parameters_.forward_parking_use_clothoid) {
+  if (is_forward && use_clothoid) {
     paths.push_back(straight_path);
     for (const auto & path : arc_paths) {
       for (const auto & path_point : path.points) {
@@ -190,7 +190,7 @@ void GeometricParallelParking::clearPaths()
 bool GeometricParallelParking::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)
 {
   const auto & common_params = planner_data_->parameters;
   const double end_pose_offset = is_forward ? -parameters_.after_forward_parking_straight_distance
@@ -219,7 +219,7 @@ bool GeometricParallelParking::planPullOver(
       }
 
       const auto paths = generatePullOverPaths(
-        *start_pose, goal_pose, R_E_far, road_lanes, pull_over_lanes, is_forward, left_side_parking,
+        *start_pose, goal_pose, R_E_far, road_lanes, pull_over_lanes, is_forward, left_side_parking, use_clothoid,
         end_pose_offset, parameters_.forward_parking_velocity);
       if (!paths.empty()) {
         paths_ = paths;
@@ -242,7 +242,7 @@ bool GeometricParallelParking::planPullOver(
 
       const auto paths = generatePullOverPaths(
         *start_pose, goal_pose, R_E_min_, road_lanes, pull_over_lanes, is_forward,
-        left_side_parking, end_pose_offset, parameters_.backward_parking_velocity);
+        left_side_parking, use_clothoid, end_pose_offset, parameters_.backward_parking_velocity);
       if (!paths.empty()) {
         paths_ = paths;
         return true;
@@ -255,7 +255,7 @@ bool GeometricParallelParking::planPullOver(
 
 bool GeometricParallelParking::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 lanelet::ConstLanelets & pull_over_lanes, const bool left_side_start, const bool use_clothoid,
   const std::shared_ptr<autoware::lane_departure_checker::LaneDepartureChecker>
     lane_departure_checker)
 {
@@ -275,7 +275,7 @@ bool GeometricParallelParking::planPullOut(
 
     // plan reverse path of parking. end_pose <-> start_pose
     std::vector<PathWithLaneId> arc_paths;
-    if (parameters_.pull_out_use_clothoid) {
+    if (use_clothoid) {
       const double L_min = std::abs(
         parameters_.pull_out_velocity *
         (parameters_.pull_out_max_steer_angle / parameters_.pull_out_steer_rate_lim));
@@ -982,7 +982,7 @@ std::vector<PathWithLaneId> GeometricParallelParking::generateClothoidalSequence
     std::reverse(p.lane_ids.begin(), p.lane_ids.end());
   }
 
-  if (theta != 0) {
+  if (abs(theta) > 1e-6) {
     // generate arc path which connect two clothoid paths
     const auto end_of_prev_path = clothoid_path_first.points.back().point.pose;
     const auto start_of_next_path = clothoid_path_second.points.front().point.pose;

planning/behavior_path_planner/autoware_behavior_path_start_planner_module/config/start_planner.param.yaml

@@ -36,6 +36,7 @@
       end_pose_curvature_threshold: 0.1
       # geometric pull out
       enable_geometric_pull_out: true
+      enable_clothoid_pull_out: true
       geometric_collision_check_distance_from_end: 0.0
       arc_path_interval: 1.0
       divide_pull_out_path: true
@@ -45,7 +46,6 @@
       backward_velocity: -1.0
       pull_out_max_steer_angle: 0.26  # 15deg
       pull_out_steer_rate_lim: 0.35
-      pull_out_use_clothoid: true
       # search start pose backward
       enable_back: true
       search_priority: "efficient_path"  # "efficient_path" or "short_back_distance"

planning/behavior_path_planner/autoware_behavior_path_start_planner_module/include/autoware/behavior_path_start_planner_module/data_structs.hpp

@@ -132,6 +132,7 @@ struct StartPlannerParameters
   double maximum_longitudinal_deviation{0.0};
   // geometric pull out
   bool enable_geometric_pull_out{false};
+  bool enable_clothoid_pull_out{false};
   double geometric_collision_check_distance_from_end{0.0};
   bool divide_pull_out_path{false};
   ParallelParkingParameters parallel_parking_parameters{};

planning/behavior_path_planner/autoware_behavior_path_start_planner_module/include/autoware/behavior_path_start_planner_module/geometric_pull_out.hpp

@@ -32,7 +32,7 @@ class GeometricPullOut : public PullOutPlannerBase
 {
 public:
   explicit GeometricPullOut(
-    rclcpp::Node & node, const StartPlannerParameters & parameters,
+    rclcpp::Node & node, const StartPlannerParameters & parameters, const bool use_clothoid,
     std::shared_ptr<universe_utils::TimeKeeper> time_keeper =
       std::make_shared<universe_utils::TimeKeeper>());
 
@@ -45,6 +45,7 @@ class GeometricPullOut : public PullOutPlannerBase
   GeometricParallelParking planner_;
   ParallelParkingParameters parallel_parking_parameters_;
   std::shared_ptr<autoware::lane_departure_checker::LaneDepartureChecker> lane_departure_checker_;
+  const bool use_clothoid_;
 
   friend class TestGeometricPullOut;
 };

planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/data_structs.cpp

@@ -72,6 +72,8 @@ StartPlannerParameters StartPlannerParameters::init(rclcpp::Node & node)
     // geometric pull out
     p.enable_geometric_pull_out =
       getOrDeclareParameter<bool>(node, ns + "enable_geometric_pull_out");
+    p.enable_clothoid_pull_out =
+      getOrDeclareParameter<bool>(node, ns + "enable_clothoid_pull_out");
     p.geometric_collision_check_distance_from_end =
       getOrDeclareParameter<double>(node, ns + "geometric_collision_check_distance_from_end");
     p.divide_pull_out_path = getOrDeclareParameter<bool>(node, ns + "divide_pull_out_path");

planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/geometric_pull_out.cpp

@@ -35,10 +35,11 @@ namespace autoware::behavior_path_planner
 using start_planner_utils::getPullOutLanes;
 
 GeometricPullOut::GeometricPullOut(
-  rclcpp::Node & node, const StartPlannerParameters & parameters,
+  rclcpp::Node & node, const StartPlannerParameters & parameters, const bool use_clothoid,
   std::shared_ptr<universe_utils::TimeKeeper> time_keeper)
 : PullOutPlannerBase{node, parameters, time_keeper},
-  parallel_parking_parameters_{parameters.parallel_parking_parameters}
+  parallel_parking_parameters_{parameters.parallel_parking_parameters},
+  use_clothoid_{use_clothoid}
 {
   auto lane_departure_checker_params = autoware::lane_departure_checker::Param{};
   lane_departure_checker_params.footprint_extra_margin =
@@ -70,7 +71,7 @@ std::optional<PullOutPath> GeometricPullOut::plan(
     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, lane_departure_checker_);
+    start_pose, goal_pose, road_lanes, pull_out_lanes, left_side_start, use_clothoid_, lane_departure_checker_);
   if (!found_valid_path) {
     planner_debug_data.conditions_evaluation.emplace_back("no path found");
     return {};