From ba292be31fcb069081ae0cb56a265989a8e59969 Mon Sep 17 00:00:00 2001 From: Kyoichi Sugahara Date: Fri, 31 May 2024 18:37:39 +0900 Subject: [PATCH] refactor(start_planner): improve readability of end condition (#7181) * refactor(start_planner): clarify condition of transition to success Signed-off-by: kyoichi-sugahara * modify flowchart Signed-off-by: kyoichi-sugahara * add debug print Signed-off-by: kyoichi-sugahara --------- Signed-off-by: kyoichi-sugahara --- .../README.md | 76 +++++++++++++------ .../start_planner_module.hpp | 3 +- .../src/start_planner_module.cpp | 48 +++++++++--- 3 files changed, 92 insertions(+), 35 deletions(-) diff --git a/planning/behavior_path_start_planner_module/README.md b/planning/behavior_path_start_planner_module/README.md index ff0550e4d4867..dd3dddef9d863 100644 --- a/planning/behavior_path_start_planner_module/README.md +++ b/planning/behavior_path_start_planner_module/README.md @@ -91,36 +91,66 @@ The `StartPlannerModule` is designed to initiate its execution based on specific ### **End Conditions** -The `StartPlannerModule` terminates if the pull out / freespace maneuver has been completed. The `canTransitSuccessState` function assesses these conditions to decide if the module should terminate its execution. +The `StartPlannerModule` terminates when specific conditions are met, depending on the type of planner being used. The `canTransitSuccessState` function determines whether the module should transition to the success state based on the following criteria: -```plantuml -@startuml -start -:Start hasFinishedPullOut(); +#### When the Freespace Planner is active -if (status_.driving_forward && status_.found_pull_out_path) then (yes) +- If the end point of the freespace path is reached, the module transitions to the success state. - if (status_.planner_type == FREESPACE) then (yes) - :Calculate distance\nto pull_out_path.end_pose; - if (distance < th_arrived_distance) then (yes) - :return true;\n(Terminate module); - else (no) - :return false;\n(Continue running); - endif - else (no) - :Calculate arclength for\ncurrent_pose and pull_out_path.end_pose; - if (arclength_current - arclength_pull_out_end > offset) then (yes) - :return true;\n(Terminate module); - else (no) - :return false;\n(Continue running); - endif - endif +#### When any other type of planner is active + +The transition to the success state is determined by the following conditions: + +- If a reverse path is being generated or the search for a pull out path fails: + - The module does not transition to the success state. +- If the end point of the pull out path's shift section is reached: + - The module transitions to the success state. + +The flowchart below illustrates the decision-making process in the `canTransitSuccessState` function: +```plantuml +@startuml +@startuml +skinparam ActivityBackgroundColor #white +skinparam ActivityBorderColor #black +skinparam ActivityBorderThickness 1 +skinparam ActivityArrowColor #black +skinparam ActivityArrowThickness 1 +skinparam ActivityStartColor #black +skinparam ActivityEndColor #black +skinparam ActivityDiamondBackgroundColor #white +skinparam ActivityDiamondBorderColor #black +skinparam ActivityDiamondFontColor #black +partition canTransitSuccessState() { +start +if (planner type is FREESPACE?) then (yes) +if (Has reached freespace end?) then (yes) +#FF006C:true; +stop else (no) - :return false;\n(Continue running); +:false; +stop endif - +else (no) +if (driving is forward?) then (yes) +if (pull out path is found?) then (yes) +if (Has reached pull out end?) then (yes) +#FF006C:true; stop +else (no) +:false; +stop +endif +else (no) +:false; +stop +endif +else (no) +:false; +stop +endif +endif +} @enduml ``` 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 591d8adb60819..ea5afb133d57f 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 @@ -306,7 +306,8 @@ class StartPlannerModule : public SceneModuleInterface const double velocity_threshold, const double object_check_backward_distance, const double object_check_forward_distance) const; bool needToPrepareBlinkerBeforeStartDrivingForward() const; - bool hasFinishedPullOut() const; + bool hasReachedFreespaceEnd() const; + bool hasReachedPullOutEnd() const; bool hasFinishedBackwardDriving() const; bool hasCollisionWithDynamicObjects() const; bool isStopped(); 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 280e1112ef7fb..d3afdb2aefae5 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 @@ -579,7 +579,35 @@ bool StartPlannerModule::isExecutionReady() const bool StartPlannerModule::canTransitSuccessState() { - return hasFinishedPullOut(); + // Freespace Planner: + // - Can transit to success if the goal position is reached. + // - Cannot transit to success if the goal position is not reached. + if (status_.planner_type == PlannerType::FREESPACE) { + if (hasReachedFreespaceEnd()) { + RCLCPP_DEBUG( + getLogger(), "Transit to success: Freespace planner reached the end point of the path."); + return true; + } + return false; + } + + // Other Planners: + // - Cannot transit to success if the vehicle is driving in reverse. + // - Cannot transit to success if a safe path cannot be found due to: + // - Insufficient margin against static objects. + // - No path found that stays within the lane. + // In such cases, a stop point needs to be embedded and keep running start_planner. + // - Can transit to success if the end point of the pullout path is reached. + if (!status_.driving_forward || !status_.found_pull_out_path) { + return false; + } + + if (hasReachedPullOutEnd()) { + RCLCPP_DEBUG(getLogger(), "Transit to success: Reached the end point of the pullout path."); + return true; + } + + return false; } BehaviorModuleOutput StartPlannerModule::plan() @@ -1180,17 +1208,16 @@ PredictedObjects StartPlannerModule::filterStopObjectsInPullOutLanes( return stop_objects_in_pull_out_lanes; } -bool StartPlannerModule::hasFinishedPullOut() const +bool StartPlannerModule::hasReachedFreespaceEnd() const { - if (!status_.driving_forward || !status_.found_pull_out_path) { - return false; - } + const auto & current_pose = planner_data_->self_odometry->pose.pose; + return tier4_autoware_utils::calcDistance2d(current_pose, status_.pull_out_path.end_pose) < + parameters_->th_arrived_distance; +} +bool StartPlannerModule::hasReachedPullOutEnd() const +{ const auto current_pose = planner_data_->self_odometry->pose.pose; - if (status_.planner_type == PlannerType::FREESPACE) { - return tier4_autoware_utils::calcDistance2d(current_pose, status_.pull_out_path.end_pose) < - parameters_->th_arrived_distance; - } // check that ego has passed pull out end point const double backward_path_length = @@ -1205,9 +1232,8 @@ bool StartPlannerModule::hasFinishedPullOut() const // offset to not finish the module before engage constexpr double offset = 0.1; - const bool has_finished = arclength_current.length - arclength_pull_out_end.length > offset; - return has_finished; + return arclength_current.length - arclength_pull_out_end.length > offset; } bool StartPlannerModule::needToPrepareBlinkerBeforeStartDrivingForward() const