From b4d5d90af6805e5cf5dc0e5d7d901020c0f0d7b7 Mon Sep 17 00:00:00 2001
From: Kosuke Takeuchi <kosuke.tnp@gmail.com>
Date: Thu, 26 Oct 2023 20:05:32 +0900
Subject: [PATCH] refactor(goal_planner): use updateData (#5416)

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

upda
---
 .../goal_planner/goal_planner_module.hpp      |  2 +-
 .../goal_planner/goal_planner_module.cpp      | 45 +++++++++++--------
 2 files changed, 27 insertions(+), 20 deletions(-)

diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/goal_planner/goal_planner_module.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/goal_planner/goal_planner_module.hpp
index 343302a022a69..8b722d3316093 100644
--- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/goal_planner/goal_planner_module.hpp
+++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/goal_planner/goal_planner_module.hpp
@@ -225,8 +225,8 @@ class GoalPlannerModule : public SceneModuleInterface
   [[deprecated]] ModuleStatus updateState() override;
   BehaviorModuleOutput plan() override;
   BehaviorModuleOutput planWaitingApproval() override;
-  void processOnEntry() override;
   void processOnExit() override;
+  void updateData() override;
   void setParameters(const std::shared_ptr<GoalPlannerParameters> & parameters);
   void acceptVisitor(
     [[maybe_unused]] const std::shared_ptr<SceneModuleVisitor> & visitor) const override
diff --git a/planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp b/planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp
index 241a820175ffe..2db3560d6e4f3 100644
--- a/planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp
+++ b/planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp
@@ -114,6 +114,13 @@ GoalPlannerModule::GoalPlannerModule(
       freespace_parking_timer_cb_group_);
   }
 
+  // Initialize safety checker
+  if (parameters_->safety_check_params.enable_safety_check) {
+    initializeSafetyCheckParameters();
+    utils::start_goal_planner_common::initializeCollisionCheckDebugMap(
+      goal_planner_data_.collision_check);
+  }
+
   status_.reset();
 }
 
@@ -232,6 +239,25 @@ BehaviorModuleOutput GoalPlannerModule::run()
   return plan();
 }
 
+void GoalPlannerModule::updateData()
+{
+  // Initialize Occupancy Grid Map
+  // This operation requires waiting for `planner_data_`, hence it is executed here instead of in
+  // the constructor. Ideally, this operation should only need to be performed once.
+  if (
+    parameters_->use_occupancy_grid_for_goal_search ||
+    parameters_->use_occupancy_grid_for_path_collision_check) {
+    initializeOccupancyGridMap();
+  }
+
+  updateOccupancyGrid();
+
+  // set current road lanes, pull over lanes, and drivable lane
+  setLanes();
+
+  generateGoalCandidates();
+}
+
 void GoalPlannerModule::initializeOccupancyGridMap()
 {
   OccupancyGridMapParam occupancy_grid_map_param{};
@@ -256,22 +282,6 @@ void GoalPlannerModule::initializeSafetyCheckParameters()
     objects_filtering_params_, parameters_);
 }
 
-void GoalPlannerModule::processOnEntry()
-{
-  // Initialize occupancy grid map
-  if (
-    parameters_->use_occupancy_grid_for_goal_search ||
-    parameters_->use_occupancy_grid_for_path_collision_check) {
-    initializeOccupancyGridMap();
-  }
-  // Initialize safety checker
-  if (parameters_->safety_check_params.enable_safety_check) {
-    initializeSafetyCheckParameters();
-    utils::start_goal_planner_common::initializeCollisionCheckDebugMap(
-      goal_planner_data_.collision_check);
-  }
-}
-
 void GoalPlannerModule::processOnExit()
 {
   resetPathCandidate();
@@ -562,8 +572,6 @@ BehaviorModuleOutput GoalPlannerModule::plan()
   resetPathCandidate();
   resetPathReference();
 
-  generateGoalCandidates();
-
   path_reference_ = getPreviousModuleOutput().reference_path;
 
   if (goal_planner_utils::isAllowedGoalModification(planner_data_->route_handler)) {
@@ -984,7 +992,6 @@ BehaviorModuleOutput GoalPlannerModule::planWaitingApprovalWithGoalModification(
 {
   waitApproval();
 
-  updateOccupancyGrid();
   BehaviorModuleOutput out;
   out.modified_goal = plan().modified_goal;  // update status_
   out.path = std::make_shared<PathWithLaneId>(generateStopPath());