refactor(goal_planner): refactor select path

Signed-off-by: kosuke55 <[email protected]>

planning/behavior_path_planner/include/behavior_path_planner/scene_module/goal_planner/goal_planner_module.hpp

@@ -168,20 +168,39 @@ class ThreadSafeData
     return false;
   }
 
-  void set_pull_over_path(const PullOverPath & value)
+  void set_pull_over_path(const PullOverPath & path)
   {
     const std::lock_guard<std::recursive_mutex> lock(mutex_);
-    pull_over_path_ = std::make_shared<PullOverPath>(value);
+    pull_over_path_ = std::make_shared<PullOverPath>(path);
+    if (path.type != PullOverPlannerType::NONE && path.type != PullOverPlannerType::FREESPACE) {
+      lane_parking_pull_over_path_ = std::make_shared<PullOverPath>(path);
+    }
+
     last_path_update_time_ = clock_->now();
   }
 
-  void set_pull_over_path(const std::shared_ptr<PullOverPath> & value)
+  void set_pull_over_path(const std::shared_ptr<PullOverPath> & path)
   {
     const std::lock_guard<std::recursive_mutex> lock(mutex_);
-    pull_over_path_ = value;
+    pull_over_path_ = path;
+    if (path->type != PullOverPlannerType::NONE && path->type != PullOverPlannerType::FREESPACE) {
+      lane_parking_pull_over_path_ = path;
+    }
     last_path_update_time_ = clock_->now();
   }
 
+  template <typename... Args>
+  void set(Args... args)
+  {
+    std::lock_guard<std::recursive_mutex> lock(mutex_);
+    (..., set(args));
+  }
+  void set(const GoalCandidates & arg) { set_goal_candidates(arg); }
+  void set(const std::vector<PullOverPath> & arg) { set_pull_over_path_candidates(arg); }
+  void set(const std::shared_ptr<PullOverPath> & arg) { set_pull_over_path(arg); }
+  void set(const PullOverPath & arg) { set_pull_over_path(arg); }
+  void set(const GoalCandidate & arg) { set_modified_goal_pose(arg); }
+
   void clearPullOverPath()
   {
     const std::lock_guard<std::recursive_mutex> lock(mutex_);
@@ -221,6 +240,7 @@ class ThreadSafeData
   }
 
   DEFINE_GETTER_WITH_MUTEX(std::shared_ptr<PullOverPath>, pull_over_path)
+  DEFINE_GETTER_WITH_MUTEX(std::shared_ptr<PullOverPath>, lane_parking_pull_over_path)
   DEFINE_GETTER_WITH_MUTEX(std::optional<rclcpp::Time>, last_path_update_time)
   DEFINE_GETTER_WITH_MUTEX(std::optional<rclcpp::Time>, last_path_idx_increment_time)
 
@@ -231,6 +251,7 @@ class ThreadSafeData
 
 private:
   std::shared_ptr<PullOverPath> pull_over_path_{nullptr};
+  std::shared_ptr<PullOverPath> lane_parking_pull_over_path_{nullptr};
   std::vector<PullOverPath> pull_over_path_candidates_;
   GoalCandidates goal_candidates_{};
   std::optional<GoalCandidate> modified_goal_pose_;
@@ -375,6 +396,7 @@ class GoalPlannerModule : public SceneModuleInterface
   void generateGoalCandidates();
 
   // stop or decelerate
+  void deceleratePath(PullOverPath & pull_over_path) const;
   void decelerateForTurnSignal(const Pose & stop_pose, PathWithLaneId & path) const;
   void decelerateBeforeSearchStart(
     const Pose & search_start_offset_pose, PathWithLaneId & path) const;
@@ -409,13 +431,15 @@ class GoalPlannerModule : public SceneModuleInterface
 
   // freespace parking
   bool planFreespacePath();
-  void returnToLaneParking();
+  bool canReturnToLaneParking();
 
   // plan pull over path
   BehaviorModuleOutput planRunning();
   BehaviorModuleOutput planPullOver();
   BehaviorModuleOutput planPullOverAsCandidate();
-  void selectSafePullOverPath();
+  std::optional<std::pair<PullOverPath, GoalCandidate>> selectPullOverPath(
+    const std::vector<PullOverPath> & pull_over_path_candidates,
+    const GoalCandidates & goal_candidates) const;
   std::vector<PullOverPath> sortPullOverPathCandidatesByGoalPriority(
     const std::vector<PullOverPath> & pull_over_path_candidates,
     const GoalCandidates & goal_candidates) const;

planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp

@@ -54,7 +54,7 @@ namespace behavior_path_planner
 GoalPlannerModule::GoalPlannerModule(
   const std::string & name, rclcpp::Node & node,
   const std::shared_ptr<GoalPlannerParameters> & parameters,
-  const std::unordered_map<std::string, std::shared_ptr<RTCInterface> > & rtc_interface_ptr_map)
+  const std::unordered_map<std::string, std::shared_ptr<RTCInterface>> & rtc_interface_ptr_map)
 : SceneModuleInterface{name, node, rtc_interface_ptr_map},
   parameters_{parameters},
   vehicle_info_{vehicle_info_util::VehicleInfoUtil(node).getVehicleInfo()},
