refactor(start_planner): support new interface (#5606)

* refactor(start_planner): support new interface refactor state transition logic in StartPlannerModule Signed-off-by: kyoichi-sugahara <[email protected]> --------- Signed-off-by: kyoichi-sugahara <[email protected]>
autowarefoundation · Nov 17, 2023 · 05156ad · 05156ad
1 parent db74641
commit 05156ad
Show file tree

Hide file tree

Showing 2 changed files with 9 additions and 32 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
@@ -91,8 +91,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 +121,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()
@@ -278,7 +259,7 @@ BehaviorModuleOutput StartPlannerModule::plan()
   PathWithLaneId path;
 
   // Check if backward motion is finished
-  if (status_.driving_forward) {
+  if (status_.driving_forward || status_.backward_driving_complete) {
     // Increment path index if the current path is finished
     if (hasFinishedCurrentPath()) {
       RCLCPP_INFO(getLogger(), "Increment path index");
@@ -739,8 +720,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,