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

…on#4932) Signed-off-by: kosuke55 <[email protected]>
zulfaqar-azmi-t4 · Sep 10, 2023 · 477ce95 · 477ce95
1 parent 1a30fc3
commit 477ce95
Showing 1 changed file with 8 additions and 0 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
@@ -141,6 +141,10 @@ bool StartPlannerModule::isExecutionRequested() const
 
 bool StartPlannerModule::isExecutionReady() const
 {
+  if (!status_.is_safe_static_objects) {
+    return false;
+  }
+
   if (status_.pull_out_path.partial_paths.empty()) {
     return true;
   }
@@ -185,6 +189,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;
   }
 
@@ -202,6 +207,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;
   }
 
@@ -298,6 +304,7 @@ BehaviorModuleOutput StartPlannerModule::planWaitingApproval()
     clearWaitingApproval();
     const auto output = generateStopOutput();
     setDebugData();  // use status updated in generateStopOutput()
+    updateRTCStatus(0, 0);
     return output;
   }
 
@@ -308,6 +315,7 @@ BehaviorModuleOutput StartPlannerModule::planWaitingApproval()
     clearWaitingApproval();
     const auto output = generateStopOutput();
     setDebugData();  // use status updated in generateStopOutput()
+    updateRTCStatus(0, 0);
     return output;
   }