feat(behavior_path_palnner): update geometric parallel parking for pu…

…ll_out module (#1534)

Signed-off-by: kosuke55 <kosuke.tnp@gmail.com>

Signed-off-by: kosuke55 <kosuke.tnp@gmail.com>

launch/tier4_planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/pull_over/pull_over.param.yaml

@@ -9,13 +9,10 @@
       pull_over_minimum_velocity: 0.3
       margin_from_boundary: 0.5
       decide_path_distance: 10.0
-      min_acc: -0.5
-      enable_shift_parking: true
-      enable_arc_forward_parking: true
-      enable_arc_backward_parking: true
+      maximum_deceleration: 0.5
       # goal research
-      search_priority: "efficient_path" # "efficient_path" or "close_goal"
       enable_goal_research: true
+      search_priority: "efficient_path" # "efficient_path" or "close_goal"
       forward_goal_search_length: 20.0
       backward_goal_search_length: 20.0
       goal_search_interval: 1.0
@@ -25,20 +22,25 @@
       theta_size: 360
       obstacle_threshold: 60
       # 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
-      maximum_deceleration: 1.0
       after_pull_over_straight_distance: 5.0
       before_pull_over_straight_distance: 5.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: 0.3
       backward_parking_velocity: -0.3
+      forward_parking_lane_departure_margin: 0.0
+      backward_parking_lane_departure_margin: 0.0
       arc_path_interval: 1.0
-      # hazard_on when parked
+      max_steer_rad: 0.35  # 20deg
+      # hazard on when parked
       hazard_on_threshold_dis: 1.0
       hazard_on_threshold_vel: 0.5
       # check safety with dynamic objects. Not used now.

planning/behavior_path_planner/config/pull_over/pull_over.param.yaml

@@ -9,36 +9,38 @@
       pull_over_minimum_velocity: 0.3
       margin_from_boundary: 0.5
       decide_path_distance: 10.0
-      min_acc: -0.5
-      enable_shift_parking: true
-      enable_arc_forward_parking: true
-      enable_arc_backward_parking: true
+      maximum_deceleration: 0.5
       # goal research
-      search_priority: "efficient_path" # "efficient_path" or "close_goal"
       enable_goal_research: true
+      search_priority: "efficient_path" # "efficient_path" or "close_goal"
       forward_goal_search_length: 20.0
       backward_goal_search_length: 20.0
       goal_search_interval: 1.0
       goal_to_obj_margin: 2.0
       # occupancy grid map
-      collision_check_margin: 0.0
+      collision_check_margin: 0.5
       theta_size: 360
       obstacle_threshold: 60
       # 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
-      maximum_deceleration: 1.0
       after_pull_over_straight_distance: 5.0
       before_pull_over_straight_distance: 5.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: 0.3
       backward_parking_velocity: -0.3
+      forward_parking_lane_departure_margin: 0.0
+      backward_parking_lane_departure_margin: 0.0
       arc_path_interval: 1.0
-      # hazard. Not used now.
+      max_steer_rad: 0.35  # 20deg
+      # hazard on when parked
       hazard_on_threshold_dis: 1.0
       hazard_on_threshold_vel: 0.5
       # check safety with dynamic objects. Not used now.

planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_over/pull_over_module.hpp

@@ -56,10 +56,7 @@ struct PullOverParameters
   double th_stopped_time_sec;
   double margin_from_boundary;
   double decide_path_distance;
-  double min_acc;
-  bool enable_shift_parking;
-  bool enable_arc_forward_parking;
-  bool enable_arc_backward_parking;
+  double maximum_deceleration;
   // goal research
   std::string search_priority;  // "efficient_path" or "close_goal"
   bool enable_goal_research;
@@ -72,22 +69,27 @@ struct PullOverParameters
   double theta_size;
   double obstacle_threshold;
   // shift path
+  bool enable_shift_parking;
   int pull_over_sampling_num;
   double maximum_lateral_jerk;
   double minimum_lateral_jerk;
   double deceleration_interval;
   double pull_over_velocity;
   double pull_over_minimum_velocity;
-  double maximum_deceleration;
   double after_pull_over_straight_distance;
   double before_pull_over_straight_distance;
   // parallel parking
+  bool enable_arc_forward_parking;
+  bool enable_arc_backward_parking;
   double after_forward_parking_straight_distance;
   double after_backward_parking_straight_distance;
   double forward_parking_velocity;
   double backward_parking_velocity;
+  double forward_parking_lane_departure_margin;
+  double backward_parking_lane_departure_margin;
   double arc_path_interval;
-  // hazard. Not used now.
+  double max_steer_rad;
+  // hazard
   double hazard_on_threshold_dis;
   double hazard_on_threshold_vel;
   // check safety with dynamic objects. Not used now.
@@ -198,13 +200,14 @@ class PullOverModule : public SceneModuleInterface
   std::pair<bool, bool> getSafePath(ShiftParkingPath & safe_path) const;
   Pose getRefinedGoal() const;
   Pose getParkingStartPose() const;
-  bool isLongEnoughToParkingStart(const PathWithLaneId path, const Pose parking_start_pose) const;
+  ParallelParkingParameters getGeometricPullOutParameters() const;
+  bool isLongEnoughToParkingStart(
+    const PathWithLaneId & path, const Pose & parking_start_pose) const;
   bool isLongEnough(
-    const lanelet::ConstLanelets & lanelets, const Pose goal_pose, const double buffer = 0) const;
+    const lanelet::ConstLanelets & lanelets, const Pose & goal_pose, const double buffer = 0) const;
   bool isArcPath() const;
   double calcMinimumShiftPathDistance() const;
   double calcDistanceToPathChange() const;
