Merge pull request autowarefoundation#981 from tier4/fix/revert_goal_…

…planner

fix(goal_planner): revert "refactor(goal_planner): rebuild state transition (autowarefoundation#5371)" (autowarefoundation#5399)

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

@@ -217,28 +217,31 @@ class GoalPlannerModule : public SceneModuleInterface
     }
   }
 
-  CandidateOutput planCandidate() const override;
-  BehaviorModuleOutput plan() override;
-  BehaviorModuleOutput planWaitingApproval() override;
+  // TODO(someone): remove this, and use base class function
+  [[deprecated]] BehaviorModuleOutput run() override;
   bool isExecutionRequested() const override;
   bool isExecutionReady() const override;
+  // TODO(someone): remove this, and use base class function
+  [[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
   {
   }
 
+  // not used, but need to override
+  CandidateOutput planCandidate() const override { return CandidateOutput{}; };
+
 private:
-  // The start_planner activates when it receives a new route,
-  // so there is no need to terminate the goal planner.
-  // If terminating it, it may switch to lane following and could generate an inappropriate path.
   bool canTransitSuccessState() override { return false; }
 
   bool canTransitFailureState() override { return false; }
 
-  bool canTransitIdleToRunningState() override { return true; }
+  bool canTransitIdleToRunningState() override { return false; }
 
   mutable StartGoalPlannerData goal_planner_data_;
 

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

@@ -113,13 +113,6 @@ 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();
 }
 
@@ -226,18 +219,16 @@ void GoalPlannerModule::onFreespaceParkingTimer()
   }
 }
 
-void GoalPlannerModule::updateData()
+BehaviorModuleOutput GoalPlannerModule::run()
 {
-  // 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();
+  current_state_ = ModuleStatus::RUNNING;
+  updateOccupancyGrid();
+
+  if (!isActivated()) {
+    return planWaitingApproval();
   }
 
-  updateOccupancyGrid();
+  return plan();
 }
 
 void GoalPlannerModule::initializeOccupancyGridMap()
@@ -264,6 +255,22 @@ 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();
@@ -430,6 +437,13 @@ Pose GoalPlannerModule::calcRefinedGoal(const Pose & goal_pose) const
   return refined_goal_pose;
 }
 
+ModuleStatus GoalPlannerModule::updateState()
+{
+  // start_planner module will be run when setting new goal, so not need finishing pull_over module.
+  // Finishing it causes wrong lane_following path generation.
+  return current_state_;
+}
+
 bool GoalPlannerModule::planFreespacePath()
 {
   goal_searcher_->setPlannerData(planner_data_);
@@ -883,12 +897,6 @@ void GoalPlannerModule::decideVelocity()
   status_.set_has_decided_velocity(true);
 }
 
-CandidateOutput GoalPlannerModule::planCandidate() const
-{
-  return CandidateOutput(
-    status_.get_pull_over_path() ? status_.get_pull_over_path()->getFullPath() : PathWithLaneId());
-}
-
 BehaviorModuleOutput GoalPlannerModule::planWithGoalModification()
 {
   constexpr double path_update_duration = 1.0;
@@ -927,7 +935,7 @@ BehaviorModuleOutput GoalPlannerModule::planWithGoalModification()
   // set output and status
   BehaviorModuleOutput output{};
   setOutput(output);
-  path_candidate_ = std::make_shared<PathWithLaneId>(planCandidate().path_candidate);
+  path_candidate_ = std::make_shared<PathWithLaneId>(status_.get_pull_over_path()->getFullPath());
   path_reference_ = getPreviousModuleOutput().reference_path;
 
   // return to lane parking if it is possible
@@ -980,7 +988,10 @@ BehaviorModuleOutput GoalPlannerModule::planWaitingApprovalWithGoalModification(
   out.modified_goal = plan().modified_goal;  // update status_
   out.path = std::make_shared<PathWithLaneId>(generateStopPath());
   out.reference_path = getPreviousModuleOutput().reference_path;
-  path_candidate_ = std::make_shared<PathWithLaneId>(planCandidate().path_candidate);
+  path_candidate_ =
+    status_.get_is_safe_static_objects()
+      ? std::make_shared<PathWithLaneId>(status_.get_pull_over_path()->getFullPath())
+      : out.path;
   path_reference_ = getPreviousModuleOutput().reference_path;
   const auto distance_to_path_change = calcDistanceToPathChange();