@@ -515,32 +515,31 @@ bool GoalPlannerModule::planFreespacePath()
   return false;
 }
 
-void GoalPlannerModule::returnToLaneParking()
+bool GoalPlannerModule::canReturnToLaneParking()
 {
   // return only before starting free space parking
   if (!isStopped()) {
-    return;
+    return false;
   }
 
-  if (!status_.get_lane_parking_pull_over_path()) {
-    return;
+  if (!thread_safe_data_.get_lane_parking_pull_over_path()) {
+    return false;
   }
 
-  const PathWithLaneId path = status_.get_lane_parking_pull_over_path()->getFullPath();
+  const PathWithLaneId path = thread_safe_data_.get_lane_parking_pull_over_path()->getFullPath();
   if (checkCollision(path)) {
-    return;
+    return false;
   }
 
   const Point & current_point = planner_data_->self_odometry->pose.pose.position;
   constexpr double th_distance = 0.5;
   const bool is_close_to_path =
     std::abs(motion_utils::calcLateralOffset(path.points, current_point)) < th_distance;
   if (!is_close_to_path) {
-    return;
+    return false;
   }
 
-  thread_safe_data_.set_pull_over_path(status_.get_lane_parking_pull_over_path());
-  RCLCPP_INFO(getLogger(), "return to lane parking");
+  return true;
 }
 
 void GoalPlannerModule::generateGoalCandidates()
@@ -616,23 +615,10 @@ std::vector<PullOverPath> GoalPlannerModule::sortPullOverPathCandidatesByGoalPri
   return sorted_pull_over_path_candidates;
 }
 
-void GoalPlannerModule::selectSafePullOverPath()
+std::optional<std::pair<PullOverPath, GoalCandidate>> GoalPlannerModule::selectPullOverPath(
+  const std::vector<PullOverPath> & pull_over_path_candidates,
+  const GoalCandidates & goal_candidates) const
 {
-  // select safe lane pull over path from candidates
-  std::vector<PullOverPath> pull_over_path_candidates{};
-  GoalCandidates goal_candidates{};
-  {
-    const std::lock_guard<std::recursive_mutex> lock(mutex_);
-    goal_searcher_->setPlannerData(planner_data_);
-    goal_candidates = thread_safe_data_.get_goal_candidates();
-    goal_searcher_->update(goal_candidates);
-    thread_safe_data_.set_goal_candidates(goal_candidates);
-    thread_safe_data_.set_pull_over_path_candidates(sortPullOverPathCandidatesByGoalPriority(
-      thread_safe_data_.get_pull_over_path_candidates(), thread_safe_data_.get_goal_candidates()));
-    pull_over_path_candidates = thread_safe_data_.get_pull_over_path_candidates();
-    thread_safe_data_.clearPullOverPath();
-  }
-
   for (const auto & pull_over_path : pull_over_path_candidates) {
     // check if goal is safe
     const auto goal_candidate_it = std::find_if(
@@ -651,42 +637,10 @@ void GoalPlannerModule::selectSafePullOverPath()
       continue;
     }
 
-    // found safe pull over path
-    {
-      const std::lock_guard<std::recursive_mutex> lock(mutex_);
-      thread_safe_data_.set_pull_over_path(pull_over_path);
-      thread_safe_data_.set_modified_goal_pose(*goal_candidate_it);
-      status_.set_lane_parking_pull_over_path(thread_safe_data_.get_pull_over_path());
-    }
-    break;
+    return std::make_pair(pull_over_path, *goal_candidate_it);
   }
 
-  if (!thread_safe_data_.foundPullOverPath()) {
-    return;
-  }
-
-  // decelerate before the search area start
-  const auto decel_pose = calcLongitudinalOffsetPose(
-    thread_safe_data_.get_pull_over_path()->getFullPath().points,
-    status_.get_closest_goal_candidate_pose().position, -approximate_pull_over_distance_);
-  auto & first_path = thread_safe_data_.get_pull_over_path()->partial_paths.front();
-  if (decel_pose) {
-    decelerateBeforeSearchStart(*decel_pose, first_path);
-    return;
-  }
-
-  // if already passed the search start offset pose, set pull_over_velocity to first_path.
-  const auto min_decel_distance = calcFeasibleDecelDistance(
-    planner_data_, parameters_->maximum_deceleration, parameters_->maximum_jerk,
-    parameters_->pull_over_velocity);
-  for (auto & p : first_path.points) {
-    const double distance_from_ego = calcSignedArcLengthFromEgo(first_path, p.point.pose);
-    if (min_decel_distance && distance_from_ego < *min_decel_distance) {
-      continue;
-    }
-    p.point.longitudinal_velocity_mps = std::min(
-      p.point.longitudinal_velocity_mps, static_cast<float>(parameters_->pull_over_velocity));
-  }
+  return {};
 }
 
 std::vector<DrivableLanes> GoalPlannerModule::generateDrivableLanes() const
