fix(start_planner): prevent approval when stop path (autowarefoundati…

…on#4932) Signed-off-by: kosuke55 <[email protected]>
tier4 · Sep 12, 2023 · 4d5708b · 4d5708b
1 parent 12af290
commit 4d5708b
Showing 1 changed file with 9 additions and 2 deletions.
diff --git a/planning/behavior_path_planner/src/scene_module/start_planner/start_planner_module.cpp b/planning/behavior_path_planner/src/scene_module/start_planner/start_planner_module.cpp
@@ -176,7 +176,10 @@ bool StartPlannerModule::isExecutionRequested() const
 
 bool StartPlannerModule::isExecutionReady() const
 {
-  if (getCurrentPath().points.empty()) {
+  if (!status_.is_safe_static_objects) {
+    return false;
+  }
+  if (status_.pull_out_path.partial_paths.empty()) {
     return true;
   }
 
@@ -227,6 +230,7 @@ BehaviorModuleOutput StartPlannerModule::plan()
       getLogger(), *clock_, 5000, "Start plan for a backward goal is not supported now");
     const auto output = generateStopOutput();
     setDebugData();  // use status updated in generateStopOutput()
+    updateRTCStatus(0, 0);
     return output;
   }
 
@@ -244,6 +248,7 @@ BehaviorModuleOutput StartPlannerModule::plan()
       getLogger(), *clock_, 5000, "Not found safe pull out path, publish stop path");
     const auto output = generateStopOutput();
     setDebugData();  // use status updated in generateStopOutput()
+    updateRTCStatus(0, 0);
     return output;
   }
 
@@ -362,6 +367,7 @@ BehaviorModuleOutput StartPlannerModule::planWaitingApproval()
     clearWaitingApproval();
     const auto output = generateStopOutput();
     setDebugData();  // use status updated in generateStopOutput()
+    updateRTCStatus(0, 0);
     return output;
   }
 
@@ -372,6 +378,7 @@ BehaviorModuleOutput StartPlannerModule::planWaitingApproval()
     clearWaitingApproval();
     const auto output = generateStopOutput();
     setDebugData();  // use status updated in generateStopOutput()
+    updateRTCStatus(0, 0);
     return output;
   }
 
@@ -383,7 +390,7 @@ BehaviorModuleOutput StartPlannerModule::planWaitingApproval()
     planner_data_, backward_path_length, std::numeric_limits<double>::max(),
     /*forward_only_in_route*/ true);
 
-  auto stop_path = status_.need_backward_driving ? status_.backward_path: getCurrentPath();
+  auto stop_path = status_.need_backward_driving ? status_.backward_path : getCurrentPath();
   const auto drivable_lanes = generateDrivableLanes(stop_path);
   const auto & dp = planner_data_->drivable_area_expansion_parameters;
   const auto expanded_lanes = utils::expandLanelets(