From 4eef2c74cd575061e7f562d7e4a75f73616abdf6 Mon Sep 17 00:00:00 2001
From: Mamoru Sobue <hilo.soblin@gmail.com>
Date: Wed, 12 Feb 2025 19:21:44 +0900
Subject: [PATCH] feat(goal_planner): ensure stop while path candidates are
 empty (#10101)

Signed-off-by: Mamoru Sobue <mamoru.sobue@tier4.jp>
---
 .../goal_searcher.hpp                         |  2 +-
 .../src/goal_planner_module.cpp               | 26 +++++++++++++------
 .../src/goal_searcher.cpp                     |  2 +-
 3 files changed, 20 insertions(+), 10 deletions(-)

diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/goal_searcher.hpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/goal_searcher.hpp
index 89b440c5bb795..8744fc1d12f43 100644
--- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/goal_searcher.hpp
+++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/goal_searcher.hpp
@@ -49,7 +49,7 @@ class GoalSearcher
 
   // todo(kosuke55): Functions for this specific use should not be in the interface,
   // so it is better to consider interface design when we implement other goal searchers.
-  GoalCandidate getClosetGoalCandidateAlongLanes(
+  std::optional<GoalCandidate> getClosestGoalCandidateAlongLanes(
     const GoalCandidates & goal_candidates,
     const std::shared_ptr<const PlannerData> & planner_data) const;
   bool isSafeGoalWithMarginScaleFactor(
diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_planner_module.cpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_planner_module.cpp
index 3c676707ad55b..411695c63f8dc 100644
--- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_planner_module.cpp
+++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_planner_module.cpp
@@ -1464,7 +1464,9 @@ BehaviorModuleOutput GoalPlannerModule::planPullOverAsCandidate(
 
   // if pull over path candidates generation is not finished, use previous module output
   if (context_data.lane_parking_response.pull_over_path_candidates.empty()) {
-    return getPreviousModuleOutput();
+    auto stop_path = getPreviousModuleOutput();
+    stop_path.path = generateStopPath(context_data, detail);
+    return stop_path;
   }
 
   BehaviorModuleOutput output{};
@@ -1735,11 +1737,13 @@ PathWithLaneId GoalPlannerModule::generateStopPath(
   // calculate search start offset pose from the closest goal candidate pose with
   // approximate_pull_over_distance_ ego vehicle decelerates to this position. or if no feasible
   // stop point is found, stop at this position.
-  const auto closest_goal_candidate =
-    goal_searcher.getClosetGoalCandidateAlongLanes(goal_candidates_, planner_data_);
+  const auto closest_searched_goal_candidate =
+    goal_searcher.getClosestGoalCandidateAlongLanes(goal_candidates_, planner_data_);
+  const auto closest_goal_candidate = closest_searched_goal_candidate
+                                        ? closest_searched_goal_candidate.value().goal_pose
+                                        : route_handler->getOriginalGoalPose();
   const auto decel_pose = calcLongitudinalOffsetPose(
-    extended_prev_path.points, closest_goal_candidate.goal_pose.position,
-    -approximate_pull_over_distance_);
+    extended_prev_path.points, closest_goal_candidate.position, -approximate_pull_over_distance_);
 
   // if not approved stop road lane.
   // stop point priority is
@@ -2066,12 +2070,18 @@ double GoalPlannerModule::calcSignedArcLengthFromEgo(
 void GoalPlannerModule::deceleratePath(PullOverPath & pull_over_path) const
 {
   universe_utils::ScopedTimeTrack st(__func__, *time_keeper_);
+  assert(goal_searcher_);
+  const auto & goal_searcher = goal_searcher_.value();
 
   // decelerate before the search area start
-  const auto closest_goal_candidate =
-    goal_searcher_->getClosetGoalCandidateAlongLanes(goal_candidates_, planner_data_);
+  const auto & route_handler = planner_data_->route_handler;
+  const auto closest_searched_goal_candidate =
+    goal_searcher.getClosestGoalCandidateAlongLanes(goal_candidates_, planner_data_);
+  const auto closest_goal_candidate = closest_searched_goal_candidate
+                                        ? closest_searched_goal_candidate.value().goal_pose
+                                        : route_handler->getOriginalGoalPose();
   const auto decel_pose = calcLongitudinalOffsetPose(
-    pull_over_path.full_path().points, closest_goal_candidate.goal_pose.position,
+    pull_over_path.full_path().points, closest_goal_candidate.position,
     -approximate_pull_over_distance_);
   auto & first_path = pull_over_path.partial_paths().front();
   if (decel_pose) {
diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_searcher.cpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_searcher.cpp
index 70bf3132362a4..cea726235238d 100644
--- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_searcher.cpp
+++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_searcher.cpp
@@ -550,7 +550,7 @@ void GoalSearcher::createAreaPolygons(
   }
 }
 
-GoalCandidate GoalSearcher::getClosetGoalCandidateAlongLanes(
+std::optional<GoalCandidate> GoalSearcher::getClosestGoalCandidateAlongLanes(
   const GoalCandidates & goal_candidates,
   const std::shared_ptr<const PlannerData> & planner_data) const
 {