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 5ad4660dcf39c..f08eab4da4bd8 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; @@ -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 parameters_; 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 888594e0c41e5..6de31535e7174 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( @@ -153,7 +146,7 @@ BehaviorModuleOutput LaneChangeInterface::planWaitingApproval() if (!module_type_->isValidPath()) { updateRTCStatus( - std::numeric_limits::lowest(), std::numeric_limits::lowest(), true, + std::numeric_limits::lowest(), std::numeric_limits::lowest(), false, State::FAILED); path_candidate_ = std::make_shared(); 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");