From da36ddbfecb6805c84b55e8f0d4a83c1ae57e559 Mon Sep 17 00:00:00 2001 From: kosuke55 Date: Sun, 21 Jan 2024 22:09:25 +0900 Subject: [PATCH] feat(goal_planner): add diciding status considering recognition stability time Signed-off-by: kosuke55 --- .../goal_planner_module.hpp | 13 +++++- .../src/goal_planner_module.cpp | 43 ++++++++++++++++--- 2 files changed, 47 insertions(+), 9 deletions(-) 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 56ca29543fcec..db9b58e70a8a7 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 @@ -230,6 +230,13 @@ struct LastApprovalData Pose pose{}; }; +enum class DecidingPathStatus { + NOT_DECIDED, + DECIDING, + DECIDED, +}; +using DecidingPathStatusWithStamp = std::pair; + struct PreviousPullOverData { struct SafetyStatus @@ -242,12 +249,13 @@ struct PreviousPullOverData { found_path = false; safety_status = SafetyStatus{}; - has_decided_path = false; + deciding_path_status = DecidingPathStatusWithStamp{}; } bool found_path{false}; SafetyStatus safety_status{}; - bool has_decided_path{false}; + // bool has_decided_path{false}; + DecidingPathStatusWithStamp deciding_path_status{}; }; // store stop_pose_ pointer with reason string @@ -439,6 +447,7 @@ class GoalPlannerModule : public SceneModuleInterface bool needPathUpdate(const double path_update_duration) const; bool isStuck(); bool hasDecidedPath() const; + DecidingPathStatus checkDecidingPathStatus() const; void decideVelocity(); bool foundPullOverPath() const; void updateStatus(const BehaviorModuleOutput & output); 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 83ac671b40f5d..923a60f012289 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 @@ -902,22 +902,44 @@ void GoalPlannerModule::updateSteeringFactor( } bool GoalPlannerModule::hasDecidedPath() const +{ + return checkDecidingPathStatus() == DecidingPathStatus::DECIDED; +} + +DecidingPathStatus GoalPlannerModule::checkDecidingPathStatus() const { // Once this function returns true, it will continue to return true thereafter - if (prev_data_.has_decided_path) { - return true; + if (prev_data_.deciding_path_status.first == DecidingPathStatus::DECIDED) { + return DecidingPathStatus::DECIDED; } // if path is not safe, not decided if (!thread_safe_data_.foundPullOverPath()) { - return false; + return DecidingPathStatus::NOT_DECIDED; } // If it is dangerous before approval, do not determine the path. // This eliminates a unsafe path to be approved if ( parameters_->safety_check_params.enable_safety_check && !isSafePath().first && !isActivated()) { - return false; + return DecidingPathStatus::NOT_DECIDED; + } + + const auto current_path = thread_safe_data_.get_pull_over_path()->getCurrentPath(); + if (prev_data_.deciding_path_status.first == DecidingPathStatus::DECIDING) { + if ( + parameters_->use_object_recognition && + checkObjectsCollision( + current_path, parameters_->object_recognition_collision_check_margin, + true /*update_debug_data*/)) { + return DecidingPathStatus::NOT_DECIDED; + } + + if ((clock_->now() - prev_data_.deciding_path_status.second).seconds() > 1.0) { + return DecidingPathStatus::DECIDED; + } + + return DecidingPathStatus::DECIDING; } // if ego is sufficiently close to the start of the nearest candidate path, the path is decided @@ -932,7 +954,11 @@ bool GoalPlannerModule::hasDecidedPath() const thread_safe_data_.get_pull_over_path()->getCurrentPath().points, current_pose.position, ego_segment_idx, thread_safe_data_.get_pull_over_path()->start_pose.position, start_pose_segment_idx); - return dist_to_parking_start_pose < parameters_->decide_path_distance; + if (dist_to_parking_start_pose > parameters_->decide_path_distance) { + return DecidingPathStatus::NOT_DECIDED; + } + + return DecidingPathStatus::DECIDING; } void GoalPlannerModule::decideVelocity() @@ -993,7 +1019,9 @@ BehaviorModuleOutput GoalPlannerModule::planPullOverAsOutput() return getPreviousModuleOutput(); } - if (!hasDecidedPath() && needPathUpdate(1.0 /*path_update_duration*/)) { + if ( + checkDecidingPathStatus() == DecidingPathStatus::NOT_DECIDED && + needPathUpdate(1.0 /*path_update_duration*/)) { // if the final path is not decided and enough time has passed since last path update, // select safe path from lane parking pull over path candidates // and set it to thread_safe_data_ @@ -1083,7 +1111,8 @@ void GoalPlannerModule::updatePreviousData() // this is used to determine whether to generate a new stop path or keep the current stop path. prev_data_.found_path = thread_safe_data_.foundPullOverPath(); - prev_data_.has_decided_path = hasDecidedPath(); + prev_data_.deciding_path_status.first = checkDecidingPathStatus(); + prev_data_.deciding_path_status.second = clock_->now(); // This is related to safety check, so if it is disabled, return here. if (!parameters_->safety_check_params.enable_safety_check) {