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