@@ -843,7 +797,7 @@ void GoalPlannerModule::updateSteeringFactor(
     pose, distance, SteeringFactor::GOAL_PLANNER, steering_factor_direction, type, "");
 }
 
-// NOTE: this function returns true once it returns true. Because selectSafePullOverPath() will
+// NOTE: this function returns true once it returns true. Because selectPullOverPath() will
 // not select new path.
 bool GoalPlannerModule::hasDecidedPath() const
 {
@@ -922,17 +876,42 @@ BehaviorModuleOutput GoalPlannerModule::planPullOver()
   if (!hasDecidedPath() && needPathUpdate(1.0 /*path_update_duration*/)) {
     // if the final path is not decided and enough time has passed since last path update,
     // select safe path from lane parking pull over path candidates
-    // and set it to thread_safe_data_.get_pull_over_path()
-    selectSafePullOverPath();
+    // and set it to thread_safe_data_
+
+    thread_safe_data_.clearPullOverPath();
+
+    // update goal candidates
+    goal_searcher_->setPlannerData(planner_data_);
+    auto goal_candidates = thread_safe_data_.get_goal_candidates();
+    goal_searcher_->update(goal_candidates);
+
+    // update pull over path candidates
+    const auto pull_over_path_candidates = sortPullOverPathCandidatesByGoalPriority(
+      thread_safe_data_.get_pull_over_path_candidates(), goal_candidates);
+
+    // select pull over path which is safe against static objects and get it's goal
+    const auto path_and_goal_opt = selectPullOverPath(pull_over_path_candidates, goal_candidates);
+
+    // update thread_safe_data_
+    if (path_and_goal_opt) {
+      auto [pull_over_path, modified_goal] = *path_and_goal_opt;
+      deceleratePath(pull_over_path);
+      thread_safe_data_.set(
+        goal_candidates, pull_over_path_candidates, pull_over_path, modified_goal);
+    } else {
+      thread_safe_data_.set(goal_candidates, pull_over_path_candidates);
+    }
   }
 
   // set output and status
   BehaviorModuleOutput output{};
   setOutput(output);
 
   // return to lane parking if it is possible
-  if (thread_safe_data_.getPullOverPlannerType() == PullOverPlannerType::FREESPACE) {
-    returnToLaneParking();
+  if (
+    thread_safe_data_.getPullOverPlannerType() == PullOverPlannerType::FREESPACE &&
+    canReturnToLaneParking()) {
+    thread_safe_data_.set_pull_over_path(thread_safe_data_.get_lane_parking_pull_over_path());
   }
 
   // For debug
@@ -1121,7 +1100,7 @@ PathWithLaneId GoalPlannerModule::generateStopPath() const
     return reference_path;
   }
 
-  // if already passed the decle pose, set pull_over_velocity to reference_path.
+  // if already passed the decel pose, set pull_over_velocity to reference_path.
   const auto min_decel_distance = calcFeasibleDecelDistance(
     planner_data_, parameters_->maximum_deceleration, parameters_->maximum_jerk,
     pull_over_velocity);
@@ -1439,6 +1418,32 @@ double GoalPlannerModule::calcSignedArcLengthFromEgo(
     path.points, current_pose.position, ego_idx, pose.position, target_idx);
 }
 
+void GoalPlannerModule::deceleratePath(PullOverPath & pull_over_path) const
+{
+  // decelerate before the search area start
+  const auto decel_pose = calcLongitudinalOffsetPose(
+    pull_over_path.getFullPath().points, status_.get_closest_goal_candidate_pose().position,
+    -approximate_pull_over_distance_);
+  auto & first_path = pull_over_path.partial_paths.front();
+  if (decel_pose) {
+    decelerateBeforeSearchStart(*decel_pose, first_path);
+    return;
+  }
+
+  // if already passed the search start offset pose, set pull_over_velocity to first_path.
+  const auto min_decel_distance = calcFeasibleDecelDistance(
+    planner_data_, parameters_->maximum_deceleration, parameters_->maximum_jerk,
+    parameters_->pull_over_velocity);
+  for (auto & p : first_path.points) {
+    const double distance_from_ego = calcSignedArcLengthFromEgo(first_path, p.point.pose);
+    if (min_decel_distance && distance_from_ego < *min_decel_distance) {
+      continue;
+    }
+    p.point.longitudinal_velocity_mps = std::min(
+      p.point.longitudinal_velocity_mps, static_cast<float>(parameters_->pull_over_velocity));
+  }
+}
+
 void GoalPlannerModule::decelerateForTurnSignal(const Pose & stop_pose, PathWithLaneId & path) const
 {
   const double time = planner_data_->parameters.turn_signal_search_time;