@@ -39,12 +39,28 @@ IntersectionModule::getOcclusionStatus(
39
39
const auto & intersection_lanelets = intersection_lanelets_.value ();
40
40
const auto & occlusion_attention_lanelets = intersection_lanelets.occlusion_attention ();
41
41
42
- const bool is_amber_or_red =
42
+ // ==========================================================================================
43
+ // for the convenience of Psim user, this module ignores occlusion if there has not been any
44
+ // information published for the associated traffic light, and only runs collision checking on
45
+ // that intersection lane.
46
+ //
47
+ // this is because Psim-users/scenario-files do not set traffic light information perfectly
48
+ // most of the times, and they just set bare minimum traffic information only for traffic lights
49
+ // they are interested in or want to test.
50
+ //
51
+ // no_tl_info_ever variable is defined for that purpose. if there has been any
52
+ // information published for the associated traffic light in the real world through perception/V2I
53
+ // or in the simulation, then it should be kept in last_tl_valid_observation_ and this variable
54
+ // becomes false
55
+ // ==========================================================================================
56
+ const bool no_tl_info_ever = !last_tl_valid_observation_.has_value ();
57
+ const bool is_amber_or_red_or_no_tl_info_ever =
43
58
(traffic_prioritized_level == TrafficPrioritizedLevel::PARTIALLY_PRIORITIZED) ||
44
- (traffic_prioritized_level == TrafficPrioritizedLevel::FULLY_PRIORITIZED);
59
+ (traffic_prioritized_level == TrafficPrioritizedLevel::FULLY_PRIORITIZED) || no_tl_info_ever ;
45
60
// check occlusion on detection lane
46
61
auto occlusion_status =
47
- (planner_param_.occlusion .enable && !occlusion_attention_lanelets.empty () && !is_amber_or_red)
62
+ (planner_param_.occlusion .enable && !occlusion_attention_lanelets.empty () &&
63
+ !is_amber_or_red_or_no_tl_info_ever)
48
64
? detectOcclusion (interpolated_path_info)
49
65
: OcclusionType::NOT_OCCLUDED;
50
66
occlusion_stop_state_machine_.setStateWithMarginTime (
0 commit comments