From ee834690f7d7fe29ef1e8dc0f1bf49dfee2153c9 Mon Sep 17 00:00:00 2001 From: Mamoru Sobue Date: Fri, 2 Feb 2024 19:17:45 +0900 Subject: [PATCH] feat(intersection_occlusion): treat traffic light color as RED initially Signed-off-by: Mamoru Sobue --- .../src/scene_intersection_occlusion.cpp | 22 ++++++++++++++++--- 1 file changed, 19 insertions(+), 3 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 b7c2d4c12878c..b9fedaceb1fed 100644 --- a/planning/behavior_velocity_intersection_module/src/scene_intersection_occlusion.cpp +++ b/planning/behavior_velocity_intersection_module/src/scene_intersection_occlusion.cpp @@ -39,12 +39,28 @@ IntersectionModule::getOcclusionStatus( const auto & intersection_lanelets = intersection_lanelets_.value(); const auto & occlusion_attention_lanelets = intersection_lanelets.occlusion_attention(); - const bool is_amber_or_red = + // ========================================================================================== + // for the convenience of Psim user, this module ignores occlusion if there has not been any + // information published for the associated traffic light, and only runs collision checking on + // that intersection lane. + // + // this is because Psim-users/scenario-files do not set traffic light information perfectly + // most of the times, and they just set bare minimum traffic information only for traffic lights + // they are interested in or want to test. + // + // no_tl_info_ever variable is defined for that purpose. if there has been any + // information published for the associated traffic light in the real world through perception/V2I + // or in the simulation, then it should be kept in last_tl_valid_observation_ and this variable + // becomes false + // ========================================================================================== + const bool no_tl_info_ever = !last_tl_valid_observation_.has_value(); + const bool is_amber_or_red_or_no_tl_info_ever = (traffic_prioritized_level == TrafficPrioritizedLevel::PARTIALLY_PRIORITIZED) || - (traffic_prioritized_level == TrafficPrioritizedLevel::FULLY_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) + (planner_param_.occlusion.enable && !occlusion_attention_lanelets.empty() && + !is_amber_or_red_or_no_tl_info_ever) ? detectOcclusion(interpolated_path_info) : OcclusionType::NOT_OCCLUDED; occlusion_stop_state_machine_.setStateWithMarginTime(