From 5a32e6870d49c971958d59614e78f0ead7ae11da Mon Sep 17 00:00:00 2001 From: kyoichi-sugahara Date: Thu, 30 May 2024 19:59:18 +0900 Subject: [PATCH 1/5] refactor(start_planner): clarify condition of transition to success Signed-off-by: kyoichi-sugahara --- .../start_planner_module.hpp | 3 +- .../src/start_planner_module.cpp | 38 +++++++++++++------ 2 files changed, 29 insertions(+), 12 deletions(-) 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..8dc90ba3bfac8 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,25 @@ 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) { + return hasReachedFreespaceEnd(); + } + + // 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; + } + + return hasReachedPullOutEnd(); } BehaviorModuleOutput StartPlannerModule::plan() @@ -1180,17 +1198,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 +1222,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 From a37034efe47d457891f9fa7bb58f24b3b3e49fa5 Mon Sep 17 00:00:00 2001 From: kyoichi-sugahara Date: Fri, 31 May 2024 01:52:55 +0900 Subject: [PATCH 2/5] modify flowchart Signed-off-by: kyoichi-sugahara --- .../README.md | 76 +++++++++++++------ 1 file changed, 53 insertions(+), 23 deletions(-) diff --git a/planning/behavior_path_start_planner_module/README.md b/planning/behavior_path_start_planner_module/README.md index ff0550e4d4867..6729162dfe422 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 ``` From 8fd8bd5e1dfe0f988575ea4c0f12c982784a8a63 Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Thu, 30 May 2024 17:06:46 +0000 Subject: [PATCH 3/5] style(pre-commit): autofix --- planning/behavior_path_start_planner_module/README.md | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/planning/behavior_path_start_planner_module/README.md b/planning/behavior_path_start_planner_module/README.md index 6729162dfe422..dd3dddef9d863 100644 --- a/planning/behavior_path_start_planner_module/README.md +++ b/planning/behavior_path_start_planner_module/README.md @@ -93,11 +93,11 @@ The `StartPlannerModule` is designed to initiate its execution based on specific 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: -#### When the Freespace Planner is active: +#### When the Freespace Planner is active - If the end point of the freespace path is reached, the module transitions to the success state. -#### When any other type of planner is active: +#### When any other type of planner is active The transition to the success state is determined by the following conditions: From 1dfe8b96a35883bf665e55130ea1d9238b7445b3 Mon Sep 17 00:00:00 2001 From: kyoichi-sugahara Date: Fri, 31 May 2024 11:43:21 +0900 Subject: [PATCH 4/5] add debug print Signed-off-by: kyoichi-sugahara --- .../src/start_planner_module.cpp | 14 ++++++++++++-- 1 file changed, 12 insertions(+), 2 deletions(-) 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 8dc90ba3bfac8..3e2f36dda4c65 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 @@ -583,7 +583,12 @@ bool StartPlannerModule::canTransitSuccessState() // - 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) { - return hasReachedFreespaceEnd(); + if (hasReachedFreespaceEnd()) { + RCLCPP_DEBUG( + getLogger(), "Transit to success: Freespace planner reached the end point of the path."); + return true; + } + return false; } // Other Planners: @@ -597,7 +602,12 @@ bool StartPlannerModule::canTransitSuccessState() return false; } - return hasReachedPullOutEnd(); + if (hasReachedPullOutEnd()) { + RCLCPP_DEBUG(getLogger(), "Transit to success: Reached the end point of the pullout path."); + return true; + } + + return false; } BehaviorModuleOutput StartPlannerModule::plan() From 97d117245f180d0538034a9bbf03cf323a26b51b Mon Sep 17 00:00:00 2001 From: kyoichi-sugahara Date: Fri, 31 May 2024 15:27:24 +0900 Subject: [PATCH 5/5] using reference Signed-off-by: kyoichi-sugahara --- .../src/start_planner_module.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) 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 3e2f36dda4c65..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 @@ -1210,7 +1210,7 @@ PredictedObjects StartPlannerModule::filterStopObjectsInPullOutLanes( bool StartPlannerModule::hasReachedFreespaceEnd() const { - const auto current_pose = planner_data_->self_odometry->pose.pose; + 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; }