diff --git a/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/scene.hpp b/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/scene.hpp index 4de8c4cb9b9a8..a74c3a69d7bce 100644 --- a/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/scene.hpp +++ b/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/scene.hpp @@ -78,8 +78,6 @@ class AvoidanceModule : public SceneModuleInterface bool canTransitFailureState() override { return false; } - bool canTransitIdleToWaitingApprovalState() override { return true; } - /** * @brief update RTC status for candidate shift line. * @param candidate path. diff --git a/planning/behavior_path_dynamic_avoidance_module/include/behavior_path_dynamic_avoidance_module/scene.hpp b/planning/behavior_path_dynamic_avoidance_module/include/behavior_path_dynamic_avoidance_module/scene.hpp index 7161f5e39fdcf..5cae89a04a539 100644 --- a/planning/behavior_path_dynamic_avoidance_module/include/behavior_path_dynamic_avoidance_module/scene.hpp +++ b/planning/behavior_path_dynamic_avoidance_module/include/behavior_path_dynamic_avoidance_module/scene.hpp @@ -345,7 +345,7 @@ class DynamicAvoidanceModule : public SceneModuleInterface bool canTransitFailureState() override { return false; } - bool canTransitIdleToWaitingApprovalState() override { return false; } + ModuleStatus setInitState() override { return ModuleStatus::IDLE; } bool isLabelTargetObstacle(const uint8_t label) const; void updateTargetObjects(); diff --git a/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/goal_planner_module.hpp b/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/goal_planner_module.hpp index 8ec82e610408e..47de3c802c949 100644 --- a/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/goal_planner_module.hpp +++ b/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/goal_planner_module.hpp @@ -356,7 +356,6 @@ class GoalPlannerModule : public SceneModuleInterface // If terminating it, it may switch to lane following and could generate an inappropriate path. bool canTransitSuccessState() override { return false; } bool canTransitFailureState() override { return false; } - bool canTransitIdleToWaitingApprovalState() override { return true; } mutable StartGoalPlannerData goal_planner_data_; 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..41144f81bf6a1 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 @@ -126,7 +126,7 @@ class LaneChangeInterface : public SceneModuleInterface bool canTransitFailureState() override; - bool canTransitIdleToWaitingApprovalState() override; + ModuleStatus setInitState() override { return ModuleStatus::WAITING_APPROVAL; }; void setObjectDebugVisualization() const; diff --git a/planning/behavior_path_lane_change_module/src/interface.cpp b/planning/behavior_path_lane_change_module/src/interface.cpp index fecf6bf2c4716..8c41dd987fe86 100644 --- a/planning/behavior_path_lane_change_module/src/interface.cpp +++ b/planning/behavior_path_lane_change_module/src/interface.cpp @@ -289,29 +289,6 @@ bool LaneChangeInterface::canTransitFailureState() return false; } -bool LaneChangeInterface::canTransitIdleToWaitingApprovalState() -{ - setObjectDebugVisualization(); - - auto log_debug_throttled = [&](std::string_view message) -> void { - RCLCPP_DEBUG(getLogger(), "%s", message.data()); - }; - - log_debug_throttled(__func__); - - if (!isActivated()) { - 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; -} - void LaneChangeInterface::setObjectDebugVisualization() const { debug_marker_.markers.clear(); 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 816c1ffbeffb0..e6e5b84a8ce34 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 @@ -373,13 +373,10 @@ class SceneModuleInterface RCLCPP_DEBUG(getLogger(), "%s", message.data()); }; if (current_state_ == ModuleStatus::IDLE) { - if (canTransitIdleToWaitingApprovalState()) { - log_debug_throttled("transiting from IDLE to WAITING_APPROVAL"); - return ModuleStatus::WAITING_APPROVAL; - } - - log_debug_throttled("transiting from IDLE to IDLE"); - return ModuleStatus::IDLE; + auto init_state = setInitState(); + RCLCPP_DEBUG( + getLogger(), "transiting from IDLE to %s", magic_enum::enum_name(init_state).data()); + return init_state; } if (current_state_ == ModuleStatus::RUNNING) { @@ -460,9 +457,9 @@ class SceneModuleInterface virtual bool canTransitFailureState() = 0; /** - * @brief State transition condition IDLE -> RUNNING + * @brief Explicitly set the initial state */ - virtual bool canTransitIdleToWaitingApprovalState() = 0; + virtual ModuleStatus setInitState() { return ModuleStatus::RUNNING; } /** * @brief Get candidate path. This information is used for external judgement. diff --git a/planning/behavior_path_side_shift_module/include/behavior_path_side_shift_module/scene.hpp b/planning/behavior_path_side_shift_module/include/behavior_path_side_shift_module/scene.hpp index 762ff985bbf94..1748f57a61bec 100644 --- a/planning/behavior_path_side_shift_module/include/behavior_path_side_shift_module/scene.hpp +++ b/planning/behavior_path_side_shift_module/include/behavior_path_side_shift_module/scene.hpp @@ -75,8 +75,6 @@ class SideShiftModule : public SceneModuleInterface bool canTransitFailureState() override { return false; } - bool canTransitIdleToWaitingApprovalState() override { return true; } - void initVariables(); // non-const methods diff --git a/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/start_planner_module.hpp b/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/start_planner_module.hpp index 5b5b28766c5a7..909b4c694653f 100644 --- a/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/start_planner_module.hpp +++ b/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/start_planner_module.hpp @@ -139,7 +139,7 @@ class StartPlannerModule : public SceneModuleInterface bool canTransitFailureState() override { return false; } - bool canTransitIdleToWaitingApprovalState() override; + ModuleStatus setInitState() override; /** * @brief init member variables. diff --git a/planning/behavior_path_start_planner_module/src/start_planner_module.cpp b/planning/behavior_path_start_planner_module/src/start_planner_module.cpp index e7c29b8fe9a26..6377c7cfe844d 100644 --- a/planning/behavior_path_start_planner_module/src/start_planner_module.cpp +++ b/planning/behavior_path_start_planner_module/src/start_planner_module.cpp @@ -21,6 +21,7 @@ #include "behavior_path_start_planner_module/util.hpp" #include "motion_utils/trajectory/trajectory.hpp" +#include #include #include #include @@ -347,9 +348,9 @@ bool StartPlannerModule::canTransitSuccessState() return hasFinishedPullOut(); } -bool StartPlannerModule::canTransitIdleToWaitingApprovalState() +ModuleStatus StartPlannerModule::setInitState() { - return isActivated(); + return isActivated() ? ModuleStatus::RUNNING : ModuleStatus::IDLE; } BehaviorModuleOutput StartPlannerModule::plan()