From f0fe5ce854f7d42d0039a3e2949de690d85fb1dc Mon Sep 17 00:00:00 2001 From: Muhammad Zulfaqar Azmi Date: Tue, 9 Jan 2024 19:57:02 +0900 Subject: [PATCH] refactor(lane_change): remove waitApproval dependencies Signed-off-by: Muhammad Zulfaqar Azmi --- .../interface.hpp | 19 --------------- .../utils/base_class.hpp | 2 +- .../src/interface.cpp | 23 ++++--------------- .../src/scene.cpp | 1 + 4 files changed, 6 insertions(+), 39 deletions(-) diff --git a/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/interface.hpp b/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/interface.hpp index 9f4ba2b8eeeba..f4edca7a2e6d3 100644 --- a/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/interface.hpp +++ b/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/interface.hpp @@ -96,25 +96,6 @@ class LaneChangeInterface : public SceneModuleInterface TurnSignalInfo getCurrentTurnSignalInfo( const PathWithLaneId & path, const TurnSignalInfo & original_turn_signal_info); - // 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/behavior_path_lane_change_module/include/behavior_path_lane_change_module/utils/base_class.hpp b/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/utils/base_class.hpp index 451899fbf27e6..7b7fa9350b2e0 100644 --- a/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/utils/base_class.hpp +++ b/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/utils/base_class.hpp @@ -237,7 +237,7 @@ class LaneChangeBase LaneChangeStatus status_{}; PathShifter path_shifter_{}; - LaneChangeStates current_lane_change_state_{}; + LaneChangeStates current_lane_change_state_{LaneChangeStates::Normal}; std::shared_ptr lane_change_parameters_{}; std::shared_ptr abort_path_{}; diff --git a/planning/behavior_path_lane_change_module/src/interface.cpp b/planning/behavior_path_lane_change_module/src/interface.cpp index 63b0befced767..95c236089e2d5 100644 --- a/planning/behavior_path_lane_change_module/src/interface.cpp +++ b/planning/behavior_path_lane_change_module/src/interface.cpp @@ -49,9 +49,10 @@ LaneChangeInterface::LaneChangeInterface( void LaneChangeInterface::processOnEntry() { - waitApproval(); module_type_->setPreviousModulePaths( getPreviousModuleOutput().reference_path, getPreviousModuleOutput().path); + module_type_->setPreviousDrivableAreaInfo(getPreviousModuleOutput().drivable_area_info); + module_type_->setPreviousTurnSignalInfo(getPreviousModuleOutput().turn_signal_info); module_type_->updateLaneChangeStatus(); } @@ -80,6 +81,8 @@ void LaneChangeInterface::updateData() { module_type_->setPreviousModulePaths( getPreviousModuleOutput().reference_path, getPreviousModuleOutput().path); + module_type_->setPreviousDrivableAreaInfo(getPreviousModuleOutput().drivable_area_info); + module_type_->setPreviousTurnSignalInfo(getPreviousModuleOutput().turn_signal_info); module_type_->updateSpecialData(); module_type_->resetStopPose(); } @@ -98,8 +101,6 @@ BehaviorModuleOutput LaneChangeInterface::plan() return {}; } - module_type_->setPreviousDrivableAreaInfo(getPreviousModuleOutput().drivable_area_info); - module_type_->setPreviousTurnSignalInfo(getPreviousModuleOutput().turn_signal_info); auto output = module_type_->generateOutput(); path_reference_ = std::make_shared(output.reference_path); *prev_approved_path_ = getPreviousModuleOutput().path; @@ -112,7 +113,6 @@ BehaviorModuleOutput LaneChangeInterface::plan() } updateSteeringFactorPtr(output); - clearWaitingApproval(); return output; } @@ -129,8 +129,6 @@ BehaviorModuleOutput LaneChangeInterface::planWaitingApproval() out.turn_signal_info = getPreviousModuleOutput().turn_signal_info; out.drivable_area_info = getPreviousModuleOutput().drivable_area_info; - module_type_->setPreviousModulePaths( - getPreviousModuleOutput().reference_path, getPreviousModuleOutput().path); module_type_->updateLaneChangeStatus(); setObjectDebugVisualization(); @@ -227,11 +225,6 @@ bool LaneChangeInterface::canTransitFailureState() log_debug_throttled(__func__); - if (module_type_->isAbortState() && !module_type_->hasFinishedAbort()) { - log_debug_throttled("Abort process has on going."); - return false; - } - if (isWaitingApproval()) { log_debug_throttled("Can't transit to failure state. Module is WAITING_FOR_APPROVAL"); return false; @@ -298,14 +291,6 @@ bool LaneChangeInterface::canTransitIdleToWaitingApprovalState() log_debug_throttled(__func__); - if (!isActivated() || isWaitingApproval()) { - if (module_type_->specialRequiredCheck()) { - return true; - } - log_debug_throttled("Module is idling."); - return false; - } - log_debug_throttled("Can lane change safely. Executing lane change."); module_type_->toNormalState(); return true; diff --git a/planning/behavior_path_lane_change_module/src/scene.cpp b/planning/behavior_path_lane_change_module/src/scene.cpp index c7ddd8fe63636..274406017e212 100644 --- a/planning/behavior_path_lane_change_module/src/scene.cpp +++ b/planning/behavior_path_lane_change_module/src/scene.cpp @@ -420,6 +420,7 @@ void NormalLaneChange::resetParameters() is_abort_approval_requested_ = false; current_lane_change_state_ = LaneChangeStates::Normal; abort_path_ = nullptr; + status_ = {}; object_debug_.clear(); }