feat(behavior_path_planner): plan shift pull over/out on curve (autow…

…arefoundation#2335)

* feat(behavior_path_planner): plan shift pull over/out on curve

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

* Update planning/behavior_path_planner/src/scene_module/pull_over/goal_searcher.cpp

Co-authored-by: Fumiya Watanabe <rej55.g@gmail.com>
Signed-off-by: kosuke55 <kosuke.tnp@gmail.com>

* Update planning/behavior_path_planner/src/scene_module/pull_over/pull_over_module.cpp

Co-authored-by: Fumiya Watanabe <rej55.g@gmail.com>
Signed-off-by: kosuke55 <kosuke.tnp@gmail.com>

* remove pull_out_finish_judge_buffer

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

* remove commentout

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

Signed-off-by: kosuke55 <kosuke.tnp@gmail.com>
Co-authored-by: Fumiya Watanabe <rej55.g@gmail.com>

common/motion_utils/include/motion_utils/trajectory/trajectory.hpp

@@ -24,6 +24,7 @@
 #include <algorithm>
 #include <limits>
 #include <stdexcept>
+#include <utility>
 #include <vector>
 
 namespace motion_utils
@@ -580,6 +581,43 @@ double calcArcLength(const T & points)
   return calcSignedArcLength(points, 0, points.size() - 1);
 }
 
+template <class T>
+inline std::vector<double> calcCurvature(const T & points)
+{
+  std::vector<double> curvature_vec(points.size());
+
+  for (size_t i = 1; i < points.size() - 1; ++i) {
+    const auto p1 = tier4_autoware_utils::getPoint(points.at(i - 1));
+    const auto p2 = tier4_autoware_utils::getPoint(points.at(i));
+    const auto p3 = tier4_autoware_utils::getPoint(points.at(i + 1));
+    curvature_vec.at(i) = (tier4_autoware_utils::calcCurvature(p1, p2, p3));
+  }
+  curvature_vec.at(0) = curvature_vec.at(1);
+  curvature_vec.at(curvature_vec.size() - 1) = curvature_vec.at(curvature_vec.size() - 2);
+
+  return curvature_vec;
+}
+
+template <class T>
+inline std::vector<std::pair<double, double>> calcCurvatureAndArcLength(const T & points)
+{
+  // Note that arclength is for the segment, not the sum.
+  std::vector<std::pair<double, double>> curvature_arc_length_vec;
+  curvature_arc_length_vec.push_back(std::pair(0.0, 0.0));
+  for (size_t i = 1; i < points.size() - 1; ++i) {
+    const auto p1 = tier4_autoware_utils::getPoint(points.at(i - 1));
+    const auto p2 = tier4_autoware_utils::getPoint(points.at(i));
+    const auto p3 = tier4_autoware_utils::getPoint(points.at(i + 1));
+    const double curvature = tier4_autoware_utils::calcCurvature(p1, p2, p3);
+    const double arc_length = tier4_autoware_utils::calcDistance2d(points.at(i - 1), points.at(i)) +
+                              tier4_autoware_utils::calcDistance2d(points.at(i), points.at(i + 1));
+    curvature_arc_length_vec.push_back(std::pair(curvature, arc_length));
+  }
+  curvature_arc_length_vec.push_back(std::pair(0.0, 0.0));
+
+  return curvature_arc_length_vec;
+}
+
 /**
  * @brief Calculate distance to the forward stop point from the given src index
  */

planning/behavior_path_planner/README.md

@@ -271,15 +271,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           |
-| before_pull_over_straight_distance | [m]    | double | straight line distance before pull over end point. a safe path should have a distance that includes this | 5.0           |
+| 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           |
 
 ##### **geometric parallel parking**
 
@@ -362,13 +361,13 @@ The Pull Out module is activated when ego-vehicle is stationary and footprint of
 
 #### General parameters for pull_out
 
