feat(goal_planner): calculate stop pose from closest goal candidate (#…

…5410)

* feat(goal_planner): calculate stop pose from closest goal candidate

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

* style(pre-commit): autofix

* Update planning/behavior_path_planner/src/utils/goal_planner/goal_searcher.cpp

Co-authored-by: Kyoichi Sugahara <[email protected]>

* style(pre-commit): autofix

* Update planning/behavior_path_planner/src/utils/goal_planner/goal_searcher.cpp

* style(pre-commit): autofix

---------

Signed-off-by: kosuke55 <[email protected]>
Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com>
Co-authored-by: Kyoichi Sugahara <[email protected]>

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

@@ -142,6 +142,7 @@ class PullOverStatus
   DEFINE_SETTER_GETTER(std::optional<GoalCandidate>, modified_goal_pose)
   DEFINE_SETTER_GETTER(Pose, refined_goal_pose)
   DEFINE_SETTER_GETTER(GoalCandidates, goal_candidates)
+  DEFINE_SETTER_GETTER(Pose, closest_goal_candidate_pose)
   DEFINE_SETTER_GETTER(std::vector<PullOverPath>, pull_over_path_candidates)
   DEFINE_SETTER_GETTER(std::optional<Pose>, closest_start_pose)
 
@@ -174,6 +175,7 @@ class PullOverStatus
   std::optional<GoalCandidate> modified_goal_pose_;
   Pose refined_goal_pose_{};
   GoalCandidates goal_candidates_{};
+  Pose closest_goal_candidate_pose_{};
 
   //  pull over path
   std::vector<PullOverPath> pull_over_path_candidates_;

planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/goal_searcher.hpp

@@ -35,6 +35,8 @@ class GoalSearcher : public GoalSearcherBase
 
   GoalCandidates search() override;
   void update(GoalCandidates & goal_candidates) const override;
+  GoalCandidate getClosetGoalCandidateAlongLanes(
+    const GoalCandidates & goal_candidates) const override;
 
 private:
   void countObjectsToAvoid(

planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/goal_searcher_base.hpp

@@ -58,6 +58,8 @@ class GoalSearcherBase
   MultiPolygon2d getAreaPolygons() { return area_polygons_; }
   virtual GoalCandidates search() = 0;
   virtual void update([[maybe_unused]] GoalCandidates & goal_candidates) const { return; }
+  virtual GoalCandidate getClosetGoalCandidateAlongLanes(
+    const GoalCandidates & goal_candidates) const = 0;
 
 protected:
   GoalPlannerParameters parameters_{};

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

@@ -557,13 +557,19 @@ void GoalPlannerModule::generateGoalCandidates()
     goal_searcher_->setPlannerData(planner_data_);
     goal_searcher_->setReferenceGoal(status_.get_refined_goal_pose());
     status_.set_goal_candidates(goal_searcher_->search());
+    const auto current_lanes = utils::getExtendedCurrentLanes(
+      planner_data_, parameters_->backward_goal_search_length,
+      parameters_->forward_goal_search_length, false);
+    status_.set_closest_goal_candidate_pose(
+      goal_searcher_->getClosetGoalCandidateAlongLanes(status_.get_goal_candidates()).goal_pose);
   } else {
     GoalCandidate goal_candidate{};
     goal_candidate.goal_pose = goal_pose;
     goal_candidate.distance_from_original_goal = 0.0;
     GoalCandidates goal_candidates{};
     goal_candidates.push_back(goal_candidate);
     status_.set_goal_candidates(goal_candidates);
+    status_.set_closest_goal_candidate_pose(goal_pose);
   }
 }
 
@@ -1089,20 +1095,23 @@ PathWithLaneId GoalPlannerModule::generateStopPath()
   //     difference between the outer and inner sides)
   // 4. feasible stop
   const auto search_start_offset_pose = calcLongitudinalOffsetPose(
-    reference_path.points, status_.get_refined_goal_pose().position,
-    -parameters_->backward_goal_search_length - common_parameters.base_link2front -
-      approximate_pull_over_distance_);
+    reference_path.points, status_.get_closest_goal_candidate_pose().position,
+    -approximate_pull_over_distance_);
   if (
     !status_.get_is_safe_static_objects() && !status_.get_closest_start_pose() &&
     !search_start_offset_pose) {
     return generateFeasibleStopPath();
   }
 
-  const Pose stop_pose =
-    status_.get_is_safe_static_objects()
-      ? status_.get_pull_over_path()->start_pose
-      : (status_.get_closest_start_pose() ? status_.get_closest_start_pose().value()
-                                          : *search_start_offset_pose);
+  const Pose stop_pose = [&]() -> Pose {
+    if (status_.get_is_safe_static_objects()) {
+      return status_.get_pull_over_path()->start_pose;
+    }
+    if (status_.get_closest_start_pose()) {
+      return status_.get_closest_start_pose().value();
+    }
+    return *search_start_offset_pose;
+  }();
 
   // if stop pose is closer than min_stop_distance, stop as soon as possible
   const double ego_to_stop_distance = calcSignedArcLengthFromEgo(reference_path, stop_pose);

planning/behavior_path_planner/src/utils/goal_planner/goal_searcher.cpp

@@ -468,4 +468,30 @@ bool GoalSearcher::isInAreas(const LinearRing2d & footprint, const BasicPolygons
   return false;
 }
 
+GoalCandidate GoalSearcher::getClosetGoalCandidateAlongLanes(
+  const GoalCandidates & goal_candidates) const
+{
+  const auto current_lanes = utils::getExtendedCurrentLanes(
+    planner_data_, parameters_.backward_goal_search_length, parameters_.forward_goal_search_length,
+    /*forward_only_in_route*/ false);
+
+  // Define a lambda function to compute the arc length for a given goal candidate.
+  auto getGoalArcLength = [&current_lanes](const auto & candidate) {
+    return lanelet::utils::getArcCoordinates(current_lanes, candidate.goal_pose).length;
+  };
+
+  // Find the closest goal candidate by comparing the arc lengths of each candidate.
+  const auto closest_goal_candidate = std::min_element(
+    goal_candidates.begin(), goal_candidates.end(),
+    [&getGoalArcLength](const auto & a, const auto & b) {
+      return getGoalArcLength(a) < getGoalArcLength(b);
+    });
+
+  if (closest_goal_candidate == goal_candidates.end()) {
+    return {};  // return empty GoalCandidate in case no valid candidates are found.
+  }
+
+  return *closest_goal_candidate;
+}
+
 }  // namespace behavior_path_planner