refactor(start_planner): improve readability of end condition (autowa…

…refoundation#7181) * refactor(start_planner): clarify condition of transition to success Signed-off-by: kyoichi-sugahara <[email protected]> * modify flowchart Signed-off-by: kyoichi-sugahara <[email protected]> * add debug print Signed-off-by: kyoichi-sugahara <[email protected]> --------- Signed-off-by: kyoichi-sugahara <[email protected]>
Interplai · Jun 3, 2024 · dcd7234 · dcd7234
1 parent 0850eb4
commit dcd7234
Show file tree

Hide file tree

Showing 3 changed files with 92 additions and 35 deletions.
diff --git 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/..._start_planner_module/include/behavior_path_start_planner_module/start_planner_module.hpp b/..._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
@@ -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