feat(goal_planner): introduce bezier based pullover for bus stop area (#10027)

Signed-off-by: Mamoru Sobue <mamoru.sobue@tier4.jp>

Author: soblin

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

@@ -25,8 +25,8 @@
         high_curvature_threshold: 0.1
         bus_stop_area:
           use_bus_stop_area: false
-          goal_search_interval: 0.5
-          lateral_offset_interval: 0.25
+          goal_search_interval: 0.75
+          lateral_offset_interval: 0.3
 
       # occupancy grid map
       occupancy_grid:
@@ -52,14 +52,15 @@
       # pull over
       pull_over:
         minimum_request_length: 0.0
+        pull_over_prepare_length: 100.0
         pull_over_velocity: 3.0
         pull_over_minimum_velocity: 1.38
         decide_path_distance: 10.0
         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)
-        lane_departure_check_expansion_margin: 0.0
+        lane_departure_check_expansion_margin: 0.2
 
         # shift parking
         shift_parking:
@@ -73,21 +74,21 @@
         # parallel parking path
         parallel_parking:
           path_interval: 1.0
-          max_steer_angle: 0.35  # 20deg
+          max_steer_angle: 0.4 #22.9deg
           forward:
             enable_arc_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.35  # 20deg
+            forward_parking_max_steer_angle: 0.4  # 22.9deg
           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.35  # 20deg
+            backward_parking_max_steer_angle: 0.4  # 22.9deg
 
         # freespace parking
         freespace_parking:
@@ -130,6 +131,10 @@
             neighbor_radius: 8.0
             margin: 1.0
 
+        bezier_parking:
+          pull_over_angle_threshold: 0.5
+          after_shift_straight_distance: 1.5
+
       stop_condition:
         maximum_deceleration_for_stop: 1.0
         maximum_jerk_for_stop: 1.0
@@ -139,13 +144,13 @@
           min_velocity: 1.0
           min_acceleration: 1.0
           max_velocity: 1.0
-          time_horizon_for_front_object: 10.0
-          time_horizon_for_rear_object: 10.0
+          time_horizon_for_front_object: 5.0
+          time_horizon_for_rear_object: 5.0
           time_resolution: 0.5
           delay_until_departure: 1.0
         # For target object filtering
         target_filtering:
-          safety_check_time_horizon: 10.0
+          safety_check_time_horizon: 5.0
           safety_check_time_resolution: 1.0
           # detection range
           object_check_forward_distance: 100.0
@@ -179,7 +184,7 @@
           # safety check configuration
           enable_safety_check: true
           method: "integral_predicted_polygon"
-          keep_unsafe_time: 3.0
+          keep_unsafe_time: 0.5
           # collision check parameters
           publish_debug_marker: false
           rss_params:
@@ -192,7 +197,7 @@
             forward_margin: 1.0
             backward_margin: 1.0
             lat_margin: 1.0
-            time_horizon: 10.0
+            time_horizon: 5.0
           # hysteresis factor to expand/shrink polygon with the value
           hysteresis_factor_expand_rate: 1.0
           collision_check_yaw_diff_threshold: 3.1416

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

@@ -18,6 +18,7 @@
 #include "autoware/behavior_path_goal_planner_module/decision_state.hpp"
 #include "autoware/behavior_path_goal_planner_module/fixed_goal_planner_base.hpp"
 #include "autoware/behavior_path_goal_planner_module/goal_planner_parameters.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/freespace_pull_over.hpp"
 #include "autoware/behavior_path_goal_planner_module/thread_data.hpp"
 #include "autoware/behavior_path_planner_common/utils/parking_departure/common_module_data.hpp"
@@ -86,8 +87,8 @@ struct PullOverContextData
   explicit PullOverContextData(
     const bool is_stable_safe_path, const PredictedObjects & static_objects,
     const PredictedObjects & dynamic_objects, const PathDecisionState & prev_state,
-    const bool is_stopped, const LaneParkingResponse & lane_parking_response,
-    const FreespaceParkingResponse & freespace_parking_response)
+    const bool is_stopped, LaneParkingResponse && lane_parking_response,
+    FreespaceParkingResponse && freespace_parking_response)
   : is_stable_safe_path(is_stable_safe_path),
     static_target_objects(static_objects),
     dynamic_target_objects(dynamic_objects),
@@ -112,8 +113,8 @@ struct PullOverContextData
   void update(
     const bool is_stable_safe_path_, const PredictedObjects static_target_objects_,
     const PredictedObjects dynamic_target_objects_, const PathDecisionState prev_state_for_debug_,
-    const bool is_stopped_, const LaneParkingResponse & lane_parking_response_,
-    const FreespaceParkingResponse & freespace_parking_response_)
+    const bool is_stopped_, LaneParkingResponse && lane_parking_response_,
+    FreespaceParkingResponse && freespace_parking_response_)
   {
     is_stable_safe_path = is_stable_safe_path_;
     static_target_objects = static_target_objects_;
@@ -196,6 +197,21 @@ class LaneParkingPlanner
     original_upstream_module_output_;  //<! upstream_module_output used for generating last
                                        // pull_over_path_candidates(only updated when new candidates
                                        // are generated)
+  std::shared_ptr<BezierPullOver> bezier_pull_over_planner_;
+  const double pull_over_angle_threshold;
+
+  bool switch_bezier_{false};
+  void normal_pullover_planning_helper(
+    const std::shared_ptr<PlannerData> planner_data, const GoalCandidates & goal_candidates,
+    const BehaviorModuleOutput & upstream_module_output,
+    const lanelet::ConstLanelets current_lanelets, std::optional<Pose> & closest_start_pose,
+    std::vector<PullOverPath> & path_candidates);
+  void bezier_planning_helper(
+    const std::shared_ptr<PlannerData> planner_data, const GoalCandidates & goal_candidates,
+    const BehaviorModuleOutput & upstream_module_output,
+    const lanelet::ConstLanelets current_lanelets, std::optional<Pose> & closest_start_pose,
+    std::vector<PullOverPath> & path_candidates,
+    std::optional<std::vector<size_t>> & sorted_indices) const;
 };
 
 class FreespaceParkingPlanner
