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 889d48c481b6f..e82741433bdf7 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,7 +78,7 @@ class AvoidanceModule : public SceneModuleInterface bool canTransitFailureState() override { return false; } - bool canTransitIdleToRunningState() override { return true; } + bool canTransitIdleToWaitingApprovalState() override { return true; } /** * @brief update RTC status for candidate shift line. 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 d2f9cd95066d9..b2769a7fd6bdd 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 canTransitIdleToRunningState() override { return false; } + bool canTransitIdleToWaitingApprovalState() override { return false; } 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 e90162c958be5..cf35b0e244ebd 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 @@ -330,7 +330,7 @@ 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 canTransitIdleToRunningState() override { return true; } + bool canTransitIdleToWaitingApprovalState() override { return true; } mutable StartGoalPlannerData goal_planner_data_; diff --git a/planning/behavior_path_goal_planner_module/src/goal_planner_module.cpp b/planning/behavior_path_goal_planner_module/src/goal_planner_module.cpp index 7bd97b3775bad..325c3ae4fca19 100644 --- a/planning/behavior_path_goal_planner_module/src/goal_planner_module.cpp +++ b/planning/behavior_path_goal_planner_module/src/goal_planner_module.cpp @@ -1765,6 +1765,9 @@ void GoalPlannerModule::updateSafetyCheckTargetObjectsData( std::pair GoalPlannerModule::isSafePath() const { + if (!thread_safe_data_.get_pull_over_path()) { + return {false, false}; + } const auto pull_over_path = thread_safe_data_.get_pull_over_path()->getCurrentPath(); const auto & current_pose = planner_data_->self_odometry->pose.pose; const double current_velocity = std::hypot( 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 4e741a2b3db2c..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_; @@ -126,7 +107,7 @@ class LaneChangeInterface : public SceneModuleInterface bool canTransitFailureState() override; - bool canTransitIdleToRunningState() override; + bool canTransitIdleToWaitingApprovalState() override; void setObjectDebugVisualization() const; 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 448f83ecaf15e..7aa84a351bc64 100644 --- a/planning/behavior_path_lane_change_module/src/interface.cpp +++ b/planning/behavior_path_lane_change_module/src/interface.cpp @@ -49,10 +49,12 @@ 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(); + setObjectDebugVisualization(); } void LaneChangeInterface::processOnExit() @@ -80,6 +82,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(); } @@ -87,6 +91,7 @@ void LaneChangeInterface::updateData() void LaneChangeInterface::postProcess() { post_process_safety_status_ = module_type_->isApprovedPathSafe(); + setObjectDebugVisualization(); } BehaviorModuleOutput LaneChangeInterface::plan() @@ -98,8 +103,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 +115,6 @@ BehaviorModuleOutput LaneChangeInterface::plan() } updateSteeringFactorPtr(output); - clearWaitingApproval(); return output; } @@ -129,8 +131,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(); @@ -226,7 +226,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; @@ -293,26 +292,8 @@ bool LaneChangeInterface::canTransitFailureState() return false; } -bool LaneChangeInterface::canTransitIdleToRunningState() +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() || 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(); } 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 1a8c8241abe1a..101e57e37842b 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,9 +373,9 @@ class SceneModuleInterface RCLCPP_DEBUG(getLogger(), "%s", message.data()); }; if (current_state_ == ModuleStatus::IDLE) { - if (canTransitIdleToRunningState()) { + if (canTransitIdleToWaitingApprovalState()) { log_debug_throttled("transiting from IDLE to RUNNING"); - return ModuleStatus::RUNNING; + return ModuleStatus::WAITING_APPROVAL; } log_debug_throttled("transiting from IDLE to IDLE"); @@ -462,7 +462,7 @@ class SceneModuleInterface /** * @brief State transition condition IDLE -> RUNNING */ - virtual bool canTransitIdleToRunningState() = 0; + virtual bool canTransitIdleToWaitingApprovalState() = 0; /** * @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 7d72afebfb359..762ff985bbf94 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,7 +75,7 @@ class SideShiftModule : public SceneModuleInterface bool canTransitFailureState() override { return false; } - bool canTransitIdleToRunningState() override { return true; } + bool canTransitIdleToWaitingApprovalState() override { return true; } void initVariables(); 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 eb1827ec0045d..f3363c82e990c 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 canTransitIdleToRunningState() override; + bool canTransitIdleToWaitingApprovalState() 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 6702e37f19077..339c7d40184eb 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 @@ -337,9 +337,9 @@ bool StartPlannerModule::canTransitSuccessState() return hasFinishedPullOut(); } -bool StartPlannerModule::canTransitIdleToRunningState() +bool StartPlannerModule::canTransitIdleToWaitingApprovalState() { - return isActivated() && !isWaitingApproval(); + return isActivated(); } BehaviorModuleOutput StartPlannerModule::plan()