From 1319febe78efd129a4c4dbb1c02f955f56316c58 Mon Sep 17 00:00:00 2001 From: Mamoru Sobue Date: Mon, 22 May 2023 18:53:46 +0900 Subject: [PATCH] call setSafe() regardless of occlusion_stop_required Signed-off-by: Mamoru Sobue --- .../src/scene_intersection.cpp | 5 ++--- 1 file changed, 2 insertions(+), 3 deletions(-) diff --git a/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp b/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp index 529564847ffcb..e0a602b5f605a 100644 --- a/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp +++ b/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp @@ -413,10 +413,9 @@ bool IntersectionModule::modifyPathVelocity(PathWithLaneId * path, StopReason * if (occlusion_stop_required) { occlusion_first_stop_required_ = first_phase_stop_required; occlusion_safety_ = is_occlusion_cleared; - } else { - /* collision */ - setSafe(collision_state_machine_.getState() == StateMachine::State::GO); } + /* collision */ + setSafe(collision_state_machine_.getState() == StateMachine::State::GO); /* make decision */ const double baselink2front = planner_data_->vehicle_info_.max_longitudinal_offset_m;