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

This reverts commit 92f78a4.
autowarefoundation · Oct 26, 2023 · eb25293 · eb25293
1 parent 9b99898
commit eb25293
Show file tree

Hide file tree

Showing 2 changed files with 15 additions and 37 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
@@ -219,14 +219,11 @@ class GoalPlannerModule : public SceneModuleInterface
     }
   }
 
-  // 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;
+  CandidateOutput planCandidate() const override;
   BehaviorModuleOutput plan() override;
   BehaviorModuleOutput planWaitingApproval() override;
+  bool isExecutionRequested() const override;
+  bool isExecutionReady() const override;
   void processOnExit() override;
   void updateData() override;
   void setParameters(const std::shared_ptr<GoalPlannerParameters> & parameters);
@@ -235,15 +232,15 @@ class GoalPlannerModule : public SceneModuleInterface
   {
   }
 
-  // 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 false; }
+  bool canTransitIdleToRunningState() override { return true; }
 
   mutable StartGoalPlannerData goal_planner_data_;
 

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
@@ -227,18 +227,6 @@ void GoalPlannerModule::onFreespaceParkingTimer()
   }
 }
 
-BehaviorModuleOutput GoalPlannerModule::run()
-{
-  current_state_ = ModuleStatus::RUNNING;
-  updateOccupancyGrid();
-
-  if (!isActivated()) {
-    return planWaitingApproval();
-  }
-
-  return plan();
-}
-
 void GoalPlannerModule::updateData()
 {
   // Initialize Occupancy Grid Map
@@ -448,13 +436,6 @@ 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_);
@@ -912,6 +893,12 @@ 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()
 {
   // if pull over path candidates generation is not finished, use previous module output
@@ -936,7 +923,6 @@ BehaviorModuleOutput GoalPlannerModule::planWithGoalModification()
         status_.set_last_approved_pose(
           std::make_shared<Pose>(planner_data_->self_odometry->pose.pose));
       }
-      clearWaitingApproval();
       decideVelocity();
     }
     transitionToNextPathIfFinishingCurrentPath();
@@ -952,7 +938,7 @@ BehaviorModuleOutput GoalPlannerModule::planWithGoalModification()
   // set output and status
   BehaviorModuleOutput output{};
   setOutput(output);
-  path_candidate_ = std::make_shared<PathWithLaneId>(status_.get_pull_over_path()->getFullPath());
+  path_candidate_ = std::make_shared<PathWithLaneId>(planCandidate().path_candidate);
   path_reference_ = getPreviousModuleOutput().reference_path;
 
   // return to lane parking if it is possible
@@ -1003,16 +989,11 @@ BehaviorModuleOutput GoalPlannerModule::planWaitingApprovalWithGoalModification(
     return getPreviousModuleOutput();
   }
 
-  waitApproval();
-
   BehaviorModuleOutput out;
   out.modified_goal = plan().modified_goal;  // update status_
   out.path = std::make_shared<PathWithLaneId>(generateStopPath());
   out.reference_path = getPreviousModuleOutput().reference_path;
-  path_candidate_ =
-    status_.get_is_safe_static_objects()
-      ? std::make_shared<PathWithLaneId>(status_.get_pull_over_path()->getFullPath())
-      : out.path;
+  path_candidate_ = std::make_shared<PathWithLaneId>(planCandidate().path_candidate);
   path_reference_ = getPreviousModuleOutput().reference_path;
   const auto distance_to_path_change = calcDistanceToPathChange();