-| Name                         | Unit  | Type   | Description                                                    | Default value |
-| :--------------------------- | :---- | :----- | :------------------------------------------------------------- | :------------ |
-| th_arrived_distance_m        | [m]   | double | distance threshold for arrival of path termination             | 1.0           |
-| th_stopped_velocity_mps      | [m/s] | double | velocity threshold for arrival of path termination             | 0.01          |
-| th_stopped_time_sec          | [s]   | double | time threshold for arrival of path termination                 | 1.0           |
-| collision_check_margin       | [m]   | double | Obstacle collision check margin                                | 1.0           |
-| pull_out_finish_judge_buffer | [m]   | double | threshold for finish judgment distance from pull out end point | 1.0           |
+| Name                              | Unit  | Type   | Description                                                          | Default value |
+| :-------------------------------- | :---- | :----- | :------------------------------------------------------------------- | :------------ |
+| th_arrived_distance_m             | [m]   | double | distance threshold for arrival of path termination                   | 1.0           |
+| th_stopped_velocity_mps           | [m/s] | double | velocity threshold for arrival of path termination                   | 0.01          |
+| th_stopped_time_sec               | [s]   | double | time threshold for arrival of path termination                       | 1.0           |
+| collision_check_margin            | [m]   | double | Obstacle collision check margin                                      | 1.0           |
+| collision_check_distance_from_end | [m]   | double | collision check distance from end point. currently only for pull out | 15.0          |
 
 #### **Safe check with obstacles in shoulder lane**
 
@@ -396,15 +395,14 @@ Pull out distance is calculated by the speed, lateral deviation, and the lateral
 
 ###### parameters for shift pull out
 
-| Name                              | Unit   | Type   | Description                                                                                                          | Default value |
-| :-------------------------------- | :----- | :----- | :------------------------------------------------------------------------------------------------------------------- | :------------ |
-| enable_shift_pull_out             | -      | bool   | flag whether to enable shift pull out                                                                                | true          |
-| shift_pull_out_velocity           | [m/s]  | double | velocity of shift pull out                                                                                           | 2.0           |
-| pull_out_sampling_num             | -      | int    | Number of samplings in the minimum to maximum range of lateral_jerk                                                  | 4             |
-| before_pull_out_straight_distance | [m]    | double | distance before pull out start point                                                                                 | 0.0           |
-| maximum_lateral_jerk              | [m/s3] | double | maximum lateral jerk                                                                                                 | 2.0           |
-| minimum_lateral_jerk              | [m/s3] | double | minimum lateral jerk                                                                                                 | 0.5           |
-| minimum_shift_pull_out_distance   | [m]    | double | minimum shift pull out distance. if calculated pull out distance is shorter than this, use this for path generation. | 20.0          |
+| Name                            | Unit   | Type   | Description                                                                                                          | Default value |
+| :------------------------------ | :----- | :----- | :------------------------------------------------------------------------------------------------------------------- | :------------ |
+| enable_shift_pull_out           | -      | bool   | flag whether to enable shift pull out                                                                                | true          |
+| shift_pull_out_velocity         | [m/s]  | double | velocity of shift pull out                                                                                           | 2.0           |
+| pull_out_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           |
+| minimum_shift_pull_out_distance | [m]    | double | minimum shift pull out distance. if calculated pull out distance is shorter than this, use this for path generation. | 20.0          |
 
 ##### **geometric pull out**
 

planning/behavior_path_planner/config/pull_out/pull_out.param.yaml

@@ -5,12 +5,11 @@
       th_stopped_velocity: 0.01
       th_stopped_time: 1.0
       collision_check_margin: 1.0
-      pull_out_finish_judge_buffer: 1.0
+      collision_check_distance_from_end: 1.0
       # shift pull out
       enable_shift_pull_out: true
       shift_pull_out_velocity: 2.0
       pull_out_sampling_num: 4
-      before_pull_out_straight_distance: 0.0
       minimum_shift_pull_out_distance: 20.0
       maximum_lateral_jerk: 2.0
       minimum_lateral_jerk: 0.5

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

@@ -35,7 +35,6 @@
       minimum_lateral_jerk: 0.5
       deceleration_interval: 15.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

planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_out/pull_out_module.hpp

@@ -114,6 +114,7 @@ class PullOutModule : public SceneModuleInterface
     const std::vector<Pose> & start_pose_candidates, const Pose & goal_pose);
   void planWithPriorityOnShortBackDistance(
     const std::vector<Pose> & start_pose_candidates, const Pose & goal_pose);