@@ -302,6 +318,7 @@ class GoalPlannerModule : public SceneModuleInterface
 private:
   std::pair<LaneParkingResponse, FreespaceParkingResponse> syncWithThreads();
 
+  bool trigger_thread_on_approach_{false};
   // NOTE: never access to following variables except in updateData()!!!
   std::mutex lane_parking_mutex_;
   std::optional<LaneParkingRequest> lane_parking_request_;
@@ -433,7 +450,8 @@ class GoalPlannerModule : public SceneModuleInterface
   std::optional<PullOverPath> selectPullOverPath(
     const PullOverContextData & context_data,
     const std::vector<PullOverPath> & pull_over_path_candidates,
-    const GoalCandidates & goal_candidates) const;
+    const GoalCandidates & goal_candidates,
+    const std::optional<std::vector<size_t>> sorted_bezier_indices_opt) const;
 
   // lanes and drivable area
   std::vector<DrivableLanes> generateDrivableLanes() const;

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

@@ -88,6 +88,7 @@ struct GoalPlannerParameters
 
   // pull over general params
   double pull_over_minimum_request_length{0.0};
+  double pull_over_prepare_length{0.0};
   double pull_over_velocity{0.0};
   double pull_over_minimum_velocity{0.0};
   double decide_path_distance{0.0};
@@ -119,6 +120,12 @@ struct GoalPlannerParameters
   AstarParam astar_parameters{};
   RRTStarParam rrt_star_parameters{};
 
+  struct BezierParking
+  {
+    double pull_over_angle_threshold;
+    double after_shift_straight_distance;
+  } bezier_parking;
+
   // stop condition
   double maximum_deceleration_for_stop{0.0};
   double maximum_jerk_for_stop{0.0};

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

@@ -68,6 +68,10 @@ struct PullOverPath
   const std::vector<double> & parking_path_curvatures() const { return parking_path_curvatures_; }
   double full_path_max_curvature() const { return full_path_max_curvature_; }
   double parking_path_max_curvature() const { return parking_path_max_curvature_; }
+  double parking_path_curvature_total_derivative() const
+  {
+    return parking_path_curvature_total_derivative_;
+  }
   size_t path_idx() const { return path_idx_; }
 
   bool incrementPathIndex();
@@ -94,7 +98,7 @@ struct PullOverPath
     const PathWithLaneId & full_path, const PathWithLaneId & parking_path,
     const std::vector<double> & full_path_curvatures,
     const std::vector<double> & parking_path_curvatures, const double full_path_max_curvature,
-    const double parking_path_max_curvature,
+    const double parking_path_max_curvature, const double parking_path_curvature_total_derivative,
     const std::vector<std::pair<double, double>> & pairs_terminal_velocity_and_accel);
 
   PullOverPlannerType type_;
@@ -109,6 +113,7 @@ struct PullOverPath
   std::vector<double> parking_path_curvatures_;
   double full_path_max_curvature_;
   double parking_path_max_curvature_;
+  double parking_path_curvature_total_derivative_;
 
   // accelerate with constant acceleration to the target velocity
   size_t path_idx_;

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

@@ -43,7 +43,8 @@ class LaneParkingRequest
   void update(
     const PlannerData & planner_data, const ModuleStatus & current_status,
     const BehaviorModuleOutput & upstream_module_output,
-    const std::optional<PullOverPath> & pull_over_path, const PathDecisionState & prev_data);
+    const std::optional<PullOverPath> & pull_over_path, const PathDecisionState & prev_data,
+    const bool trigger_thread_on_approach);
 
   const autoware::universe_utils::LinearRing2d vehicle_footprint_;
   const GoalCandidates goal_candidates_;
@@ -56,19 +57,22 @@ class LaneParkingRequest
   }
   const std::optional<PullOverPath> & get_pull_over_path() const { return pull_over_path_; }
   const PathDecisionState & get_prev_data() const { return prev_data_; }
+  bool trigger_thread_on_approach() const { return trigger_thread_on_approach_; }
 
 private:
   std::shared_ptr<PlannerData> planner_data_;
   ModuleStatus current_status_;
   BehaviorModuleOutput upstream_module_output_;
   std::optional<PullOverPath> pull_over_path_;  //<! pull over path selected by main thread
   PathDecisionState prev_data_;
+  bool trigger_thread_on_approach_{false};
 };
 
 struct LaneParkingResponse
 {
   std::vector<PullOverPath> pull_over_path_candidates;
   std::optional<Pose> closest_start_pose;
+  std::optional<std::vector<size_t>> sorted_bezier_indices_opt;
 };
 
 class FreespaceParkingRequest