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 f1072b87755b0..4e741a2b3db2c 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 @@ -67,7 +67,10 @@ class LaneChangeInterface : public SceneModuleInterface bool isExecutionReady() const override; - bool isRootLaneletToBeUpdated() const override { return current_state_ == ModuleStatus::SUCCESS; } + bool isRootLaneletToBeUpdated() const override + { + return getCurrentStatus() == ModuleStatus::SUCCESS; + } void updateData() override; diff --git a/planning/behavior_path_lane_change_module/src/interface.cpp b/planning/behavior_path_lane_change_module/src/interface.cpp index e5aa8e06ece4c..850aecf9a3b54 100644 --- a/planning/behavior_path_lane_change_module/src/interface.cpp +++ b/planning/behavior_path_lane_change_module/src/interface.cpp @@ -64,7 +64,7 @@ void LaneChangeInterface::processOnExit() bool LaneChangeInterface::isExecutionRequested() const { - if (current_state_ == ModuleStatus::RUNNING) { + if (getCurrentStatus() == ModuleStatus::RUNNING) { return true; } @@ -359,7 +359,7 @@ MarkerArray LaneChangeInterface::getModuleVirtualWall() return marker; } - if (isWaitingApproval() || current_state_ != ModuleStatus::RUNNING) { + if (isWaitingApproval() || getCurrentStatus() != ModuleStatus::RUNNING) { return marker; } const auto & start_pose = module_type_->getLaneChangePath().info.lane_changing_start; diff --git a/planning/behavior_path_planner_common/include/behavior_path_planner_common/interface/scene_module_interface.hpp b/planning/behavior_path_planner_common/include/behavior_path_planner_common/interface/scene_module_interface.hpp index 82002ead9a2d1..1a8c8241abe1a 100644 --- a/planning/behavior_path_planner_common/include/behavior_path_planner_common/interface/scene_module_interface.hpp +++ b/planning/behavior_path_planner_common/include/behavior_path_planner_common/interface/scene_module_interface.hpp @@ -111,21 +111,6 @@ class SceneModuleInterface virtual void acceptVisitor(const std::shared_ptr & visitor) const = 0; - /** - * @brief Set the current_state_ based on updateState output. - */ - virtual void updateCurrentState() - { - const auto print = [this](const auto & from, const auto & to) { - RCLCPP_DEBUG( - getLogger(), "[%s] Transit from %s to %s.", name_.c_str(), from.data(), to.data()); - }; - - const auto & from = current_state_; - current_state_ = updateState(); - print(magic_enum::enum_name(from), magic_enum::enum_name(current_state_)); - } - /** * @brief Return true if the module has request for execution (not necessarily feasible) */ @@ -159,6 +144,21 @@ class SceneModuleInterface return isWaitingApproval() ? planWaitingApproval() : plan(); } + /** + * @brief Set the current_state_ based on updateState output. + */ + void updateCurrentState() + { + const auto print = [this](const auto & from, const auto & to) { + RCLCPP_DEBUG( + getLogger(), "[%s] Transit from %s to %s.", name_.c_str(), from.data(), to.data()); + }; + + const auto & from = current_state_; + current_state_ = updateState(); + print(magic_enum::enum_name(from), magic_enum::enum_name(current_state_)); + } + /** * @brief Called on the first time when the module goes into RUNNING. */ @@ -362,8 +362,84 @@ class SceneModuleInterface return existApprovedRequest(); } + /** + * @brief Return SUCCESS if plan is not needed or plan is successfully finished, + * FAILURE if plan has failed, RUNNING if plan is on going. + * These condition is to be implemented in each modules. + */ + ModuleStatus updateState() + { + auto log_debug_throttled = [&](std::string_view message) -> void { + RCLCPP_DEBUG(getLogger(), "%s", message.data()); + }; + if (current_state_ == ModuleStatus::IDLE) { + if (canTransitIdleToRunningState()) { + log_debug_throttled("transiting from IDLE to RUNNING"); + return ModuleStatus::RUNNING; + } + + log_debug_throttled("transiting from IDLE to IDLE"); + return ModuleStatus::IDLE; + } + + if (current_state_ == ModuleStatus::RUNNING) { + if (canTransitSuccessState()) { + log_debug_throttled("transiting from RUNNING to SUCCESS"); + return ModuleStatus::SUCCESS; + } + + if (canTransitFailureState()) { + log_debug_throttled("transiting from RUNNING to FAILURE"); + return ModuleStatus::FAILURE; + } + + if (canTransitWaitingApprovalState()) { + log_debug_throttled("transiting from RUNNING to WAITING_APPROVAL"); + return ModuleStatus::WAITING_APPROVAL; + } + + log_debug_throttled("transiting from RUNNING to RUNNING"); + return ModuleStatus::RUNNING; + } + + if (current_state_ == ModuleStatus::WAITING_APPROVAL) { + if (canTransitSuccessState()) { + log_debug_throttled("transiting from WAITING_APPROVAL to SUCCESS"); + return ModuleStatus::SUCCESS; + } + + if (canTransitFailureState()) { + log_debug_throttled("transiting from WAITING_APPROVAL to FAILURE"); + return ModuleStatus::FAILURE; + } + + if (canTransitWaitingApprovalToRunningState()) { + log_debug_throttled("transiting from WAITING_APPROVAL to RUNNING"); + return ModuleStatus::RUNNING; + } + + log_debug_throttled("transiting from WAITING_APPROVAL to WAITING APPROVAL"); + return ModuleStatus::WAITING_APPROVAL; + } + + if (current_state_ == ModuleStatus::SUCCESS) { + log_debug_throttled("already SUCCESS"); + return ModuleStatus::SUCCESS; + } + + if (current_state_ == ModuleStatus::FAILURE) { + log_debug_throttled("already FAILURE"); + return ModuleStatus::FAILURE; + } + + log_debug_throttled("already IDLE"); + return ModuleStatus::IDLE; + } + std::string name_; + ModuleStatus current_state_{ModuleStatus::IDLE}; + BehaviorModuleOutput previous_module_output_; StopReason stop_reason_; @@ -442,80 +518,6 @@ class SceneModuleInterface } } - /** - * @brief Return SUCCESS if plan is not needed or plan is successfully finished, - * FAILURE if plan has failed, RUNNING if plan is on going. - * These condition is to be implemented in each modules. - */ - virtual ModuleStatus updateState() - { - auto log_debug_throttled = [&](std::string_view message) -> void { - RCLCPP_WARN(getLogger(), "%s", message.data()); - }; - if (current_state_ == ModuleStatus::IDLE) { - if (canTransitIdleToRunningState()) { - log_debug_throttled("transiting from IDLE to RUNNING"); - return ModuleStatus::RUNNING; - } - - log_debug_throttled("transiting from IDLE to IDLE"); - return ModuleStatus::IDLE; - } - - if (current_state_ == ModuleStatus::RUNNING) { - if (canTransitSuccessState()) { - log_debug_throttled("transiting from RUNNING to SUCCESS"); - return ModuleStatus::SUCCESS; - } - - if (canTransitFailureState()) { - log_debug_throttled("transiting from RUNNING to FAILURE"); - return ModuleStatus::FAILURE; - } - - if (canTransitWaitingApprovalState()) { - log_debug_throttled("transiting from RUNNING to WAITING_APPROVAL"); - return ModuleStatus::WAITING_APPROVAL; - } - - log_debug_throttled("transiting from RUNNING to RUNNING"); - return ModuleStatus::RUNNING; - } - - if (current_state_ == ModuleStatus::WAITING_APPROVAL) { - if (canTransitSuccessState()) { - log_debug_throttled("transiting from WAITING_APPROVAL to SUCCESS"); - return ModuleStatus::SUCCESS; - } - - if (canTransitFailureState()) { - log_debug_throttled("transiting from WAITING_APPROVAL to FAILURE"); - return ModuleStatus::FAILURE; - } - - if (canTransitWaitingApprovalToRunningState()) { - log_debug_throttled("transiting from WAITING_APPROVAL to RUNNING"); - return ModuleStatus::RUNNING; - } - - log_debug_throttled("transiting from WAITING_APPROVAL to WAITING APPROVAL"); - return ModuleStatus::WAITING_APPROVAL; - } - - if (current_state_ == ModuleStatus::SUCCESS) { - log_debug_throttled("already SUCCESS"); - return ModuleStatus::SUCCESS; - } - - if (current_state_ == ModuleStatus::FAILURE) { - log_debug_throttled("already FAILURE"); - return ModuleStatus::FAILURE; - } - - log_debug_throttled("already IDLE"); - return ModuleStatus::IDLE; - } - /** * @brief Return true if the activation command is received from the RTC interface. * If no RTC interface is registered, return true. @@ -610,8 +612,6 @@ class SceneModuleInterface PlanResult path_candidate_; PlanResult path_reference_; - ModuleStatus current_state_{ModuleStatus::IDLE}; - std::unordered_map> rtc_interface_ptr_map_; std::unordered_map>