-
   bool planShiftPath();
   bool isStopped();
   bool hasFinishedCurrentPath();
@@ -218,7 +221,7 @@ class PullOverModule : public SceneModuleInterface
   std::pair<TurnIndicatorsCommand, double> getTurnInfo() const;
 
   // debug
-  Marker createParkingAreaMarker(const Pose back_pose, const Pose front_pose, const int32_t id);
+  Marker createParkingAreaMarker(const Pose & back_pose, const Pose & front_pose, const int32_t id);
   void publishDebugData();
   void printParkingPositionError() const;
 };

planning/behavior_path_planner/include/behavior_path_planner/scene_module/utils/geometric_parallel_parking.hpp

@@ -54,57 +54,74 @@ struct ParallelParkingParameters
   double after_backward_parking_straight_distance;
   double forward_parking_velocity;
   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 min_acc;
+  double maximum_deceleration;
+  double max_steer_rad;
 };
 
 class GeometricParallelParking
 {
 public:
   bool isParking() const;
-  bool plan(const Pose goal_pose, lanelet::ConstLanelets lanes, const bool is_forward);
-  void setParams(
-    const std::shared_ptr<const PlannerData> & planner_data, ParallelParkingParameters parameters);
+  bool planPullOver(
+    const Pose & goal_pose, const lanelet::ConstLanelets & road_lanes,
+    const lanelet::ConstLanelets & shoulder_lanesconst, const bool is_forward);
+  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 incrementPathIndex();
-  void clear();
 
+  std::vector<PathWithLaneId> getArcPaths() const { return arc_paths_; }
+  std::vector<PathWithLaneId> getPaths() const { return paths_; }
+  PathWithLaneId getPathByIdx(size_t const idx) const;
   PathWithLaneId getCurrentPath() const;
   PathWithLaneId getFullPath() const;
   PathWithLaneId getArcPath() const;
   PoseStamped getCr() const { return Cr_; }
   PoseStamped getCl() const { return Cl_; }
   PoseStamped getStartPose() const { return start_pose_; }
   PoseStamped getArcEndPose() const { return arc_end_pose_; }
-
   PoseArray getPathPoseArray() const { return path_pose_array_; }
 
 private:
   std::shared_ptr<const PlannerData> planner_data_;
   ParallelParkingParameters parameters_;
 
-  // todo: use vehicle info after merging
-  // https://github.com/autowarefoundation/autoware.universe/pull/740
-  float max_steer_deg_ = 40.0;  // max steering angle [deg].
-  float max_steer_rad_;
-  float R_E_min_;   // base_link
-  float R_Bl_min_;  // front_left
+  double R_E_min_;   // base_link
+  double R_Bl_min_;  // front_left
 
+  std::vector<PathWithLaneId> arc_paths_;
   std::vector<PathWithLaneId> paths_;
   size_t current_path_idx_ = 0;
 
-  bool planOneTraial(
-    const Pose goal_pose, const double start_pose_offset, const double R_E_r,
-    const lanelet::ConstLanelets lanes, const bool is_forward);
+  void clearPaths();
+  bool isEnoughDistanceToStart(const Pose & start_pose) const;
+  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_lanesconst,
+    bool is_forward, const double end_pose_offset, const double lane_departure_margin);
   PathWithLaneId generateArcPath(
-    const Pose & center, const float radius, const float start_rad, float end_rad,
+    const Pose & center, const double radius, const double start_yaw, double end_yaw,
     const bool is_left_turn, const bool is_forward);
   PathPointWithLaneId generateArcPathPoint(
-    const Pose & center, const float radius, const float yaw, const bool is_left_turn,
+    const Pose & center, const double radius, const double yaw, const bool is_left_turn,
     const bool is_forward);
