diff --git a/planning/autoware_behavior_path_lane_change_module/include/autoware_behavior_path_lane_change_module/interface.hpp b/planning/autoware_behavior_path_lane_change_module/include/autoware_behavior_path_lane_change_module/interface.hpp index cff8609d77831..4f57c830d6627 100644 --- a/planning/autoware_behavior_path_lane_change_module/include/autoware_behavior_path_lane_change_module/interface.hpp +++ b/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; diff --git a/planning/autoware_behavior_path_lane_change_module/src/interface.cpp b/planning/autoware_behavior_path_lane_change_module/src/interface.cpp index 644368eaa970b..701f77d25865b 100644 --- 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(candidate.path_candidate); updateRTCStatus( std::numeric_limits::lowest(), std::numeric_limits::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");