1# pyright: reportPrivateUsage=false
2
3from __future__ import annotations
4
5import random
6from typing import Optional, TYPE_CHECKING, cast
7
8import carla
9from carla import TrafficLightState
10
11from agents.tools.hints import TrafficLightDetectionResult
12from agents.tools.logs import logger
13from agents.tools.misc import is_within_distance
14from classes.constants import AgentState
15from classes.information_manager import InformationManager
16from launch_tools import CarlaDataProvider
17
18if TYPE_CHECKING:
19 from classes.type_protocols import ActorList, CanDetectNearbyTrafficLights
20
21
22def _is_red_light(traffic_light: "carla.TrafficLight") -> bool:
23 """Filter function to check if a traffic light is red."""
24 return traffic_light.state == TrafficLightState.Red
25
26
27def _is_red_or_yellow(traffic_light: "carla.TrafficLight") -> bool:
28 """Filter function to check if a traffic light is red or yellow."""
29 return traffic_light.state in (TrafficLightState.Red, TrafficLightState.Yellow)
30
31
[docs]
32def affected_by_traffic_light(self: "CanDetectNearbyTrafficLights",
33 lights_list: Optional[ActorList[carla.TrafficLight]] = None,
34 max_distance: Optional[float] = None) -> TrafficLightDetectionResult:
35 """
36 Method to check if there is a red light affecting the vehicle.
37
38 Parameters:
39 lights_list: list containing traffic light objects.
40 If None, all traffic lights in the scene are used.
41 max_distance: max distance for a traffic lights to be considered relevant.
42 If None, the base threshold value is used.
43 """
44 if self.config.obstacles.ignore_traffic_lights:
45 return TrafficLightDetectionResult(False, None)
46
47 detect_yellow_tlighs = self.config.obstacles.detect_yellow_tlights
48
49 # Currently affected by a traffic light
50 if self._last_traffic_light:
51 if self._last_traffic_light.state != TrafficLightState.Red and (not detect_yellow_tlighs or self._last_traffic_light.state != TrafficLightState.Yellow):
52 self._last_traffic_light = None
53 else: # Still Red
54 return TrafficLightDetectionResult(True, self._last_traffic_light)
55
56 if lights_list is None:
57 if self._world_model._args.debug:
58 logger.warning("No traffic lights list provided, using all traffic lights in the scene. This should not happen."
59 "You possibly want to pass agent.traffic_lights_nearby or agent._lights_list instead.")
60 lights_list = cast("carla.ActorList[carla.TrafficLight]",
61 CarlaDataProvider.get_all_actors().filter("*traffic_light*"))
62 if len(lights_list) == 0:
63 return TrafficLightDetectionResult(False, None)
64
65 if not max_distance: # NOTE: dynamic selection is done in traffic_light_manager
66 max_distance = self.config.obstacles.base_tlight_threshold
67
68 ego_vehicle_location = self.config.live_info.current_location
69 ego_vehicle_waypoint = self._current_waypoint
70
71 filtered_lights = filter(_is_red_or_yellow if detect_yellow_tlighs else _is_red_light, lights_list) # type: ignore
72
73 for traffic_light in filtered_lights:
74 trigger_wp = InformationManager.get_trafficlight_trigger_waypoint(traffic_light)
75
76 if trigger_wp.road_id != ego_vehicle_waypoint.road_id:
77 continue
78
79 if trigger_wp.transform.location.distance(ego_vehicle_location) > max_distance:
80 continue
81
82 ve_dir = ego_vehicle_waypoint.transform.get_forward_vector()
83 wp_dir = trigger_wp.transform.get_forward_vector()
84 dot_ve_wp = ve_dir.x * wp_dir.x + ve_dir.y * wp_dir.y + ve_dir.z * wp_dir.z
85
86 if dot_ve_wp < 0:
87 continue
88
89 if is_within_distance(trigger_wp.transform, self._vehicle.get_transform(), max_distance, [0, 90]):
90 self._last_traffic_light = traffic_light
91 return TrafficLightDetectionResult(True, traffic_light)
92
93 return TrafficLightDetectionResult(False, None)
94
95
[docs]
96def detect_traffic_light(self: CanDetectNearbyTrafficLights,
97 traffic_lights: Optional[ActorList[carla.TrafficLight]] = None) -> TrafficLightDetectionResult:
98 """
99 This method is in charge of behaviors for red lights.
100 """
101
102 # Introduce a random chance to ignore the traffic light
103 if random.random() < self.config.obstacles.ignore_lights_percentage:
104 return TrafficLightDetectionResult(False, None)
105
106 traffic_lights = traffic_lights or self.traffic_lights_nearby
107
108 # Behavior setting:
109 max_tlight_distance = self.config.obstacles.base_tlight_threshold
110 if self.config.obstacles.dynamic_threshold:
111 # Basic agent setting:
112 #logger.info("Increased threshold for traffic light detection from {} to {}".format(max_tlight_distance,
113 # max_tlight_distance + self.config.obstacles.detection_speed_ratio * self.config.live_info.current_speed))
114 max_tlight_distance += self.config.obstacles.detection_speed_ratio * self.config.live_info.current_speed
115
116 # TODO: Time to pass the traffic light; i.e. can we pass it without stopping? -> How risky are we?
117
118 # TODO check if lights should be copied.
119 # lights = self.lights_list.copy() #could remove certain lights, or the current one for some ticks
120 affected_traffic_light: TrafficLightDetectionResult = affected_by_traffic_light(self, traffic_lights,
121 max_distance=max_tlight_distance)
122
123 if (affected_traffic_light.traffic_light_was_found
124 and affected_traffic_light.traffic_light.state == TrafficLightState.Red): # type: ignore[attr]
125 self.current_states[AgentState.BLOCKED_RED_LIGHT] += 1
126 else:
127 self.current_states[AgentState.BLOCKED_RED_LIGHT] = 0
128
129 # TODO: Implement other behaviors if needed, like taking a wrong turn or additional actions
130
131 return affected_traffic_light