feat(behavior_path_planner): generate pull over path candidates in ad…

…vance (autowarefoundation#2578)

* feat(behavior_path_planner): generate pull over path candidates in advance

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

* add update

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>

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

Co-authored-by: Fumiya Watanabe <rej55.g@gmail.com>

* copy for multithread

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>

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

@@ -37,6 +37,7 @@ class GoalSearcher : public GoalSearcherBase
     const std::shared_ptr<OccupancyGridBasedCollisionDetector> & occupancy_grid_map);
 
   GoalCandidates search(const Pose & original_goal_pose) override;
+  void update(GoalCandidates & goal_candidates) const override;
 
 private:
   void createAreaPolygons(std::vector<Pose> original_search_poses);

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

@@ -36,9 +36,16 @@ struct GoalCandidate
   double distance_from_original_goal{0.0};
   double lateral_offset{0.0};
   size_t id{0};
+  bool is_safe{true};
 
   bool operator<(const GoalCandidate & other) const noexcept
   {
+    const double diff = distance_from_original_goal - other.distance_from_original_goal;
+    constexpr double eps = 0.01;
+    if (std::abs(diff) < eps) {
+      return lateral_offset < other.lateral_offset;
+    }
+
     return distance_from_original_goal < other.distance_from_original_goal;
   }
 };
@@ -57,6 +64,7 @@ class GoalSearcherBase
 
   MultiPolygon2d getAreaPolygons() { return area_polygons_; }
   virtual GoalCandidates search(const Pose & original_goal_pose) = 0;
+  virtual void update([[maybe_unused]] GoalCandidates & goal_candidates) const { return; }
 
 protected:
   PullOverParameters parameters_;

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

