fix(goal_planner): generate goal candidates after execution (#3854)

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

Author: kosuke55

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

@@ -185,6 +185,7 @@ class GoalPlannerModule : public SceneModuleInterface
 
   // goal seach
   Pose calcRefinedGoal(const Pose & goal_pose) const;
+  void generateGoalCandidates();
 
   // stop or decelerate
   void decelerateForTurnSignal(const Pose & stop_pose, PathWithLaneId & path) const;

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

@@ -258,32 +258,10 @@ void GoalPlannerModule::initializeOccupancyGridMap()
 
 void GoalPlannerModule::processOnEntry()
 {
-  const auto & route_handler = planner_data_->route_handler;
-
   // Initialize occupancy grid map
   if (parameters_->use_occupancy_grid) {
     initializeOccupancyGridMap();
   }
-
-  // initialize when receiving new route
-  if (!last_received_time_ || *last_received_time_ != route_handler->getRouteHeader().stamp) {
-    // Initialize parallel parking planner status
-    resetStatus();
-
-    // calculate goal candidates
-    const Pose goal_pose = route_handler->getGoalPose();
-    refined_goal_pose_ = calcRefinedGoal(goal_pose);
-    if (allow_goal_modification_) {
-      goal_searcher_->setPlannerData(planner_data_);
-      goal_candidates_ = goal_searcher_->search(refined_goal_pose_);
-    } else {
-      GoalCandidate goal_candidate{};
-      goal_candidate.goal_pose = goal_pose;
-      goal_candidate.distance_from_original_goal = 0.0;
-      goal_candidates_.push_back(goal_candidate);
-    }
-  }
-  last_received_time_ = std::make_unique<rclcpp::Time>(route_handler->getRouteHeader().stamp);
 }
 
 void GoalPlannerModule::processOnExit()
@@ -486,8 +464,34 @@ void GoalPlannerModule::returnToLaneParking()
   RCLCPP_INFO(getLogger(), "return to lane parking");
 }
 
+void GoalPlannerModule::generateGoalCandidates()
+{
+  // initialize when receiving new route
+  const auto & route_handler = planner_data_->route_handler;
+  if (!last_received_time_ || *last_received_time_ != route_handler->getRouteHeader().stamp) {
+    // Initialize parallel parking planner status
+    resetStatus();
+
+    // calculate goal candidates
+    const Pose goal_pose = route_handler->getGoalPose();
+    refined_goal_pose_ = calcRefinedGoal(goal_pose);
+    if (allow_goal_modification_) {
+      goal_searcher_->setPlannerData(planner_data_);
+      goal_candidates_ = goal_searcher_->search(refined_goal_pose_);
+    } else {
+      GoalCandidate goal_candidate{};
+      goal_candidate.goal_pose = goal_pose;
+      goal_candidate.distance_from_original_goal = 0.0;
+      goal_candidates_.push_back(goal_candidate);
+    }
+  }
+  last_received_time_ = std::make_unique<rclcpp::Time>(route_handler->getRouteHeader().stamp);
+}
+
 BehaviorModuleOutput GoalPlannerModule::plan()
 {
+  generateGoalCandidates();
+
   if (allow_goal_modification_) {
     return planWithGoalModification();
   } else {