refactor(behavior_path_planner): refactoring goal_planner and pull_ou…

…t params Signed-off-by: kosuke55 <kosuke.tnp@gmail.com>
autowarefoundation · May 3, 2023 · 3fd346e · 3fd346e
1 parent ff7c808
commit 3fd346e
Show file tree

Hide file tree

Showing 17 changed files with 361 additions and 336 deletions.
diff --git a/planning/behavior_path_planner/config/goal_planner/goal_planner.param.yaml b/planning/behavior_path_planner/config/goal_planner/goal_planner.param.yaml
@@ -1,96 +1,108 @@
 /**:
   ros__parameters:
     goal_planner:
+      # general params
       minimum_request_length: 100.0
       th_arrived_distance: 1.0
       th_stopped_velocity: 0.01
       th_stopped_time: 2.0  # It must be greater than the state_machine's.
-      pull_over_velocity: 3.0
-      pull_over_minimum_velocity: 1.38
-      margin_from_boundary: 0.5
-      decide_path_distance: 10.0
-      maximum_deceleration: 1.0
-      maximum_jerk: 1.0
+
       # goal search
-      search_priority: "efficient_path" # "efficient_path" or "close_goal"
-      parking_policy: "left_side" # "left_side" or "right_side"
-      forward_goal_search_length: 20.0
-      backward_goal_search_length: 20.0
-      goal_search_interval: 2.0
-      longitudinal_margin: 3.0
-      max_lateral_offset: 0.5
-      lateral_offset_interval: 0.25
-      ignore_distance_from_lane_start: 15.0
+      goal_search:
+        search_priority: "efficient_path" # "efficient_path" or "close_goal"
+        parking_policy: "left_side" # "left_side" or "right_side"
+        forward_goal_search_length: 20.0
+        backward_goal_search_length: 20.0
+        goal_search_interval: 2.0
+        longitudinal_margin: 3.0
+        max_lateral_offset: 0.5
+        lateral_offset_interval: 0.25
+        ignore_distance_from_lane_start: 15.0
+        margin_from_boundary: 0.5
+
       # occupancy grid map
-      use_occupancy_grid: true
-      use_occupancy_grid_for_longitudinal_margin: false
-      occupancy_grid_collision_check_margin: 0.0
-      theta_size: 360
-      obstacle_threshold: 60
+      occupancy_grid:
+        use_occupancy_grid: true
+        use_occupancy_grid_for_longitudinal_margin: false
+        occupancy_grid_collision_check_margin: 0.0
+        theta_size: 360
+        obstacle_threshold: 60
+
       # object recognition
-      use_object_recognition: true
-      object_recognition_collision_check_margin: 1.0
-      # shift path
-      enable_shift_parking: true
-      pull_over_sampling_num: 4
-      maximum_lateral_jerk: 2.0
-      minimum_lateral_jerk: 0.5
-      deceleration_interval: 15.0
-      after_pull_over_straight_distance: 1.0
-      # freespace parking
-      enable_freespace_parking: true
-      freespace_parking:
-        planning_algorithm: "astar"  # options: astar, rrtstar
-        velocity: 1.0
-        vehicle_shape_margin: 1.0
-        time_limit: 3000.0
-        minimum_turning_radius: 5.0
-        maximum_turning_radius: 5.0
-        turning_radius_size: 1
-        # search configs
-        theta_size: 144
-        angle_goal_range: 6.0
-        curve_weight: 1.2
-        reverse_weight: 1.0
-        lateral_goal_range: 0.5
-        longitudinal_goal_range: 2.0
-        # costmap configs
-        obstacle_threshold: 30
-        # -- A* search Configurations --
-        astar:
-          only_behind_solutions: false
-          use_back: false
-          distance_heuristic_weight: 1.0
-        # -- RRT* search Configurations --
-        rrtstar:
-          enable_update: true
-          use_informed_sampling: true
-          max_planning_time: 150.0
-          neighbor_radius: 8.0
-          margin: 1.0
-      # parallel parking path
-      enable_arc_forward_parking: true
-      enable_arc_backward_parking: true
-      after_forward_parking_straight_distance: 2.0
-      after_backward_parking_straight_distance: 2.0
-      forward_parking_velocity: 1.38
-      backward_parking_velocity: -1.38
-      forward_parking_lane_departure_margin: 0.0
-      backward_parking_lane_departure_margin: 0.0
-      arc_path_interval: 1.0
-      pull_over_max_steer_angle: 0.35  # 20deg
-      # hazard on when parked
-      hazard_on_threshold_distance: 1.0
-      hazard_on_threshold_velocity: 0.5
-      # check safety with dynamic objects. Not used now.
-      pull_over_duration: 2.0
-      pull_over_prepare_duration: 4.0
-      min_stop_distance: 5.0
-      stop_time: 2.0
-      hysteresis_buffer_distance: 2.0
-      prediction_time_resolution: 0.5
-      enable_collision_check_at_prepare_phase: false
-      use_predicted_path_outside_lanelet: false
-      use_all_predicted_path: false
+      object_recognition:
+        use_object_recognition: true
+        object_recognition_collision_check_margin: 1.0
+
+      # pull over
+      pull_over:
+        pull_over_velocity: 3.0
+        pull_over_minimum_velocity: 1.38
+        decide_path_distance: 10.0
+        maximum_deceleration: 1.0
+        maximum_jerk: 1.0
+
+        # shift parking
+        shift_parking:
+          enable_shift_parking: true
+          shift_sampling_num: 4
+          maximum_lateral_jerk: 2.0
+          minimum_lateral_jerk: 0.5
+          deceleration_interval: 15.0
+          after_shift_straight_distance: 1.0
+
+        # parallel parking path
+        parallel_parking:
+          path_interval: 1.0
+          max_steer_angle: 0.35  # 20deg
+          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
+          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
+
+        # freespace parking
+        freespace_parking:
+          enable_freespace_parking: true
+          freespace_parking_algorithm: "astar"  # options: astar, rrtstar
+          velocity: 1.0
+          vehicle_shape_margin: 1.0
+          time_limit: 3000.0
+          minimum_turning_radius: 5.0
+          maximum_turning_radius: 5.0
+          turning_radius_size: 1
+          # search configs
+          search_configs:
+            theta_size: 144
+            angle_goal_range: 6.0
+            curve_weight: 1.2
+            reverse_weight: 1.0
+            lateral_goal_range: 0.5
+            longitudinal_goal_range: 2.0
+          # costmap configs
+          costmap_configs:
+            obstacle_threshold: 30
+          # -- A* search Configurations --
+          astar:
+            only_behind_solutions: false
+            use_back: false
+            distance_heuristic_weight: 1.0
+          # -- RRT* search Configurations --
+          rrtstar:
+            enable_update: true
+            use_informed_sampling: true
+            max_planning_time: 150.0
+            neighbor_radius: 8.0
+            margin: 1.0
+
       # debug
-      print_debug_info: false
+      debug:
+        print_debug_info: false
diff --git a/planning/behavior_path_planner/docs/behavior_path_planner_goal_planner_design.md b/planning/behavior_path_planner/docs/behavior_path_planner_goal_planner_design.md
@@ -153,14 +153,14 @@ The lateral jerk is searched for among the predetermined minimum and maximum val
 
 #### Parameters for shift parking
 
-| Name                              | Unit   | Type   | Description                                                         | Default value |
-| :-------------------------------- | :----- | :----- | :------------------------------------------------------------------ | :------------ |
-| enable_shift_parking              | [-]    | bool   | flag whether to enable shift parking                                | true          |
-| pull_over_sampling_num            | [-]    | int    | Number of samplings in the minimum to maximum range of lateral_jerk | 4             |
-| maximum_lateral_jerk              | [m/s3] | double | maximum lateral jerk                                                | 2.0           |
-| minimum_lateral_jerk              | [m/s3] | double | minimum lateral jerk                                                | 0.5           |
-| deceleration_interval             | [m]    | double | distance of deceleration section                                    | 15.0          |
-| after_pull_over_straight_distance | [m]    | double | straight line distance after pull over end point                    | 5.0           |
+| Name                          | Unit   | Type   | Description                                                         | Default value |
+| :---------------------------- | :----- | :----- | :------------------------------------------------------------------ | :------------ |
+| enable_shift_parking          | [-]    | bool   | flag whether to enable shift parking                                | true          |
+| shift_sampling_num            | [-]    | int    | Number of samplings in the minimum to maximum range of lateral_jerk | 4             |
+| maximum_lateral_jerk          | [m/s3] | double | maximum lateral jerk                                                | 2.0           |
+| minimum_lateral_jerk          | [m/s3] | double | minimum lateral jerk                                                | 0.5           |
+| deceleration_interval         | [m]    | double | distance of deceleration section                                    | 15.0          |
+| after_shift_straight_distance | [m]    | double | straight line distance after pull over end point                    | 5.0           |
 
 ### **geometric parallel parking**
 

diff --git a/...h_planner/include/behavior_path_planner/scene_module/goal_planner/goal_planner_module.hpp b/...h_planner/include/behavior_path_planner/scene_module/goal_planner/goal_planner_module.hpp
@@ -165,7 +165,6 @@ class GoalPlannerModule : public SceneModuleInterface
   bool incrementPathIndex();
   PathWithLaneId getCurrentPath() const;
   Pose calcRefinedGoal(const Pose & goal_pose) const;
-  ParallelParkingParameters getGeometricGoalPlannerParameters() const;
   double calcSignedArcLengthFromEgo(const PathWithLaneId & path, const Pose & pose) const;
   void decelerateForTurnSignal(const Pose & stop_pose, PathWithLaneId & path) const;
   void decelerateBeforeSearchStart(const Pose & search_start_pose, PathWithLaneId & path) const;

diff --git a/...vior_path_planner/include/behavior_path_planner/scene_module/pull_out/pull_out_module.hpp b/...vior_path_planner/include/behavior_path_planner/scene_module/pull_out/pull_out_module.hpp
@@ -117,7 +117,6 @@ class PullOutModule : public SceneModuleInterface
 
   std::shared_ptr<PullOutPlannerBase> getCurrentPlanner() const;
   PathWithLaneId getFullPath() const;
-  ParallelParkingParameters getGeometricPullOutParameters() const;
   std::vector<Pose> searchPullOutStartPoses();
 
   std::shared_ptr<LaneDepartureChecker> lane_departure_checker_;

diff --git a/...ude/behavior_path_planner/utils/geometric_parallel_parking/geometric_parallel_parking.hpp b/...ude/behavior_path_planner/utils/geometric_parallel_parking/geometric_parallel_parking.hpp
@@ -49,17 +49,27 @@ struct ParallelParkingParameters
 {
   double th_arrived_distance;
   double th_stopped_velocity;
+  double maximum_deceleration;
+
+  // forward parking
   double after_forward_parking_straight_distance;
-  double after_backward_parking_straight_distance;
   double forward_parking_velocity;
+  double forward_parking_lane_departure_margin;
+  double forward_parking_path_interval;
+  double forward_parking_max_steer_angle;
+
+  // backward parking
+  double after_backward_parking_straight_distance;
   double backward_parking_velocity;
-  double departing_velocity;
   double backward_parking_lane_departure_margin;
-  double forward_parking_lane_departure_margin;
-  double departing_lane_departure_margin;
-  double arc_path_interval;
-  double maximum_deceleration;
-  double max_steer_angle;
+  double backward_parking_path_interval;
+  double backward_parking_max_steer_angle;
+
+  // pull_out
+  double pull_out_velocity;
+  double pull_out_lane_departure_margin;
+  double pull_out_path_interval;
+  double pull_out_max_steer_angle;
 };
 
 class GeometricParallelParking
@@ -72,9 +82,14 @@ class GeometricParallelParking
   bool planPullOut(
     const Pose & start_pose, const Pose & goal_pose, const lanelet::ConstLanelets & road_lanes,
     const lanelet::ConstLanelets & shoulder_lanes);
-  void setData(
-    const std::shared_ptr<const PlannerData> & planner_data,
-    const ParallelParkingParameters & parameters);
+  void setParameters(const ParallelParkingParameters & parameters) { parameters_ = parameters; }
+  void setPlannerData(const std::shared_ptr<const PlannerData> & planner_data)
+  {
+    planner_data_ = planner_data;
+  }
+  void setTurningRadius(
+    const BehaviorPathPlannerParameters & common_params, const double max_steer_angle);
+
   void incrementPathIndex();
 
   std::vector<PathWithLaneId> getArcPaths() const { return arc_paths_; }
@@ -103,10 +118,11 @@ class GeometricParallelParking
   std::vector<PathWithLaneId> planOneTrial(
     const Pose & start_pose, const Pose & goal_pose, const double R_E_r,
     const lanelet::ConstLanelets & road_lanes, const lanelet::ConstLanelets & shoulder_lanes,
-    bool is_forward, const double end_pose_offset, const double lane_departure_margin);
+    bool is_forward, const double end_pose_offset, const double lane_departure_margin,
+    const double arc_path_interval);
   PathWithLaneId generateArcPath(
     const Pose & center, const double radius, const double start_yaw, double end_yaw,
-    const bool is_left_turn, const bool is_forward);
+    const double arc_path_interval, const bool is_left_turn, const bool is_forward);
   PathPointWithLaneId generateArcPathPoint(
     const Pose & center, const double radius, const double yaw, const bool is_left_turn,
     const bool is_forward);

diff --git a/...ior_path_planner/include/behavior_path_planner/utils/goal_planner/geometric_pull_over.hpp b/...ior_path_planner/include/behavior_path_planner/utils/goal_planner/geometric_pull_over.hpp
@@ -34,7 +34,6 @@ class GeometricPullOver : public PullOverPlannerBase
 public:
   GeometricPullOver(
     rclcpp::Node & node, const GoalPlannerParameters & parameters,
-    const ParallelParkingParameters & parallel_parking_parameters,
     const LaneDepartureChecker & lane_departure_checker,
     const std::shared_ptr<OccupancyGridBasedCollisionDetector> occupancy_grid_map,
     const bool is_forward);