From a25bc2aa515f756be93933c0f4b1adaa5fd73d78 Mon Sep 17 00:00:00 2001 From: kyoichi-sugahara Date: Thu, 21 Sep 2023 22:30:05 +0900 Subject: [PATCH] update Signed-off-by: kyoichi-sugahara --- .../start_planner/start_planner_module.cpp | 15 ++++++++++++--- 1 file changed, 12 insertions(+), 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 index 4fd127e92fd6f..48c8d296203ce 100644 --- 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(*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(path); output.reference_path = getPreviousModuleOutput().reference_path;