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

* refactor(goal_planner): rebuild state transition Signed-off-by: kosuke55 <[email protected]> * fix occ Signed-off-by: kosuke55 <[email protected]> * fix canTransitIdleToRunningState Signed-off-by: kosuke55 <[email protected]> --------- Signed-off-by: kosuke55 <[email protected]>
autowarefoundation · Oct 23, 2023 · 3d2a7f6 · 3d2a7f6
1 parent 20c0d15
commit 3d2a7f6
Show file tree

Hide file tree

Showing 2 changed files with 32 additions and 46 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
@@ -240,31 +240,28 @@ 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;
-  void processOnEntry() override;
+  bool isExecutionRequested() const override;
+  bool isExecutionReady() const 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 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
@@ -124,6 +124,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();
 }
 
@@ -228,16 +235,18 @@ void GoalPlannerModule::onFreespaceParkingTimer()
   }
 }
 
-BehaviorModuleOutput GoalPlannerModule::run()
+void GoalPlannerModule::updateData()
 {
-  current_state_ = ModuleStatus::RUNNING;
-  updateOccupancyGrid();
-
-  if (!isActivated()) {
-    return planWaitingApproval();
+  // 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();
   }
 
-  return plan();
+  updateOccupancyGrid();
 }
 
 void GoalPlannerModule::initializeOccupancyGridMap()
@@ -264,22 +273,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();
@@ -446,13 +439,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_);
@@ -907,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()
 {
   constexpr double path_update_duration = 1.0;
@@ -945,7 +937,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
@@ -998,10 +990,7 @@ 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_ =
-    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();