+  void generateStopPath();
   void updatePullOutStatus();
   static bool isOverlappedWithLane(
     const lanelet::ConstLanelet & candidate_lanelet,

planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_out/pull_out_parameters.hpp

@@ -26,12 +26,11 @@ struct PullOutParameters
   double th_stopped_velocity;
   double th_stopped_time;
   double collision_check_margin;
-  double pull_out_finish_judge_buffer;
+  double collision_check_distance_from_end;
   // shift pull out
   bool enable_shift_pull_out;
   double shift_pull_out_velocity;
   int pull_out_sampling_num;
-  double before_pull_out_straight_distance;
   double minimum_shift_pull_out_distance;
   double maximum_lateral_jerk;
   double minimum_lateral_jerk;

planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_out/shift_pull_out.hpp

@@ -49,6 +49,9 @@ class ShiftPullOut : public PullOutPlannerBase
     const double pull_out_total_distance, const lanelet::ConstLanelets & current_lanes,
     const Pose & current_pose, const bool isInGoalRouteSection, const Pose & goal_pose);
 
+  double calcBeforeShiftedArcLength(
+    const PathWithLaneId & path, const double target_after_arc_length, const double dr);
+
   std::shared_ptr<LaneDepartureChecker> lane_departure_checker_;
 };
 }  // namespace behavior_path_planner

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

@@ -18,11 +18,14 @@
 #include "behavior_path_planner/scene_module/pull_over/goal_searcher_base.hpp"
 #include "behavior_path_planner/scene_module/utils/occupancy_grid_based_collision_detector.hpp"
 
+#include "autoware_auto_planning_msgs/msg/path_point_with_lane_id.hpp"
+
 #include <memory>
 #include <vector>
 
 namespace behavior_path_planner
 {
+using autoware_auto_planning_msgs::msg::PathPointWithLaneId;
 using tier4_autoware_utils::LinearRing2d;
 
 class GoalSearcher : public GoalSearcherBase
@@ -32,12 +35,14 @@ class GoalSearcher : public GoalSearcherBase
     const PullOverParameters & parameters, const LinearRing2d & vehicle_footprint,
     const std::shared_ptr<OccupancyGridBasedCollisionDetector> & occupancy_grid_map);
 
-  std::vector<GoalCandidate> search(const Pose & original_goal_pose) override;
+  GoalCandidates search(const Pose & original_goal_pose) override;
+
+private:
+  void createAreaPolygons(std::vector<Pose> original_search_poses);
   bool checkCollision(const Pose & pose) const;
   bool checkCollisionWithLongitudinalDistance(
     const Pose & ego_pose, const PredictedObjects & dynamic_objects) const;
 
-private:
   LinearRing2d vehicle_footprint_{};
   std::shared_ptr<OccupancyGridBasedCollisionDetector> occupancy_grid_map_{};
 };

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

@@ -28,22 +28,21 @@
 namespace behavior_path_planner
 {
 using geometry_msgs::msg::Pose;
+using tier4_autoware_utils::MultiPolygon2d;
 
 struct GoalCandidate
 {
   Pose goal_pose{};
   double distance_from_original_goal{0.0};
   double lateral_offset{0.0};
+  size_t id{0};
 
   bool operator<(const GoalCandidate & other) const noexcept
   {
-    // compare in order of decreasing lateral offset.
-    if (std::abs(lateral_offset - other.lateral_offset) > std::numeric_limits<double>::epsilon()) {
-      return lateral_offset < other.lateral_offset;
-    }
     return distance_from_original_goal < other.distance_from_original_goal;
   }
 };
+using GoalCandidates = std::vector<GoalCandidate>;
 
 class GoalSearcherBase
 {
@@ -56,11 +55,13 @@ class GoalSearcherBase
     planner_data_ = planner_data;
   }
 
-  virtual std::vector<GoalCandidate> search(const Pose & original_goal_pose) = 0;
+  MultiPolygon2d getAreaPolygons() { return area_polygons_; }
+  virtual GoalCandidates search(const Pose & original_goal_pose) = 0;
 
 protected:
   PullOverParameters parameters_;
   std::shared_ptr<const PlannerData> planner_data_;
+  MultiPolygon2d area_polygons_;
 };
 }  // namespace behavior_path_planner
 

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

@@ -65,6 +65,7 @@ struct PUllOverStatus
   bool prev_is_safe{false};
   bool has_decided_velocity{false};
   bool has_requested_approval{false};
+  std::optional<Pose> stop_pose{};
 };
 
 class PullOverModule : public SceneModuleInterface
@@ -103,17 +104,14 @@ class PullOverModule : public SceneModuleInterface
   PullOverPath shift_parking_path_;
   vehicle_info_util::VehicleInfo vehicle_info_;
 
