refactor(goal_planner): use updateData (#5416)

Signed-off-by: kosuke55 <[email protected]> upda
autowarefoundation · Oct 26, 2023 · b4d5d90 · b4d5d90
1 parent 64c8ac8
commit b4d5d90
Show file tree

Hide file tree

Showing 2 changed files with 27 additions and 20 deletions.
diff --git a/...h_planner/include/behavior_path_planner/scene_module/goal_planner/goal_planner_module.hpp b/...h_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
@@ -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());