From 80a271f0fcc422f954ff9ee57e526a79243d8b47 Mon Sep 17 00:00:00 2001 From: Mamoru Sobue Date: Tue, 6 Feb 2024 14:21:58 +0900 Subject: [PATCH] temp Signed-off-by: Mamoru Sobue --- .../src/scene_intersection_occlusion.cpp | 18 +++++++++++++----- 1 file changed, 13 insertions(+), 5 deletions(-) diff --git a/planning/behavior_velocity_intersection_module/src/scene_intersection_occlusion.cpp b/planning/behavior_velocity_intersection_module/src/scene_intersection_occlusion.cpp index b9fedaceb1fed..fa6a0d686a42b 100644 --- a/planning/behavior_velocity_intersection_module/src/scene_intersection_occlusion.cpp +++ b/planning/behavior_velocity_intersection_module/src/scene_intersection_occlusion.cpp @@ -58,11 +58,19 @@ IntersectionModule::getOcclusionStatus( (traffic_prioritized_level == TrafficPrioritizedLevel::PARTIALLY_PRIORITIZED) || (traffic_prioritized_level == TrafficPrioritizedLevel::FULLY_PRIORITIZED) || no_tl_info_ever; // check occlusion on detection lane - auto occlusion_status = - (planner_param_.occlusion.enable && !occlusion_attention_lanelets.empty() && - !is_amber_or_red_or_no_tl_info_ever) - ? detectOcclusion(interpolated_path_info) - : OcclusionType::NOT_OCCLUDED; + auto occlusion_status = [&]() { + if (!planner_param_.occlusion.enable || occlusion_attention_lanelets.empty()) { + return OcclusionType::NOT_OCCLUDED; + } + if (!has_traffic_light_) { + return detectOcclusion(interpolated_path_info); + } + // has_traffic_light_ + if (is_amber_or_red_or_no_tl_info_ever) { + return OcclusionType::NOT_OCCLUDED; + } + return detectOcclusion(interpolated_path_info); + }(); occlusion_stop_state_machine_.setStateWithMarginTime( occlusion_status == OcclusionType::NOT_OCCLUDED ? StateMachine::State::GO : StateMachine::STOP, logger_.get_child("occlusion_stop"), *clock_);