refactor(goal_planner): refactoring plan flow and add post process (a…

…utowarefoundation#5554) * refactor(goal_planner): refactoring plan flow and add post process Signed-off-by: kosuke55 <[email protected]> do not decel when searching Signed-off-by: kosuke55 <[email protected]> rename planPullOver Signed-off-by: kosuke55 <[email protected]> * add plan flow Signed-off-by: kosuke55 <[email protected]> * add reason of early return Signed-off-by: kosuke55 <[email protected]> * typo Signed-off-by: kosuke55 <[email protected]> * fix path_candidate_ Signed-off-by: kosuke55 <[email protected]> * pub path candidate after approval Signed-off-by: kosuke55 <[email protected]> --------- Signed-off-by: kosuke55 <[email protected]>
tier4 · Nov 22, 2023 · 5fe26c6 · 5fe26c6
1 parent 7d71687
commit 5fe26c6
Show file tree

Hide file tree

Showing 2 changed files with 154 additions and 150 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
@@ -287,27 +287,47 @@ class GoalPlannerModule : public SceneModuleInterface
     }
   }
 
-  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 postProcess() override;
   void setParameters(const std::shared_ptr<GoalPlannerParameters> & parameters);
   void acceptVisitor(
     [[maybe_unused]] const std::shared_ptr<SceneModuleVisitor> & visitor) const override
   {
   }
+  CandidateOutput planCandidate() const override { return CandidateOutput{}; }
 
 private:
+  /*　
+   * state transitions and plan function used in each state
+   *
+   * +--------------------------+
+   * | RUNNING                  |
+   * | planPullOverAsCandidate()|
+   * +------------+-------------+
+   *              | hasDecidedPath()
+   *  2           v
+   * +--------------------------+
+   * | WAITING_APPROVAL         |
+   * | planPullOverAsCandidate()|
+   * +------------+-------------+
+   *              | isActivated()
+   *  3           v
+   * +--------------------------+
+   * | RUNNING                  |
+   * | planPullOverAsOutput()   |
+   * +--------------------------+
+   */
+
   // 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; }
 
   mutable StartGoalPlannerData goal_planner_data_;
@@ -411,8 +431,9 @@ class GoalPlannerModule : public SceneModuleInterface
   void returnToLaneParking();
 
   // plan pull over path
-  BehaviorModuleOutput planWithGoalModification();
-  BehaviorModuleOutput planWaitingApprovalWithGoalModification();
+  BehaviorModuleOutput planPullOver();
+  BehaviorModuleOutput planPullOverAsOutput();
+  BehaviorModuleOutput planPullOverAsCandidate();
   void selectSafePullOverPath();
   std::vector<PullOverPath> sortPullOverPathCandidatesByGoalPriority(
     const std::vector<PullOverPath> & pull_over_path_candidates,