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