From 3753b5612f18dc2b4c0485a53c938d81d7a46288 Mon Sep 17 00:00:00 2001 From: Zulfaqar Azmi <93502286+zulfaqar-azmi-t4@users.noreply.github.com> Date: Fri, 2 Feb 2024 09:40:17 +0900 Subject: [PATCH] fix(bpp): transition from IDLE to WAITING APPROVAL (#6051) * fix(bpp): explicitly set the initial state Signed-off-by: Muhammad Zulfaqar Azmi * remove isWaitingApproval() from state transition Signed-off-by: Muhammad Zulfaqar Azmi * Update planning/behavior_path_planner_common/include/behavior_path_planner_common/interface/scene_module_interface.hpp Co-authored-by: Fumiya Watanabe * explicitly set the initial state Signed-off-by: Zulfaqar Azmi * remove setInitState in start_planner_module Signed-off-by: Zulfaqar Azmi --------- Signed-off-by: Muhammad Zulfaqar Azmi Signed-off-by: Zulfaqar Azmi Co-authored-by: Fumiya Watanabe --- .../behavior_path_avoidance_module/scene.hpp | 2 -- .../scene.hpp | 2 +- .../goal_planner_module.hpp | 1 - .../src/goal_planner_module.cpp | 3 +++ .../interface.hpp | 2 +- .../src/interface.cpp | 23 ------------------- .../interface/scene_module_interface.hpp | 15 +++++------- .../behavior_path_side_shift_module/scene.hpp | 2 -- .../start_planner_module.hpp | 2 -- 9 files changed, 11 insertions(+), 41 deletions(-) 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 0bb480bfa26b1..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 canTransitIdleToRunningState() 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 a5380b628387d..696df7f7971ea 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; } + ModuleStatus setInitState() const 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 71c309581f251..7350a7767fe01 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 canTransitIdleToRunningState() 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 3eb973dc9841e..c3404bf838a2c 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 @@ -1863,6 +1863,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 6a9a8c40c01d1..757ccd3a3116d 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 canTransitIdleToRunningState() override; + ModuleStatus setInitState() const override { return ModuleStatus::WAITING_APPROVAL; }; void updateDebugMarker() const; diff --git a/planning/behavior_path_lane_change_module/src/interface.cpp b/planning/behavior_path_lane_change_module/src/interface.cpp index c8d53d8756cf3..e3763720c2d8a 100644 --- a/planning/behavior_path_lane_change_module/src/interface.cpp +++ b/planning/behavior_path_lane_change_module/src/interface.cpp @@ -297,29 +297,6 @@ bool LaneChangeInterface::canTransitFailureState() return false; } -bool LaneChangeInterface::canTransitIdleToRunningState() -{ - updateDebugMarker(); - - 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; -} - void LaneChangeInterface::updateDebugMarker() const { if (!parameters_->publish_debug_marker) { 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..73f7448133ee7 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 (canTransitIdleToRunningState()) { - log_debug_throttled("transiting from IDLE to RUNNING"); - return ModuleStatus::RUNNING; - } - - 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 canTransitIdleToRunningState() = 0; + virtual ModuleStatus setInitState() const { 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 7d72afebfb359..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 canTransitIdleToRunningState() 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 9d3bd56701aef..e6f5d78a6eb71 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,8 +139,6 @@ class StartPlannerModule : public SceneModuleInterface bool canTransitFailureState() override { return false; } - bool canTransitIdleToRunningState() override { return true; } - /** * @brief init member variables. */