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

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

Signed-off-by: Fumiya Watanabe <rej55.g@gmail.com>

* fix(lane_change): remove overridden run() function

Signed-off-by: Fumiya Watanabe <rej55.g@gmail.com>

* fix(lane_change): fix RTC safety status

Signed-off-by: Fumiya Watanabe <rej55.g@gmail.com>

---------

Signed-off-by: Fumiya Watanabe <rej55.g@gmail.com>

Author: rej55

planning/autoware_behavior_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;
@@ -93,25 +91,6 @@ class LaneChangeInterface : public SceneModuleInterface
 
   MarkerArray getModuleVirtualWall() override;
 
-  // TODO(someone): remove this, and use base class function
-  [[deprecated]] BehaviorModuleOutput run() override
-  {
-    updateData();
-
-    if (!isWaitingApproval()) {
-      return plan();
-    }
-
-    // module is waiting approval. Check it.
-    if (isActivated()) {
-      RCLCPP_DEBUG(getLogger(), "Was waiting approval, and now approved. Do plan().");
-      return plan();
-    } else {
-      RCLCPP_DEBUG(getLogger(), "keep waiting approval... Do planCandidate().");
-      return planWaitingApproval();
-    }
-  }
-
 protected:
   std::shared_ptr<LaneChangeParameters> parameters_;
 

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(
@@ -153,7 +146,7 @@ BehaviorModuleOutput LaneChangeInterface::planWaitingApproval()
 
   if (!module_type_->isValidPath()) {
     updateRTCStatus(
-      std::numeric_limits<double>::lowest(), std::numeric_limits<double>::lowest(), true,
+      std::numeric_limits<double>::lowest(), std::numeric_limits<double>::lowest(), false,
       State::FAILED);
     path_candidate_ = std::make_shared<PathWithLaneId>();
     return out;
@@ -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");