From 188ceaffca16f179ba07d926fe94b93aff23c16e Mon Sep 17 00:00:00 2001 From: Muhammad Zulfaqar Azmi Date: Tue, 9 Jan 2024 19:37:55 +0900 Subject: [PATCH] fix(bpp): transition from IDLE to WAITING APPROVAL Signed-off-by: Muhammad Zulfaqar Azmi --- .../include/behavior_path_avoidance_module/scene.hpp | 2 +- .../behavior_path_dynamic_avoidance_module/scene.hpp | 2 +- .../goal_planner_module.hpp | 2 +- .../src/goal_planner_module.cpp | 3 +++ .../include/behavior_path_lane_change_module/interface.hpp | 2 +- planning/behavior_path_lane_change_module/src/interface.cpp | 2 +- .../interface/scene_module_interface.hpp | 6 +++--- .../include/behavior_path_side_shift_module/scene.hpp | 2 +- .../start_planner_module.hpp | 2 +- .../src/start_planner_module.cpp | 2 +- 10 files changed, 14 insertions(+), 11 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 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 493e7ec57f063..550fc1bd98724 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 @@ -1766,6 +1766,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..9f4ba2b8eeeba 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; + bool canTransitIdleToWaitingApprovalState() override; 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 22d85a2a1fc2a..63b0befced767 100644 --- a/planning/behavior_path_lane_change_module/src/interface.cpp +++ b/planning/behavior_path_lane_change_module/src/interface.cpp @@ -288,7 +288,7 @@ bool LaneChangeInterface::canTransitFailureState() return false; } -bool LaneChangeInterface::canTransitIdleToRunningState() +bool LaneChangeInterface::canTransitIdleToWaitingApprovalState() { setObjectDebugVisualization(); 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 a451a92c16ff1..f9f9f18a60f42 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 66e04d7e9bb7a..f72195f073178 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 @@ -338,7 +338,7 @@ bool StartPlannerModule::canTransitSuccessState() return hasFinishedPullOut(); } -bool StartPlannerModule::canTransitIdleToRunningState() +bool StartPlannerModule::canTransitIdleToWaitingApprovalState() { return isActivated() && !isWaitingApproval(); }