fix(lane_change): fix state transition in lane change module

Signed-off-by: Fumiya Watanabe <rej55.g@gmail.com>
autowarefoundation · Jun 11, 2024 · 30cd2bd · 30cd2bd
1 parent adde8e1
commit 30cd2bd
Show file tree

Hide file tree

Showing 2 changed files with 11 additions and 15 deletions.
diff --git a/...r_path_lane_change_module/include/autoware_behavior_path_lane_change_module/interface.hpp b/...r_path_lane_change_module/include/autoware_behavior_path_lane_change_module/interface.hpp
@@ -59,8 +59,6 @@ class LaneChangeInterface : public SceneModuleInterface
   LaneChangeInterface & operator=(LaneChangeInterface &&) = delete;
   ~LaneChangeInterface() override = default;
 
-  void processOnEntry() override;
-
   void processOnExit() override;
 
   bool isExecutionRequested() const override;

diff --git a/planning/autoware_behavior_path_lane_change_module/src/interface.cpp b/planning/autoware_behavior_path_lane_change_module/src/interface.cpp
@@ -47,11 +47,6 @@ LaneChangeInterface::LaneChangeInterface(
   logger_ = utils::lane_change::getLogger(module_type_->getModuleTypeStr());
 }
 
-void LaneChangeInterface::processOnEntry()
-{
-  waitApproval();
-}
-
 void LaneChangeInterface::processOnExit()
 {
   module_type_->resetParameters();
@@ -79,7 +74,7 @@ void LaneChangeInterface::updateData()
   module_type_->setPreviousModuleOutput(getPreviousModuleOutput());
   module_type_->updateSpecialData();
 
-  if (isWaitingApproval()) {
+  if (isWaitingApproval() || module_type_->isAbortState()) {
     module_type_->updateLaneChangeStatus();
   }
 
@@ -116,14 +111,12 @@ BehaviorModuleOutput LaneChangeInterface::plan()
 
   updateSteeringFactorPtr(output);
   if (module_type_->isAbortState()) {
-    waitApproval();
     const auto candidate = planCandidate();
     path_candidate_ = std::make_shared<PathWithLaneId>(candidate.path_candidate);
     updateRTCStatus(
       std::numeric_limits<double>::lowest(), std::numeric_limits<double>::lowest(), true,
       State::ABORTING);
   } else {
-    clearWaitingApproval();
     const auto path =
       assignToCandidate(module_type_->getLaneChangePath(), module_type_->getEgoPosition());
     updateRTCStatus(
@@ -209,11 +202,6 @@ bool LaneChangeInterface::canTransitSuccessState()
     }
   }
 
-  if (module_type_->isAbortState() && module_type_->hasFinishedAbort()) {
-    log_debug_throttled("Abort process has completed.");
-    return true;
-  }
-
   if (module_type_->hasFinishedLaneChange()) {
     module_type_->resetParameters();
     log_debug_throttled("Lane change process has completed.");
@@ -243,6 +231,16 @@ bool LaneChangeInterface::canTransitFailureState()
     return false;
   }
 
+  if (!module_type_->isValidPath()) {
+    log_debug_throttled("Transit to failure state due not to find valid path");
+    return true;
+  }
+
+  if (module_type_->isAbortState() && module_type_->hasFinishedAbort()) {
+    log_debug_throttled("Abort process has completed.");
+    return true;
+  }
+
   if (module_type_->isCancelEnabled() && module_type_->isEgoOnPreparePhase()) {
     if (module_type_->isStoppedAtRedTrafficLight()) {
       log_debug_throttled("Stopping at traffic light while in prepare phase. Cancel lane change");