@@ -53,7 +53,6 @@ enum PathType {
 
 struct PUllOverStatus
 {
-  std::shared_ptr<PullOverPlannerBase> planner{};
   PullOverPath pull_over_path{};
   size_t current_path_idx{0};
   std::shared_ptr<PathWithLaneId> prev_stop_path{nullptr};
@@ -83,9 +82,6 @@ class PullOverModule : public SceneModuleInterface
   {
     return BT::NodeStatus::SUCCESS;
   }
-  void onTimer();
-  bool planWithEfficientPath();
-  bool planWithCloseGoal();
   BehaviorModuleOutput plan() override;
   BehaviorModuleOutput planWaitingApproval() override;
   CandidateOutput planCandidate() const override;
@@ -116,6 +112,8 @@ class PullOverModule : public SceneModuleInterface
   Pose modified_goal_pose_;
   Pose refined_goal_pose_;
   GoalCandidates goal_candidates_;
+  std::vector<PullOverPath> pull_over_path_candidates_;
+  std::optional<Pose> closest_start_pose_;
   GeometricParallelParking parallel_parking_planner_;
   ParallelParkingParameters parallel_parking_parameters_;
   std::deque<nav_msgs::msg::Odometry::ConstSharedPtr> odometry_buffer_;
@@ -126,7 +124,6 @@ class PullOverModule : public SceneModuleInterface
 
   void incrementPathIndex();
   PathWithLaneId getCurrentPath() const;
-  PathWithLaneId getFullPath() const;
   Pose calcRefinedGoal(const Pose & goal_pose) const;
   ParallelParkingParameters getGeometricPullOverParameters() const;
   std::pair<double, double> calcDistanceToPathChange() const;
@@ -139,8 +136,17 @@ class PullOverModule : public SceneModuleInterface
   void updateOccupancyGrid();
   void resetStatus();
 
+  bool checkCollision(const PathWithLaneId & path) const;
+  bool hasEnoughDistance(const PullOverPath & pull_over_path) const;
+
   TurnSignalInfo calcTurnSignalInfo() const;
 
+  // timer for generating pull over path candidates
+  void onTimer();
+  rclcpp::TimerBase::SharedPtr timer_;
+  rclcpp::CallbackGroup::SharedPtr timer_cb_group_;
+  std::mutex mutex_;
+
   // debug
   void setDebugData();
   void printParkingPositionError() const;

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

@@ -43,11 +43,44 @@ enum class PullOverPlannerType {
 
 struct PullOverPath
 {
-  PathWithLaneId path{};
+  PullOverPlannerType type{PullOverPlannerType::NONE};
   std::vector<PathWithLaneId> partial_paths{};
   Pose start_pose{};
   Pose end_pose{};
   std::vector<Pose> debug_poses{};
+  size_t goal_id{};
+
+  PathWithLaneId getFullPath() const
+  {
+    PathWithLaneId path{};
+    for (size_t i = 0; i < partial_paths.size(); ++i) {
+      if (i == 0) {
+        path.points.insert(
+          path.points.end(), partial_paths.at(i).points.begin(), partial_paths.at(i).points.end());
+      } else {
+        // skip overlapping point
+        path.points.insert(
+          path.points.end(), next(partial_paths.at(i).points.begin()),
+          partial_paths.at(i).points.end());
+      }
+    }
+    path.points = motion_utils::removeOverlapPoints(path.points);
+
+    return path;
+  }
+
+  PathWithLaneId getParkingPath() const
+  {
+    const PathWithLaneId full_path = getFullPath();
+    const size_t start_idx = motion_utils::findNearestIndex(full_path.points, start_pose.position);
+
+    PathWithLaneId parking_path{};
+    std::copy(
+      full_path.points.begin() + start_idx, full_path.points.end(),
+      std::back_inserter(parking_path.points));
+
+    return parking_path;
+  }
 };
 
 class PullOverPlannerBase

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

@@ -46,10 +46,6 @@ class ShiftPullOver : public PullOverPlannerBase
   boost::optional<PullOverPath> generatePullOverPath(
     const lanelet::ConstLanelets & road_lanes, const lanelet::ConstLanelets & shoulder_lanes,
     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(

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

@@ -101,7 +101,6 @@ class GeometricParallelParking
   size_t current_path_idx_ = 0;
 
   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_lanes,

planning/behavior_path_planner/src/scene_module/pull_over/geometric_pull_over.cpp

@@ -65,22 +65,8 @@ boost::optional<PullOverPath> GeometricPullOver::plan(const Pose & goal_pose)
   // check lane departure with road and shoulder lanes
   if (lane_departure_checker_.checkPathWillLeaveLane(lanes, arc_path)) return {};
 
-  // collision check
-  if (parameters_.use_occupancy_grid || !occupancy_grid_map_) {
-    const bool check_out_of_range = false;
-    if (occupancy_grid_map_->hasObstacleOnPath(arc_path, check_out_of_range)) {
-      return {};
-    }
-  }
-  if (parameters_.use_object_recognition) {
-    if (util::checkCollisionBetweenPathFootprintsAndObjects(
-          vehicle_footprint_, arc_path, *(planner_data_->dynamic_object),
-          parameters_.object_recognition_collision_check_margin)) {
-      return {};
-    }
-  }
-
   PullOverPath pull_over_path{};
+  pull_over_path.type = getPlannerType();
   pull_over_path.partial_paths = planner_.getPaths();
   pull_over_path.start_pose = planner_.getStartPose();
   pull_over_path.end_pose = planner_.getArcEndPose();

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

@@ -44,7 +44,6 @@ GoalCandidates GoalSearcher::search(const Pose & original_goal_pose)
   GoalCandidates goal_candidates{};
 
   const auto & route_handler = planner_data_->route_handler;
-  const double vehicle_width = planner_data_->parameters.vehicle_width;
   const double forward_length = parameters_.forward_goal_search_length;
   const double backward_length = parameters_.backward_goal_search_length;
   const double margin_from_boundary = parameters_.margin_from_boundary;
@@ -66,9 +65,10 @@ GoalCandidates GoalSearcher::search(const Pose & original_goal_pose)
   const auto [shoulder_lane_objects, others] =
     util::separateObjectsByLanelets(*(planner_data_->dynamic_object), pull_over_lanes);
 
-  std::vector<Pose> original_search_poses{};
-  for (size_t goal_id = 0; goal_id < center_line_path.points.size(); ++goal_id) {
-    const auto & center_pose = center_line_path.points.at(goal_id).point.pose;
+  std::vector<Pose> original_search_poses{};  // for search area visualizing
+  size_t goal_id = 0;
+  for (const auto & p : center_line_path.points) {
+    const Pose & center_pose = p.point.pose;
 
     const auto distance_from_left_bound = util::getSignedDistanceFromShoulderLeftBoundary(
       pull_over_lanes, vehicle_footprint_, center_pose);
@@ -79,7 +79,6 @@ GoalCandidates GoalSearcher::search(const Pose & original_goal_pose)
     original_search_poses.push_back(original_search_pose);  // for createAreaPolygon
     Pose search_pose{};
     // search goal_pose in lateral direction
-    bool found_lateral_no_collision_pose = false;
     double lateral_offset = 0.0;
     for (double dy = 0; dy <= max_lateral_offset; dy += lateral_offset_interval) {
       lateral_offset = dy;
@@ -96,47 +95,52 @@ GoalCandidates GoalSearcher::search(const Pose & original_goal_pose)
         continue;
       }
 
-      if (checkCollision(search_pose)) {
-        continue;
-      }
+      GoalCandidate goal_candidate{};
+      goal_candidate.goal_pose = search_pose;
+      goal_candidate.lateral_offset = lateral_offset;
+      goal_candidate.id = goal_id;
+      goal_id++;
+      // use longitudinal_distance as distance_from_original_goal
+      goal_candidate.distance_from_original_goal = std::abs(motion_utils::calcSignedArcLength(
+        center_line_path.points, original_goal_pose.position, search_pose.position));
+      goal_candidates.push_back(goal_candidate);
+    }
+  }
+  createAreaPolygons(original_search_poses);
 
-      // if finding objects or detecting lane departure
-      // shift search_pose in lateral direction one more
-      // for avoiding them on other path points.
-      if (dy > 0 && dy < max_lateral_offset) {
-        search_pose = calcOffsetPose(search_pose, 0, -lateral_offset_interval, 0);
-      }
+  // Sort with distance from original goal
+  std::sort(goal_candidates.begin(), goal_candidates.end());
 
-      found_lateral_no_collision_pose = true;
-      break;
-    }
-    if (!found_lateral_no_collision_pose) {
+  return goal_candidates;
+}
+
+void GoalSearcher::update(GoalCandidates & goal_candidates) const
+{
+  // update is_safe
+  for (auto & goal_candidate : goal_candidates) {
+    const Pose goal_pose = goal_candidate.goal_pose;
+
+    // check collision with footprint
+    if (checkCollision(goal_pose)) {
+      goal_candidate.is_safe = false;
       continue;
     }
 
+    // check margin with pull over lane objects
+    const auto pull_over_lanes = pull_over_utils::getPullOverLanes(*(planner_data_->route_handler));
+    const auto [shoulder_lane_objects, others] =
+      util::separateObjectsByLanelets(*(planner_data_->dynamic_object), pull_over_lanes);
     constexpr bool filter_inside = true;
     const auto target_objects = pull_over_utils::filterObjectsByLateralDistance(
-      search_pose, vehicle_width, shoulder_lane_objects,
+      goal_pose, planner_data_->parameters.vehicle_width, shoulder_lane_objects,
       parameters_.object_recognition_collision_check_margin, filter_inside);
-    if (checkCollisionWithLongitudinalDistance(search_pose, target_objects)) {
+    if (checkCollisionWithLongitudinalDistance(goal_pose, target_objects)) {
+      goal_candidate.is_safe = false;
       continue;
     }
 
-    GoalCandidate goal_candidate{};
-    goal_candidate.goal_pose = search_pose;
-    goal_candidate.lateral_offset = lateral_offset;
-    goal_candidate.id = goal_id;
-    // use longitudinal_distance as distance_from_original_goal
-    goal_candidate.distance_from_original_goal = std::abs(motion_utils::calcSignedArcLength(
-      center_line_path.points, original_goal_pose.position, search_pose.position));
-    goal_candidates.push_back(goal_candidate);
+    goal_candidate.is_safe = true;
   }
-  createAreaPolygons(original_search_poses);
-
-  // Sort with distance from original goal
-  std::sort(goal_candidates.begin(), goal_candidates.end());
-
-  return goal_candidates;
 }
 
 bool GoalSearcher::checkCollision(const Pose & pose) const