update

Signed-off-by: kyoichi-sugahara <[email protected]>
kyoichi-sugahara · Sep 21, 2023 · a25bc2a · a25bc2a
1 parent dc79053
commit a25bc2a
Showing 1 changed file with 12 additions and 3 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
@@ -194,7 +194,7 @@ ModuleStatus StartPlannerModule::updateState()
   if (parameters_->safety_check_params.enable_safety_check && status_.back_finished) {
     status_.is_safe_dynamic_objects = isSafePath();
   } else {
-    status_.is_safe_dynamic_objects = false;
+    status_.is_safe_dynamic_objects = true;
   }
 
   checkBackFinished();
@@ -238,6 +238,13 @@ BehaviorModuleOutput StartPlannerModule::plan()
       incrementPathIndex();
     }
 
+    std::cerr << "\n\n\n\n\n status_.is_safe_dynamic_objects: " << status_.is_safe_dynamic_objects
+              << "\n\n\n\n\n\n\n"
+              << std::endl;
+
+    std::cerr << "\n\n\n\n\n current_state_ is running: "
+              << (current_state_ == ModuleStatus::RUNNING) << "\n\n\n\n\n\n\n"
+              << std::endl;
     if (
       !status_.is_safe_dynamic_objects && (current_state_ == ModuleStatus::RUNNING) &&
       (status_.prev_is_safe_dynamic_objects || status_.prev_stop_path_after_approval == nullptr)) {
@@ -249,10 +256,12 @@ BehaviorModuleOutput StartPlannerModule::plan()
       status_.prev_stop_path_after_approval =
         stop_path ? std::make_shared<PathWithLaneId>(*stop_path) : nullptr;
       path = stop_path ? *status_.prev_stop_path_after_approval : current_path;
+    } else {
+      path = getCurrentPath();
     }
-    path = getCurrentPath();
+  } else {
+    path = status_.backward_path;
   }
-  path = status_.backward_path;
 
   output.path = std::make_shared<PathWithLaneId>(path);
   output.reference_path = getPreviousModuleOutput().reference_path;