-  const double pull_over_lane_length_{200.0};
-  const double check_distance_{100.0};
-
   rclcpp::Subscription<OccupancyGrid>::SharedPtr occupancy_grid_sub_;
   rclcpp::Publisher<PoseStamped>::SharedPtr goal_pose_pub_;
 
   PUllOverStatus status_;
   std::shared_ptr<OccupancyGridBasedCollisionDetector> occupancy_grid_map_;
   Pose modified_goal_pose_;
   Pose refined_goal_pose_;
-  std::vector<GoalCandidate> goal_candidates_;
+  GoalCandidates goal_candidates_;
   GeometricParallelParking parallel_parking_planner_;
   ParallelParkingParameters parallel_parking_parameters_;
   std::deque<nav_msgs::msg::Odometry::ConstSharedPtr> odometry_buffer_;
@@ -125,16 +123,11 @@ class PullOverModule : public SceneModuleInterface
   void incrementPathIndex();
   PathWithLaneId getCurrentPath() const;
   PathWithLaneId getFullPath() const;
-  PathWithLaneId getReferencePath() const;
-  PathWithLaneId generateStopPath() const;
-  Pose calcRefinedGoal() const;
+  Pose calcRefinedGoal(const Pose & goal_pose) const;
   ParallelParkingParameters getGeometricPullOverParameters() 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;
-  double calcMinimumShiftPathDistance() const;
   std::pair<double, double> calcDistanceToPathChange() const;
+  PathWithLaneId generateStopPath();
+  PathWithLaneId generateEmergencyStopPath();
 
   bool isStopped();
   bool hasFinishedCurrentPath();

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

@@ -56,7 +56,6 @@ struct PullOverParameters
   double pull_over_velocity;
   double pull_over_minimum_velocity;
   double after_pull_over_straight_distance;
-  double before_pull_over_straight_distance;
   // parallel parking
   bool enable_arc_forward_parking;
   bool enable_arc_backward_parking;

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

@@ -47,6 +47,7 @@ struct PullOverPath
   std::vector<PathWithLaneId> partial_paths{};
   Pose start_pose{};
   Pose end_pose{};
+  std::vector<Pose> debug_poses{};
 };
 
 class PullOverPlannerBase

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

@@ -41,21 +41,22 @@ class ShiftPullOver : public PullOverPlannerBase
   boost::optional<PullOverPath> plan(const Pose & goal_pose) override;
 
 protected:
-  PathWithLaneId generateRoadLaneReferencePath(
-    const lanelet::ConstLanelets & road_lanes, const Pose & shift_end_pose,
-    const double pull_over_distance) const;
-  PathWithLaneId generateShoulderLaneReferencePath(
-    const lanelet::ConstLanelets & shoulder_lanes, const Pose & shift_start_pose,
-    const Pose & goal_pose, const double shoulder_center_to_goal_distance) const;
+  PathWithLaneId generateReferencePath(
+    const lanelet::ConstLanelets & road_lanes, const Pose & end_pose) const;
   boost::optional<PullOverPath> generatePullOverPath(
     const lanelet::ConstLanelets & road_lanes, const lanelet::ConstLanelets & shoulder_lanes,
-    const Pose & shift_end_pose, const Pose & goal_pose, const double lateral_jerk,
-    const double road_center_to_goal_distance, const double shoulder_center_to_goal_distance,
-    const double shoulder_left_bound_to_goal_distance) const;
+    const Pose & goal_pose, const double lateral_jerk) const;
   bool hasEnoughDistance(
     const PathWithLaneId & path, const lanelet::ConstLanelets & road_lanes, const Pose & start_pose,
     const Pose & goal_pose, const double pull_over_distance) const;
   bool isSafePath(const PathWithLaneId & path) const;
+  static double calcBeforeShiftedArcLength(
+    const PathWithLaneId & path, const double after_shifted_arc_length, const double dr);
+  static std::vector<double> splineTwoPoints(
+    std::vector<double> base_s, std::vector<double> base_x, const double begin_diff,
+    const double end_diff, std::vector<double> new_s);
+  static std::vector<Pose> interpolatePose(
+    const Pose & start_pose, const Pose & end_pose, const double resample_interval);
 
   LaneDepartureChecker lane_departure_checker_{};
   std::shared_ptr<OccupancyGridBasedCollisionDetector> occupancy_grid_map_{};