-  Pose calcStartPose(
-    const Pose goal_pose, const double start_pose_offset, const double R_E_r,
+  boost::optional<Pose> calcStartPose(
+    const Pose & goal_pose, const double start_pose_offset, const double R_E_r,
     const bool is_forward);
-  void generateStraightPath(const Pose start_pose);
+  std::vector<PathWithLaneId> generatePullOverPaths(
+    const Pose & start_pose, const Pose & goal_pose, const double R_E_r,
+    const lanelet::ConstLanelets & road_lanes, const lanelet::ConstLanelets & shoulder_lanes,
+    const bool is_forward, const double end_pose_offset, const double velocity);
+  PathWithLaneId generateStraightPath(const Pose & start_pose);
+  void setVelocityToArcPaths(std::vector<PathWithLaneId> & arc_paths, const double velocity);
 
   PoseStamped Cr_;
   PoseStamped Cl_;

planning/behavior_path_planner/src/behavior_path_planner_node.cpp

@@ -384,13 +384,10 @@ PullOverParameters BehaviorPathPlannerNode::getPullOverParam()
   p.th_stopped_time_sec = dp("th_stopped_time_sec", 2.0);
   p.margin_from_boundary = dp("margin_from_boundary", 0.3);
   p.decide_path_distance = dp("decide_path_distance", 10.0);
-  p.min_acc = dp("min_acc", -0.5);
-  p.enable_shift_parking = dp("enable_shift_parking", true);
-  p.enable_arc_forward_parking = dp("enable_arc_forward_parking", true);
-  p.enable_arc_backward_parking = dp("enable_arc_backward_parking", true);
+  p.maximum_deceleration = dp("maximum_deceleration", 0.5);
   // goal research
-  p.search_priority = dp("search_priority", "efficient_path");
   p.enable_goal_research = dp("enable_goal_research", true);
+  p.search_priority = dp("search_priority", "efficient_path");
   p.forward_goal_search_length = dp("forward_goal_search_length", 20.0);
   p.backward_goal_search_length = dp("backward_goal_search_length", 20.0);
   p.goal_search_interval = dp("goal_search_interval", 5.0);
@@ -400,21 +397,26 @@ PullOverParameters BehaviorPathPlannerNode::getPullOverParam()
   p.theta_size = dp("theta_size", 360);
   p.obstacle_threshold = dp("obstacle_threshold", 90);
   // shift path
+  p.enable_shift_parking = dp("enable_shift_parking", true);
   p.pull_over_sampling_num = dp("pull_over_sampling_num", 4);
   p.maximum_lateral_jerk = dp("maximum_lateral_jerk", 3.0);
   p.minimum_lateral_jerk = dp("minimum_lateral_jerk", 1.0);
   p.deceleration_interval = dp("deceleration_interval", 10.0);
   p.pull_over_velocity = dp("pull_over_velocity", 8.3);
   p.pull_over_minimum_velocity = dp("pull_over_minimum_velocity", 0.3);
-  p.maximum_deceleration = dp("maximum_deceleration", 1.0);
   p.after_pull_over_straight_distance = dp("after_pull_over_straight_distance", 3.0);
   p.before_pull_over_straight_distance = dp("before_pull_over_straight_distance", 3.0);
   // parallel parking
+  p.enable_arc_forward_parking = dp("enable_arc_forward_parking", true);
+  p.enable_arc_backward_parking = dp("enable_arc_backward_parking", true);
   p.after_forward_parking_straight_distance = dp("after_forward_parking_straight_distance", 0.5);
   p.after_backward_parking_straight_distance = dp("after_backward_parking_straight_distance", 0.5);
   p.forward_parking_velocity = dp("forward_parking_velocity", 1.0);
   p.backward_parking_velocity = dp("backward_parking_velocity", -0.5);
+  p.forward_parking_lane_departure_margin = dp("forward_parking_lane_departure_margin", 0.0);
+  p.backward_parking_lane_departure_margin = dp("backward_parking_lane_departure_margin", 0.0);
   p.arc_path_interval = dp("arc_path_interval", 1.0);
+  p.max_steer_rad = dp("max_steer_rad", 0.35);  // 20deg
   // hazard
   p.hazard_on_threshold_dis = dp("hazard_on_threshold_dis", 1.0);
   p.hazard_on_threshold_vel = dp("hazard_on_threshold_vel", 0.5);