refactor(start_planner): support new interface

refactor state transition logic in StartPlannerModule Signed-off-by: kyoichi-sugahara <[email protected]>
autowarefoundation · Nov 16, 2023 · 2336097 · 2336097
1 parent b5c23b1
commit 2336097
Show file tree

Hide file tree

Showing 2 changed files with 8 additions and 29 deletions.
diff --git a/...planner/include/behavior_path_planner/scene_module/start_planner/start_planner_module.hpp b/...planner/include/behavior_path_planner/scene_module/start_planner/start_planner_module.hpp
@@ -92,7 +92,6 @@ class StartPlannerModule : public SceneModuleInterface
   bool isExecutionRequested() const override;
   bool isExecutionReady() const override;
   // TODO(someone): remove this, and use base class function
-  [[deprecated]] void updateCurrentState() override;
   BehaviorModuleOutput plan() override;
   BehaviorModuleOutput planWaitingApproval() override;
   CandidateOutput planCandidate() const override;
@@ -123,11 +122,11 @@ class StartPlannerModule : public SceneModuleInterface
   bool isFreespacePlanning() const { return status_.planner_type == PlannerType::FREESPACE; }
 
 private:
-  bool canTransitSuccessState() override { return false; }
+  bool canTransitSuccessState() override;
 
   bool canTransitFailureState() override { return false; }
 
-  bool canTransitIdleToRunningState() override { return false; }
+  bool canTransitIdleToRunningState() override;
 
   void initializeSafetyCheckParameters();
 

diff --git a/planning/behavior_path_planner/src/scene_module/start_planner/start_planner_module.cpp b/planning/behavior_path_planner/src/scene_module/start_planner/start_planner_module.cpp
@@ -228,33 +228,14 @@ bool StartPlannerModule::isExecutionReady() const
   return true;
 }
 
-void StartPlannerModule::updateCurrentState()
+bool StartPlannerModule::canTransitSuccessState()
 {
-  RCLCPP_DEBUG(getLogger(), "START_PLANNER updateCurrentState");
-
-  const auto print = [this](const auto & from, const auto & to) {
-    RCLCPP_DEBUG(getLogger(), "[start_planner] Transit from %s to %s.", from.data(), to.data());
-  };
-
-  const auto & from = current_state_;
-  // current_state_ = updateState();
-
-  if (isActivated() && !isWaitingApproval()) {
-    current_state_ = ModuleStatus::RUNNING;
-  } else {
-    current_state_ = ModuleStatus::IDLE;
-  }
-
-  // TODO(someone): move to canTransitSuccessState
-  if (hasFinishedPullOut()) {
-    current_state_ = ModuleStatus::SUCCESS;
-  }
-  // TODO(someone): move to canTransitSuccessState
-  if (status_.backward_driving_complete) {
-    current_state_ = ModuleStatus::SUCCESS;  // for breaking loop
-  }
+  return hasFinishedPullOut();
+}
 
-  print(magic_enum::enum_name(from), magic_enum::enum_name(current_state_));
+bool StartPlannerModule::canTransitIdleToRunningState()
+{
+  return isActivated() && !isWaitingApproval();
 }
 
 BehaviorModuleOutput StartPlannerModule::plan()
@@ -740,7 +721,6 @@ void StartPlannerModule::updatePullOutStatus()
   if (isBackwardDrivingComplete()) {
     updateStatusAfterBackwardDriving();
     // should be moved to transition state
-    current_state_ = ModuleStatus::SUCCESS;  // for breaking loop
   } else {
     status_.backward_path = start_planner_utils::getBackwardPath(
       *route_handler, pull_out_lanes, current_pose, status_.pull_out_start_pose,