1"""
2Example of automatic vehicle control from client side
3
4Based on original CARLA example by German Ros
5"""
6
7import math
8import operator
9import os
10from datetime import timedelta
11from typing import TYPE_CHECKING, ClassVar, Iterable, List, Optional, Tuple, Union, cast
12
13import carla
14import pygame
15
16from classes import CanBeDummy
17from classes.ui.keyboard_controls import RSSKeyboardControl
18from classes.sensors.rss_visualization import RssStateVisualizer
19
20if TYPE_CHECKING:
21 from pygame._common import ColorValue # type: ignore
22
23 from classes.worldmodel import WorldModel
24
25FONT_SIZE = 20
26
27
[docs]
28def get_actor_display_name(actor: carla.Actor, truncate: int = 250):
29 """Method to get actor display name"""
30 name = " ".join(actor.type_id.replace("_", ".").title().split(".")[1:])
31 return (name[: truncate - 1] + "\u2026") if len(name) > truncate else name
32
33
34# ==============================================================================
35# -- HUD -----------------------------------------------------------------------
36# ==============================================================================
37
38
[docs]
39class HUD(CanBeDummy):
40 """
41 Class to display text and other information on the :py:mod:`pygame` screen.
42
43 If :py:attr:`.LaunchConfig.pygame` is set to False the HUD is not available.
44 """
45
46 default_font: ClassVar[str] = "ubuntumono"
47
[docs]
48 def __init__(
49 self, width: int, height: int, world: carla.World, help_text: Optional[str] = RSSKeyboardControl.__doc__
50 ):
51 """Constructor method"""
52 self.dim = (width, height)
53 self._world = world
54 self.map_name = world.get_map().name
55 font = pygame.font.Font(pygame.font.get_default_font(), 20)
56 font_name = "courier" if os.name == "nt" else "mono"
57 fonts = [x for x in pygame.font.get_fonts() if font_name in x]
58 mono = self.default_font if self.default_font in fonts else fonts[0]
59 mono = pygame.font.match_font(mono)
60 self._font_mono = pygame.font.Font(mono, 12 if os.name == "nt" else 14)
61 self._notifications = FadingText(font, (width, 40), (0, height - 40))
62 self.help = HelpText(
63 pygame.font.Font(mono, FONT_SIZE), width, height, doc=help_text if help_text is not None else False
64 )
65 self.server_fps = 0
66 self.frame = 0
67 self.simulation_time = 0
68 self._show_info = True
69 self._info_text = []
70 # TODO: can this be removed? what with the tick method?
71 self._server_clock = pygame.time.Clock()
72 # RSS
73 self.original_vehicle_control: Optional[carla.VehicleControl] = None
74 # Not None if original_vehicle_control is not None
75 self.restricted_vehicle_control: carla.VehicleControl = None # type: ignore[assignment]
76 self.allowed_steering_ranges: List[Tuple[float, float]] = []
77 self.rss_state_visualizer = RssStateVisualizer(self.dim, self._font_mono, self._world)
78
[docs]
79 def on_world_tick(self, timestamp: carla.WorldSnapshot):
80 """Callback to get information from the world at every tick"""
81 self._server_clock.tick()
82 self.server_fps = self._server_clock.get_fps()
83 # self.frame = timestamp.frame_count # NON- RSS Example
84 self.frame = timestamp.frame
85 self.simulation_time = timestamp.timestamp.elapsed_seconds
86
[docs]
87 def tick(self, world: "WorldModel", clock: pygame.time.Clock, obstacles: Optional[Iterable[carla.Actor]] = None):
88 """
89 HUD method for every tick
90
91
92 Parameters:
93 obstacles: If **obstacles** is passed these actors will be displayed on the HUD,
94 if not the closest vehicles will be displayed.
95 """
96 self._notifications.tick(clock)
97 if not self._show_info:
98 return
99 player = cast("carla.Walker | carla.Vehicle", world.player)
100
101 transform = player.get_transform()
102 location = transform.location
103 vel = player.get_velocity()
104 control = player.get_control()
105 heading = "N" if abs(transform.rotation.yaw) < 89.5 else ""
106 heading += "S" if abs(transform.rotation.yaw) > 90.5 else ""
107 heading += "E" if 179.5 > transform.rotation.yaw > 0.5 else ""
108 heading += "W" if -0.5 > transform.rotation.yaw > -179.5 else ""
109 colhist = world.collision_sensor.get_collision_history()
110 collision = [colhist[x + self.frame - 200] for x in range(200)]
111 max_col = max(1.0, max(collision)) # noqa: PLW3301
112 collision = [x / max_col for x in collision]
113 obstacles = obstacles or world.world.get_actors().filter("*vehicle*")
114
115 # TODO: could also get ready distances from InformationManager, needs access to agent instance
116 # cached info would also prevent x from being destroyed in a different thread
117 obstacles_distances: "list[tuple[float, carla.Actor]]" = [
118 (x.get_location().distance(location), x) for x in obstacles if x.id != world.player.id and x.is_alive
119 ]
120
121 self._info_text: list[
122 Union[
123 str,
124 tuple[str, bool],
125 # Sequence[Union[str, float]],
126 tuple[str, float, float, float], # min value max
127 tuple[str, float, float, float, float],
128 # tuple[str, float, float, float, float, list[list[float]]], #
129 tuple[str, float, float, float, float, list[tuple[float, float]]], # steering
130 list[float],
131 ]
132 ]
133
134 self._info_text = [
135 "Server: {: 16.0f} FPS".format(self.server_fps),
136 "Client: {: 16.0f} FPS".format(clock.get_fps()),
137 "Map: {:>20s}".format(self.map_name), # from rss
138 "",
139 "Vehicle: {:>20s}".format(get_actor_display_name(player, truncate=20)),
140 "Map: {:>20s}".format(world.map.name.split("/")[-1]),
141 "Simulation time: {!s:>12s}".format(timedelta(seconds=int(self.simulation_time))),
142 "",
143 "Speed: {: 15.0f} km/h".format(3.6 * math.sqrt(vel.x**2 + vel.y**2 + vel.z**2)),
144 "Heading:{: 16.0f}\N{DEGREE SIGN} {:>2s}".format(transform.rotation.yaw, heading),
145 # TODO maybe 'Heading: {: 20.2f}'.format(math.radians(transform.rotation.yaw)),
146 "Location:{:>20s}".format(f"({transform.location.x: 5.1f}, {transform.location.y: 5.1f})"),
147 "Height: {: 18.0f} m".format(transform.location.z),
148 ]
149 if world.gnss_sensor:
150 self._info_text.append(
151 "GNSS:{:>24s}".format(f"({world.gnss_sensor.lat: 2.6f}, {world.gnss_sensor.lon: 3.6f})")
152 )
153 self._info_text.append("") # empty line
154 if isinstance(control, carla.VehicleControl):
155 if self.original_vehicle_control:
156 orig_control = self.original_vehicle_control
157 restricted_control = self.restricted_vehicle_control
158 allowed_steering_ranges = self.allowed_steering_ranges
159 self._info_text += [
160 ("Throttle:", orig_control.throttle, 0.0, 1.0, restricted_control.throttle),
161 ("Steer:", orig_control.steer, -1.0, 1.0, restricted_control.steer, allowed_steering_ranges),
162 ("Brake:", orig_control.brake, 0.0, 1.0, restricted_control.brake),
163 ]
164 else:
165 self._info_text += [
166 ("Throttle:", control.throttle, 0.0, 1.0),
167 ("Steer:", control.steer, -1.0, 1.0),
168 ("Brake:", control.brake, 0.0, 1.0),
169 ]
170 self._info_text += [
171 ("Reverse:", control.reverse),
172 ("Hand brake:", control.hand_brake),
173 ("Manual:", control.manual_gear_shift),
174 "Gear: {}".format({-1: "R", 0: "N"}.get(control.gear, control.gear)),
175 ]
176 elif isinstance(control, carla.WalkerControl): # pyright: ignore[reportUnnecessaryIsInstance]
177 self._info_text += [("Speed:", control.speed, 0.0, 5.556), ("Jump:", control.jump)]
178 # else unknown control type
179 self._info_text += ["", "Collision:", collision, "", f"Number of vehicles: {len(obstacles_distances): 8d}"]
180
181 if len(obstacles_distances) > 1:
182 self._info_text += ["Nearby obstacles:"]
183
184 for distance, vehicle in sorted(obstacles_distances, key=operator.itemgetter(0))[
185 :20
186 ]: # display at most 20 actors
187 if distance > 200.0:
188 break
189 vehicle_type = get_actor_display_name(vehicle, truncate=22)
190 self._info_text.append(f"{distance:>4.0f}m {vehicle_type}")
191
[docs]
192 def toggle_info(self):
193 """Toggle info on or off"""
194 self._show_info = not self._show_info
195
[docs]
196 def notification(self, text: str, seconds: float = 2.0):
197 """Display a notification"""
198 self._notifications.set_text(text, seconds=seconds)
199
[docs]
200 def error(self, text: str):
201 """Display an error notification"""
202 self._notifications.set_text(f"Error: {text}", (255, 0, 0))
203
[docs]
204 def render(self, display: pygame.Surface):
205 """Render for HUD class"""
206 if self._show_info:
207 info_surface = pygame.Surface((220, self.dim[1]))
208 info_surface.set_alpha(100)
209 display.blit(info_surface, (0, 0))
210 v_offset = 4
211 bar_h_offset = 100
212 bar_width = 106
213 text_color = (255, 255, 255)
214 render_item = None
215 for item in self._info_text:
216 if v_offset + 18 > self.dim[1]:
217 break
218 if isinstance(item, list):
219 if len(item) > 1:
220 points = [(x + 8, v_offset + 8 + (1 - y) * 30) for x, y in enumerate(item)]
221 pygame.draw.lines(display, (255, 136, 0), False, points, 2)
222 render_item = None
223 v_offset += 18
224 elif isinstance(item, tuple):
225 if isinstance(item[1], bool):
226 # rect = pygame.Rect((bar_h_offset, v_offset + 8), (6, 6))
227 rect = pygame.Rect((bar_h_offset, v_offset + 2), (10, 10))
228 pygame.draw.rect(display, (255, 255, 255), rect, 0 if item[1] else 1)
229 else:
230 # draw allowed steering ranges
231 if len(item) == 6 and item[2] < 0.0:
232 for steering_range in item[5]:
233 starting_value = min(steering_range[0], steering_range[1])
234 length = (
235 max(steering_range[0], steering_range[1])
236 - min(steering_range[0], steering_range[1])
237 ) / 2
238 rect = pygame.Rect(
239 (bar_h_offset + (starting_value + 1) * (bar_width / 2), v_offset + 2),
240 (length * bar_width, 14),
241 )
242 pygame.draw.rect(display, (0, 255, 0), rect)
243
244 # draw border
245 # rect_border = pygame.Rect((bar_h_offset, v_offset + 8), (bar_width, 6))
246 rect_border = pygame.Rect((bar_h_offset, v_offset + 2), (bar_width, 14))
247 pygame.draw.rect(display, (255, 255, 255), rect_border, 1)
248
249 # draw value / restricted value
250 input_value_rect_fill = 0
251 if len(item) >= 5:
252 if item[1] != item[4]:
253 input_value_rect_fill = 1
254 f = (item[4] - item[2]) / (item[3] - item[2])
255 if item[2] < 0.0:
256 rect = pygame.Rect((bar_h_offset + 1 + f * (bar_width - 6), v_offset + 3), (12, 12))
257 else:
258 rect = pygame.Rect((bar_h_offset + 1, v_offset + 3), (f * bar_width, 12))
259 pygame.draw.rect(display, (255, 0, 0), rect)
260
261 if TYPE_CHECKING:
262 assert len(item) > 2 # narrow some types
263 f = (item[1] - item[2]) / (item[3] - item[2])
264 rect = None
265 if item[2] < 0.0:
266 # rect = pygame.Rect(
267 # (bar_h_offset + fig * (bar_width - 6), v_offset + 8), (6, 6))
268 rect = pygame.Rect((bar_h_offset + 2 + f * (bar_width - 14), v_offset + 4), (10, 10))
269 else:
270 # rect = pygame.Rect((bar_h_offset, v_offset + 8), (fig * bar_width, 6))
271 if item[1] != 0:
272 rect = pygame.Rect((bar_h_offset + 2, v_offset + 4), (f * (bar_width - 4), 10))
273 if rect:
274 # pygame.draw.rect(display, (255, 255, 255), rect)
275 pygame.draw.rect(display, (255, 255, 255), rect, input_value_rect_fill)
276 render_item = item[0]
277 else:
278 render_item = item
279 if render_item: # At this point has to be a str
280 surface = self._font_mono.render(render_item, True, text_color)
281 display.blit(surface, (8, v_offset))
282 v_offset += 18
283
284 self.rss_state_visualizer.render(display, v_offset)
285 self._notifications.render(display)
286 self.help.render(display)
287
288
289# ==============================================================================
290# -- FadingText ----------------------------------------------------------------
291# ==============================================================================
292
293
294class FadingText:
295 """
296 Class for fading text
297
298 :meta private:
299 """
300
301 def __init__(self, font: pygame.font.Font, dim: "tuple[int, int]", pos: "tuple[int, int]"):
302 """Constructor method"""
303 self.font = font
304 self.dim = dim
305 self.pos = pos
306 self.seconds_left = 0
307 self.surface = pygame.Surface(self.dim)
308
309 def set_text(self, text: str, color: "ColorValue" = (255, 255, 255), seconds: float = 2.0):
310 """Set fading text"""
311 text_texture = self.font.render(text, True, color)
312 self.surface = pygame.Surface(self.dim)
313 self.seconds_left = seconds
314 self.surface.fill((0, 0, 0, 0))
315 self.surface.blit(text_texture, (10, 11))
316
317 def tick(self, clock: pygame.time.Clock):
318 """Fading text method for every tick"""
319 delta_seconds = 1e-3 * clock.get_time()
320 self.seconds_left = max(0.0, self.seconds_left - delta_seconds)
321 self.surface.set_alpha(500.0 * self.seconds_left) # type: ignore[arg-type]
322
323 def render(self, display: pygame.Surface):
324 """Render fading text method"""
325 display.blit(self.surface, self.pos)
326
327
328# ==============================================================================
329# -- HelpText ------------------------------------------------------------------
330# ==============================================================================
331
332
333class HelpText:
334 """
335 Helper class to handle text output using pygame
336
337 :meta private:
338 """
339
340 def __init__(self, font: pygame.font.Font, width: int, height: int, doc: Optional[Union[str, bool]] = None):
341 """Constructor method"""
342 self.line_space = 18
343 self.font = font
344 self.seconds_left = 0
345 self._width = width
346 self._height = height
347 if doc is not False:
348 doc = doc or __doc__ if doc is not True else __doc__
349 assert doc, "No docstring available for help text."
350 self.create_surface(doc) # Use doc of THIS file, analog to carla examples.
351 else:
352 self.surface = None
353 self._render = False
354
355 def create_surface(self, doc: str):
356 """Create surface method"""
357 lines = doc.split("\n")
358 self.dim = (780, len(lines) * self.line_space + 12)
359 # self.dim = (680, len(lines) * 22 + 12)
360 self.pos = (0.5 * self._width - 0.5 * self.dim[0], 0.5 * self._height - 0.5 * self.dim[1])
361 self.surface = pygame.Surface(self.dim)
362 self.surface.fill((0, 0, 0, 0))
363 for i, line in enumerate(lines):
364 text_texture = self.font.render(line, True, (255, 255, 255))
365 self.surface.blit(text_texture, (22, i * self.line_space))
366 self.surface.set_alpha(220)
367
368 def toggle(self):
369 """Toggle on or off the render help"""
370 if self.surface is None:
371 print(
372 "Warning: No help text available - Initialized with doc=False. "
373 "Cannot display help. Call create_surface first."
374 )
375 return
376 self._render = not self._render
377
378 def render(self, display: pygame.Surface):
379 """Render help text method"""
380 if self._render:
381 display.blit(self.surface, self.pos) # type: ignore