no_rendering_mode.py 61 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304305306307308309310311312313314315316317318319320321322323324325326327328329330331332333334335336337338339340341342343344345346347348349350351352353354355356357358359360361362363364365366367368369370371372373374375376377378379380381382383384385386387388389390391392393394395396397398399400401402403404405406407408409410411412413414415416417418419420421422423424425426427428429430431432433434435436437438439440441442443444445446447448449450451452453454455456457458459460461462463464465466467468469470471472473474475476477478479480481482483484485486487488489490491492493494495496497498499500501502503504505506507508509510511512513514515516517518519520521522523524525526527528529530531532533534535536537538539540541542543544545546547548549550551552553554555556557558559560561562563564565566567568569570571572573574575576577578579580581582583584585586587588589590591592593594595596597598599600601602603604605606607608609610611612613614615616617618619620621622623624625626627628629630631632633634635636637638639640641642643644645646647648649650651652653654655656657658659660661662663664665666667668669670671672673674675676677678679680681682683684685686687688689690691692693694695696697698699700701702703704705706707708709710711712713714715716717718719720721722723724725726727728729730731732733734735736737738739740741742743744745746747748749750751752753754755756757758759760761762763764765766767768769770771772773774775776777778779780781782783784785786787788789790791792793794795796797798799800801802803804805806807808809810811812813814815816817818819820821822823824825826827828829830831832833834835836837838839840841842843844845846847848849850851852853854855856857858859860861862863864865866867868869870871872873874875876877878879880881882883884885886887888889890891892893894895896897898899900901902903904905906907908909910911912913914915916917918919920921922923924925926927928929930931932933934935936937938939940941942943944945946947948949950951952953954955956957958959960961962963964965966967968969970971972973974975976977978979980981982983984985986987988989990991992993994995996997998999100010011002100310041005100610071008100910101011101210131014101510161017101810191020102110221023102410251026102710281029103010311032103310341035103610371038103910401041104210431044104510461047104810491050105110521053105410551056105710581059106010611062106310641065106610671068106910701071107210731074107510761077107810791080108110821083108410851086108710881089109010911092109310941095109610971098109911001101110211031104110511061107110811091110111111121113111411151116111711181119112011211122112311241125112611271128112911301131113211331134113511361137113811391140114111421143114411451146114711481149115011511152115311541155115611571158115911601161116211631164116511661167116811691170117111721173117411751176117711781179118011811182118311841185118611871188118911901191119211931194119511961197119811991200120112021203120412051206120712081209121012111212121312141215121612171218121912201221122212231224122512261227122812291230123112321233123412351236123712381239124012411242124312441245124612471248124912501251125212531254125512561257125812591260126112621263126412651266126712681269127012711272127312741275127612771278127912801281128212831284128512861287128812891290129112921293129412951296129712981299130013011302130313041305130613071308130913101311131213131314131513161317131813191320132113221323132413251326132713281329133013311332133313341335133613371338133913401341134213431344134513461347134813491350135113521353135413551356135713581359136013611362136313641365136613671368136913701371137213731374137513761377137813791380138113821383138413851386138713881389139013911392139313941395139613971398139914001401140214031404140514061407140814091410141114121413141414151416141714181419142014211422142314241425142614271428142914301431143214331434143514361437143814391440144114421443144414451446144714481449145014511452145314541455145614571458145914601461146214631464146514661467146814691470147114721473147414751476147714781479148014811482148314841485148614871488148914901491149214931494
  1. #!/usr/bin/env python
  2. # Copyright (c) 2019 Computer Vision Center (CVC) at the Universitat Autonoma de
  3. # Barcelona (UAB).
  4. #
  5. # This work is licensed under the terms of the MIT license.
  6. # For a copy, see <https://opensource.org/licenses/MIT>.
  7. # Allows visualising a 2D map generated by vehicles.
  8. """
  9. Welcome to CARLA No-Rendering Mode Visualizer
  10. TAB : toggle hero mode
  11. Mouse Wheel : zoom in / zoom out
  12. Mouse Drag : move map (map mode only)
  13. W : throttle
  14. S : brake
  15. AD : steer
  16. Q : toggle reverse
  17. Space : hand-brake
  18. P : toggle autopilot
  19. M : toggle manual transmission
  20. ,/. : gear up/down
  21. F1 : toggle HUD
  22. I : toggle actor ids
  23. H/? : toggle help
  24. ESC : quit
  25. """
  26. # ==============================================================================
  27. # -- find carla module ---------------------------------------------------------
  28. # ==============================================================================
  29. import glob
  30. import os
  31. import sys
  32. try:
  33. sys.path.append(glob.glob('../carla/dist/carla-*%d.%d-%s.egg' % (
  34. sys.version_info.major,
  35. sys.version_info.minor,
  36. 'win-amd64' if os.name == 'nt' else 'linux-x86_64'))[0])
  37. except IndexError:
  38. pass
  39. # ==============================================================================
  40. # -- imports -------------------------------------------------------------------
  41. # ==============================================================================
  42. import carla
  43. from carla import TrafficLightState as tls
  44. import argparse
  45. import logging
  46. import datetime
  47. import weakref
  48. import math
  49. import random
  50. try:
  51. import pygame
  52. from pygame.locals import KMOD_CTRL
  53. from pygame.locals import KMOD_SHIFT
  54. from pygame.locals import K_COMMA
  55. from pygame.locals import K_DOWN
  56. from pygame.locals import K_ESCAPE
  57. from pygame.locals import K_F1
  58. from pygame.locals import K_LEFT
  59. from pygame.locals import K_PERIOD
  60. from pygame.locals import K_RIGHT
  61. from pygame.locals import K_SLASH
  62. from pygame.locals import K_SPACE
  63. from pygame.locals import K_TAB
  64. from pygame.locals import K_UP
  65. from pygame.locals import K_a
  66. from pygame.locals import K_d
  67. from pygame.locals import K_h
  68. from pygame.locals import K_i
  69. from pygame.locals import K_m
  70. from pygame.locals import K_p
  71. from pygame.locals import K_q
  72. from pygame.locals import K_s
  73. from pygame.locals import K_w
  74. except ImportError:
  75. raise RuntimeError('cannot import pygame, make sure pygame package is installed')
  76. # ==============================================================================
  77. # -- Constants -----------------------------------------------------------------
  78. # ==============================================================================
  79. # Colors
  80. # We will use the color palette used in Tango Desktop Project (Each color is indexed depending on brightness level)
  81. # See: https://en.wikipedia.org/wiki/Tango_Desktop_Project
  82. COLOR_BUTTER_0 = pygame.Color(252, 233, 79)
  83. COLOR_BUTTER_1 = pygame.Color(237, 212, 0)
  84. COLOR_BUTTER_2 = pygame.Color(196, 160, 0)
  85. COLOR_ORANGE_0 = pygame.Color(252, 175, 62)
  86. COLOR_ORANGE_1 = pygame.Color(245, 121, 0)
  87. COLOR_ORANGE_2 = pygame.Color(209, 92, 0)
  88. COLOR_CHOCOLATE_0 = pygame.Color(233, 185, 110)
  89. COLOR_CHOCOLATE_1 = pygame.Color(193, 125, 17)
  90. COLOR_CHOCOLATE_2 = pygame.Color(143, 89, 2)
  91. COLOR_CHAMELEON_0 = pygame.Color(138, 226, 52)
  92. COLOR_CHAMELEON_1 = pygame.Color(115, 210, 22)
  93. COLOR_CHAMELEON_2 = pygame.Color(78, 154, 6)
  94. COLOR_SKY_BLUE_0 = pygame.Color(114, 159, 207)
  95. COLOR_SKY_BLUE_1 = pygame.Color(52, 101, 164)
  96. COLOR_SKY_BLUE_2 = pygame.Color(32, 74, 135)
  97. COLOR_PLUM_0 = pygame.Color(173, 127, 168)
  98. COLOR_PLUM_1 = pygame.Color(117, 80, 123)
  99. COLOR_PLUM_2 = pygame.Color(92, 53, 102)
  100. COLOR_SCARLET_RED_0 = pygame.Color(239, 41, 41)
  101. COLOR_SCARLET_RED_1 = pygame.Color(204, 0, 0)
  102. COLOR_SCARLET_RED_2 = pygame.Color(164, 0, 0)
  103. COLOR_ALUMINIUM_0 = pygame.Color(238, 238, 236)
  104. COLOR_ALUMINIUM_1 = pygame.Color(211, 215, 207)
  105. COLOR_ALUMINIUM_2 = pygame.Color(186, 189, 182)
  106. COLOR_ALUMINIUM_3 = pygame.Color(136, 138, 133)
  107. COLOR_ALUMINIUM_4 = pygame.Color(85, 87, 83)
  108. COLOR_ALUMINIUM_4_5 = pygame.Color(66, 62, 64)
  109. COLOR_ALUMINIUM_5 = pygame.Color(46, 52, 54)
  110. COLOR_WHITE = pygame.Color(255, 255, 255)
  111. COLOR_BLACK = pygame.Color(0, 0, 0)
  112. # Module Defines
  113. MODULE_WORLD = 'WORLD'
  114. MODULE_HUD = 'HUD'
  115. MODULE_INPUT = 'INPUT'
  116. PIXELS_PER_METER = 12
  117. MAP_DEFAULT_SCALE = 0.1
  118. HERO_DEFAULT_SCALE = 1.0
  119. PIXELS_AHEAD_VEHICLE = 150
  120. # ==============================================================================
  121. # -- Util -----------------------------------------------------------
  122. # ==============================================================================
  123. def get_actor_display_name(actor, truncate=250):
  124. name = ' '.join(actor.type_id.replace('_', '.').title().split('.')[1:])
  125. return (name[:truncate - 1] + u'\u2026') if len(name) > truncate else name
  126. class Util(object):
  127. @staticmethod
  128. def blits(destination_surface, source_surfaces, rect=None, blend_mode=0):
  129. for surface in source_surfaces:
  130. destination_surface.blit(surface[0], surface[1], rect, blend_mode)
  131. @staticmethod
  132. def length(v):
  133. return math.sqrt(v.x**2 + v.y**2 + v.z**2)
  134. @staticmethod
  135. def get_bounding_box(actor):
  136. bb = actor.trigger_volume.extent
  137. corners = [carla.Location(x=-bb.x, y=-bb.y),
  138. carla.Location(x=bb.x, y=-bb.y),
  139. carla.Location(x=bb.x, y=bb.y),
  140. carla.Location(x=-bb.x, y=bb.y),
  141. carla.Location(x=-bb.x, y=-bb.y)]
  142. corners = [x + actor.trigger_volume.location for x in corners]
  143. t = actor.get_transform()
  144. t.transform(corners)
  145. return corners
  146. # ==============================================================================
  147. # -- ModuleManager -------------------------------------------------------------
  148. # ==============================================================================
  149. class ModuleManager(object):
  150. def __init__(self):
  151. self.modules = []
  152. def register_module(self, module):
  153. self.modules.append(module)
  154. def clear_modules(self):
  155. del self.modules[:]
  156. def tick(self, clock):
  157. # Update all the modules
  158. for module in self.modules:
  159. module.tick(clock)
  160. def render(self, display):
  161. display.fill(COLOR_ALUMINIUM_4)
  162. for module in self.modules:
  163. module.render(display)
  164. def get_module(self, name):
  165. for module in self.modules:
  166. if module.name == name:
  167. return module
  168. def start_modules(self):
  169. for module in self.modules:
  170. module.start()
  171. # ==============================================================================
  172. # -- FadingText ----------------------------------------------------------------
  173. # ==============================================================================
  174. class FadingText(object):
  175. def __init__(self, font, dim, pos):
  176. self.font = font
  177. self.dim = dim
  178. self.pos = pos
  179. self.seconds_left = 0
  180. self.surface = pygame.Surface(self.dim)
  181. def set_text(self, text, color=COLOR_WHITE, seconds=2.0):
  182. text_texture = self.font.render(text, True, color)
  183. self.surface = pygame.Surface(self.dim)
  184. self.seconds_left = seconds
  185. self.surface.fill(COLOR_BLACK)
  186. self.surface.blit(text_texture, (10, 11))
  187. def tick(self, clock):
  188. delta_seconds = 1e-3 * clock.get_time()
  189. self.seconds_left = max(0.0, self.seconds_left - delta_seconds)
  190. self.surface.set_alpha(500.0 * self.seconds_left)
  191. def render(self, display):
  192. display.blit(self.surface, self.pos)
  193. # ==============================================================================
  194. # -- HelpText ------------------------------------------------------------------
  195. # ==============================================================================
  196. class HelpText(object):
  197. def __init__(self, font, width, height):
  198. lines = __doc__.split('\n')
  199. self.font = font
  200. self.dim = (680, len(lines) * 22 + 12)
  201. self.pos = (0.5 * width - 0.5 * self.dim[0], 0.5 * height - 0.5 * self.dim[1])
  202. self.seconds_left = 0
  203. self.surface = pygame.Surface(self.dim)
  204. self.surface.fill(COLOR_BLACK)
  205. for n, line in enumerate(lines):
  206. text_texture = self.font.render(line, True, COLOR_WHITE)
  207. self.surface.blit(text_texture, (22, n * 22))
  208. self._render = False
  209. self.surface.set_alpha(220)
  210. def toggle(self):
  211. self._render = not self._render
  212. def render(self, display):
  213. if self._render:
  214. display.blit(self.surface, self.pos)
  215. # ==============================================================================
  216. # -- ModuleHUD -----------------------------------------------------------------
  217. # ==============================================================================
  218. class ModuleHUD (object):
  219. def __init__(self, name, width, height):
  220. self.name = name
  221. self.dim = (width, height)
  222. self._init_hud_params()
  223. self._init_data_params()
  224. def start(self):
  225. pass
  226. def _init_hud_params(self):
  227. fonts = [x for x in pygame.font.get_fonts() if 'mono' in x]
  228. default_font = 'ubuntumono'
  229. mono = default_font if default_font in fonts else fonts[0]
  230. mono = pygame.font.match_font(mono)
  231. self._font_mono = pygame.font.Font(mono, 14)
  232. self._header_font = pygame.font.SysFont('Arial', 14, True)
  233. self.help = HelpText(pygame.font.Font(mono, 24), *self.dim)
  234. self._notifications = FadingText(
  235. pygame.font.Font(pygame.font.get_default_font(), 20),
  236. (self.dim[0], 40), (0, self.dim[1] - 40))
  237. def _init_data_params(self):
  238. self.show_info = True
  239. self.show_actor_ids = False
  240. self._info_text = {}
  241. def notification(self, text, seconds=2.0):
  242. self._notifications.set_text(text, seconds=seconds)
  243. def tick(self, clock):
  244. self._notifications.tick(clock)
  245. def add_info(self, module_name, info):
  246. self._info_text[module_name] = info
  247. def render_vehicles_ids(self, vehicle_id_surface, list_actors, world_to_pixel, hero_actor, hero_transform):
  248. vehicle_id_surface.fill(COLOR_BLACK)
  249. if self.show_actor_ids:
  250. vehicle_id_surface.set_alpha(150)
  251. for actor in list_actors:
  252. x, y = world_to_pixel(actor[1].location)
  253. angle = 0
  254. if hero_actor is not None:
  255. angle = -hero_transform.rotation.yaw - 90
  256. color = COLOR_SKY_BLUE_0
  257. if int(actor[0].attributes['number_of_wheels']) == 2:
  258. color = COLOR_CHOCOLATE_0
  259. if actor[0].attributes['role_name'] == 'hero':
  260. color = COLOR_CHAMELEON_0
  261. font_surface = self._header_font.render(str(actor[0].id), True, color)
  262. rotated_font_surface = pygame.transform.rotate(font_surface, angle)
  263. rect = rotated_font_surface.get_rect(center=(x, y))
  264. vehicle_id_surface.blit(rotated_font_surface, rect)
  265. return vehicle_id_surface
  266. def render(self, display):
  267. if self.show_info:
  268. info_surface = pygame.Surface((240, self.dim[1]))
  269. info_surface.set_alpha(100)
  270. display.blit(info_surface, (0, 0))
  271. v_offset = 4
  272. bar_h_offset = 100
  273. bar_width = 106
  274. i = 0
  275. for module_name, module_info in self._info_text.items():
  276. if not module_info:
  277. continue
  278. surface = self._header_font.render(module_name, True, COLOR_ALUMINIUM_0).convert_alpha()
  279. display.blit(surface, (8 + bar_width / 2, 18 * i + v_offset))
  280. v_offset += 12
  281. i += 1
  282. for item in module_info:
  283. if v_offset + 18 > self.dim[1]:
  284. break
  285. if isinstance(item, list):
  286. if len(item) > 1:
  287. points = [(x + 8, v_offset + 8 + (1.0 - y) * 30) for x, y in enumerate(item)]
  288. pygame.draw.lines(display, (255, 136, 0), False, points, 2)
  289. item = None
  290. elif isinstance(item, tuple):
  291. if isinstance(item[1], bool):
  292. rect = pygame.Rect((bar_h_offset, v_offset + 8), (6, 6))
  293. pygame.draw.rect(display, COLOR_ALUMINIUM_0, rect, 0 if item[1] else 1)
  294. else:
  295. rect_border = pygame.Rect((bar_h_offset, v_offset + 8), (bar_width, 6))
  296. pygame.draw.rect(display, COLOR_ALUMINIUM_0, rect_border, 1)
  297. f = (item[1] - item[2]) / (item[3] - item[2])
  298. if item[2] < 0.0:
  299. rect = pygame.Rect((bar_h_offset + f * (bar_width - 6), v_offset + 8), (6, 6))
  300. else:
  301. rect = pygame.Rect((bar_h_offset, v_offset + 8), (f * bar_width, 6))
  302. pygame.draw.rect(display, COLOR_ALUMINIUM_0, rect)
  303. item = item[0]
  304. if item: # At this point has to be a str.
  305. surface = self._font_mono.render(item, True, COLOR_ALUMINIUM_0).convert_alpha()
  306. display.blit(surface, (8, 18 * i + v_offset))
  307. v_offset += 18
  308. v_offset += 24
  309. self._notifications.render(display)
  310. self.help.render(display)
  311. # ==============================================================================
  312. # -- TrafficLightSurfaces ------------------------------------------------------
  313. # ==============================================================================
  314. class TrafficLightSurfaces(object):
  315. """Holds the surfaces (scaled and rotated) for painting traffic lights"""
  316. def __init__(self):
  317. def make_surface(tl):
  318. w = 40
  319. surface = pygame.Surface((w, 3 * w), pygame.SRCALPHA)
  320. surface.fill(COLOR_ALUMINIUM_5 if tl != 'h' else COLOR_ORANGE_2)
  321. if tl != 'h':
  322. hw = int(w / 2)
  323. off = COLOR_ALUMINIUM_4
  324. red = COLOR_SCARLET_RED_0
  325. yellow = COLOR_BUTTER_0
  326. green = COLOR_CHAMELEON_0
  327. pygame.draw.circle(surface, red if tl == tls.Red else off, (hw, hw), int(0.4 * w))
  328. pygame.draw.circle(surface, yellow if tl == tls.Yellow else off, (hw, w + hw), int(0.4 * w))
  329. pygame.draw.circle(surface, green if tl == tls.Green else off, (hw, 2 * w + hw), int(0.4 * w))
  330. return pygame.transform.smoothscale(surface, (15, 45) if tl != 'h' else (19, 49))
  331. self._original_surfaces = {
  332. 'h': make_surface('h'),
  333. tls.Red: make_surface(tls.Red),
  334. tls.Yellow: make_surface(tls.Yellow),
  335. tls.Green: make_surface(tls.Green),
  336. tls.Off: make_surface(tls.Off),
  337. tls.Unknown: make_surface(tls.Unknown)
  338. }
  339. self.surfaces = dict(self._original_surfaces)
  340. def rotozoom(self, angle, scale):
  341. for key, surface in self._original_surfaces.items():
  342. self.surfaces[key] = pygame.transform.rotozoom(surface, angle, scale)
  343. # ==============================================================================
  344. # -- World ---------------------------------------------------------------------
  345. # ==============================================================================
  346. class MapImage(object):
  347. def __init__(self, carla_world, carla_map, pixels_per_meter, show_triggers, show_connections, show_spawn_points):
  348. self._pixels_per_meter = pixels_per_meter
  349. self.scale = 1.0
  350. self.show_triggers = show_triggers
  351. self.show_connections = show_connections
  352. self.show_spawn_points = show_spawn_points
  353. waypoints = carla_map.generate_waypoints(2)
  354. margin = 50
  355. max_x = max(waypoints, key=lambda x: x.transform.location.x).transform.location.x + margin
  356. max_y = max(waypoints, key=lambda x: x.transform.location.y).transform.location.y + margin
  357. min_x = min(waypoints, key=lambda x: x.transform.location.x).transform.location.x - margin
  358. min_y = min(waypoints, key=lambda x: x.transform.location.y).transform.location.y - margin
  359. self.width = max(max_x - min_x, max_y - min_y)
  360. self._world_offset = (min_x, min_y)
  361. width_in_pixels = int(self._pixels_per_meter * self.width)
  362. self.big_map_surface = pygame.Surface((width_in_pixels, width_in_pixels)).convert()
  363. self.draw_road_map(self.big_map_surface, carla_world, carla_map, self.world_to_pixel, self.world_to_pixel_width)
  364. self.surface = self.big_map_surface
  365. def draw_road_map(self, map_surface, carla_world, carla_map, world_to_pixel, world_to_pixel_width):
  366. map_surface.fill(COLOR_ALUMINIUM_4)
  367. precision = 0.05
  368. def lane_marking_color_to_tango(lane_marking_color):
  369. tango_color = COLOR_BLACK
  370. if lane_marking_color == carla.LaneMarkingColor.White:
  371. tango_color = COLOR_ALUMINIUM_2
  372. elif lane_marking_color == carla.LaneMarkingColor.Blue:
  373. tango_color = COLOR_SKY_BLUE_0
  374. elif lane_marking_color == carla.LaneMarkingColor.Green:
  375. tango_color = COLOR_CHAMELEON_0
  376. elif lane_marking_color == carla.LaneMarkingColor.Red:
  377. tango_color = COLOR_SCARLET_RED_0
  378. elif lane_marking_color == carla.LaneMarkingColor.Yellow:
  379. tango_color = COLOR_ORANGE_0
  380. return tango_color
  381. def draw_solid_line(surface, color, closed, points, width):
  382. if len(points) >= 2:
  383. pygame.draw.lines(surface, color, closed, points, width)
  384. def draw_broken_line(surface, color, closed, points, width):
  385. broken_lines = [x for n, x in enumerate(zip(*(iter(points),) * 20)) if n % 3 == 0]
  386. for line in broken_lines:
  387. pygame.draw.lines(surface, color, closed, line, width)
  388. def get_lane_markings(lane_marking_type, lane_marking_color, waypoints, sign):
  389. margin = 0.20
  390. if lane_marking_type == carla.LaneMarkingType.Broken or (lane_marking_type == carla.LaneMarkingType.Solid):
  391. marking_1 = [world_to_pixel(lateral_shift(w.transform, sign * w.lane_width * 0.5)) for w in waypoints]
  392. return [(lane_marking_type, lane_marking_color, marking_1)]
  393. elif lane_marking_type == carla.LaneMarkingType.SolidBroken or lane_marking_type == carla.LaneMarkingType.BrokenSolid:
  394. marking_1 = [world_to_pixel(lateral_shift(w.transform, sign * w.lane_width * 0.5)) for w in waypoints]
  395. marking_2 = [world_to_pixel(lateral_shift(w.transform,
  396. sign * (w.lane_width * 0.5 + margin * 2))) for w in waypoints]
  397. return [(carla.LaneMarkingType.Solid, lane_marking_color, marking_1),
  398. (carla.LaneMarkingType.Broken, lane_marking_color, marking_2)]
  399. elif lane_marking_type == carla.LaneMarkingType.BrokenBroken:
  400. marking = [world_to_pixel(lateral_shift(w.transform,
  401. sign * (w.lane_width * 0.5 - margin))) for w in waypoints]
  402. return [(carla.LaneMarkingType.Broken, lane_marking_color, marking)]
  403. elif lane_marking_type == carla.LaneMarkingType.SolidSolid:
  404. marking = [world_to_pixel(lateral_shift(w.transform,
  405. sign * ((w.lane_width * 0.5) - margin))) for w in waypoints]
  406. return [(carla.LaneMarkingType.Solid, lane_marking_color, marking)]
  407. return [(carla.LaneMarkingType.NONE, carla.LaneMarkingColor.Other, [])]
  408. def draw_lane_marking(surface, waypoints, is_left):
  409. sign = -1 if is_left else 1
  410. lane_marking = None
  411. marking_type = carla.LaneMarkingType.NONE
  412. previous_marking_type = carla.LaneMarkingType.NONE
  413. marking_color = carla.LaneMarkingColor.Other
  414. previous_marking_color = carla.LaneMarkingColor.Other
  415. waypoints_list = []
  416. temp_waypoints = []
  417. current_lane_marking = carla.LaneMarkingType.NONE
  418. for sample in waypoints:
  419. lane_marking = sample.left_lane_marking if sign < 0 else sample.right_lane_marking
  420. if lane_marking is None:
  421. continue
  422. marking_type = lane_marking.type
  423. marking_color = lane_marking.color
  424. if current_lane_marking != marking_type:
  425. markings = get_lane_markings(
  426. previous_marking_type,
  427. lane_marking_color_to_tango(previous_marking_color),
  428. temp_waypoints,
  429. sign)
  430. current_lane_marking = marking_type
  431. for marking in markings:
  432. waypoints_list.append(marking)
  433. temp_waypoints = temp_waypoints[-1:]
  434. else:
  435. temp_waypoints.append((sample))
  436. previous_marking_type = marking_type
  437. previous_marking_color = marking_color
  438. # Add last marking
  439. last_markings = get_lane_markings(
  440. previous_marking_type,
  441. lane_marking_color_to_tango(previous_marking_color),
  442. temp_waypoints,
  443. sign)
  444. for marking in last_markings:
  445. waypoints_list.append(marking)
  446. for markings in waypoints_list:
  447. if markings[0] == carla.LaneMarkingType.Solid:
  448. draw_solid_line(surface, markings[1], False, markings[2], 2)
  449. elif markings[0] == carla.LaneMarkingType.Broken:
  450. draw_broken_line(surface, markings[1], False, markings[2], 2)
  451. def draw_arrow(surface, transform, color=COLOR_ALUMINIUM_2):
  452. transform.rotation.yaw += 180
  453. forward = transform.get_forward_vector()
  454. transform.rotation.yaw += 90
  455. right_dir = transform.get_forward_vector()
  456. end = transform.location
  457. start = end - 2.0 * forward
  458. right = start + 0.8 * forward + 0.4 * right_dir
  459. left = start + 0.8 * forward - 0.4 * right_dir
  460. pygame.draw.lines(
  461. surface, color, False, [
  462. world_to_pixel(x) for x in [
  463. start, end]], 4)
  464. pygame.draw.lines(
  465. surface, color, False, [
  466. world_to_pixel(x) for x in [
  467. left, start, right]], 4)
  468. def draw_traffic_signs(surface, font_surface, actor, color=COLOR_ALUMINIUM_2, trigger_color=COLOR_PLUM_0):
  469. transform = actor.get_transform()
  470. waypoint = carla_map.get_waypoint(transform.location)
  471. angle = -waypoint.transform.rotation.yaw - 90.0
  472. font_surface = pygame.transform.rotate(font_surface, angle)
  473. pixel_pos = world_to_pixel(waypoint.transform.location)
  474. offset = font_surface.get_rect(center=(pixel_pos[0], pixel_pos[1]))
  475. surface.blit(font_surface, offset)
  476. # Draw line in front of stop
  477. forward_vector = carla.Location(waypoint.transform.get_forward_vector())
  478. left_vector = carla.Location(-forward_vector.y, forward_vector.x,
  479. forward_vector.z) * waypoint.lane_width / 2 * 0.7
  480. line = [(waypoint.transform.location + (forward_vector * 1.5) + (left_vector)),
  481. (waypoint.transform.location + (forward_vector * 1.5) - (left_vector))]
  482. line_pixel = [world_to_pixel(p) for p in line]
  483. pygame.draw.lines(surface, color, True, line_pixel, 2)
  484. # draw bounding box
  485. if self.show_triggers:
  486. corners = Util.get_bounding_box(actor)
  487. corners = [world_to_pixel(p) for p in corners]
  488. pygame.draw.lines(surface, trigger_color, True, corners, 2)
  489. def lateral_shift(transform, shift):
  490. transform.rotation.yaw += 90
  491. return transform.location + shift * transform.get_forward_vector()
  492. def draw_topology(carla_topology, index):
  493. topology = [x[index] for x in carla_topology]
  494. topology = sorted(topology, key=lambda w: w.transform.location.z)
  495. for waypoint in topology:
  496. # if waypoint.road_id == 150 or waypoint.road_id == 16:
  497. waypoints = [waypoint]
  498. nxt = waypoint.next(precision)
  499. if len(nxt) > 0:
  500. nxt = nxt[0]
  501. while nxt.road_id == waypoint.road_id:
  502. waypoints.append(nxt)
  503. nxt = nxt.next(precision)
  504. if len(nxt) > 0:
  505. nxt = nxt[0]
  506. else:
  507. break
  508. # Draw Road
  509. road_left_side = [lateral_shift(w.transform, -w.lane_width * 0.5) for w in waypoints]
  510. road_right_side = [lateral_shift(w.transform, w.lane_width * 0.5) for w in waypoints]
  511. polygon = road_left_side + [x for x in reversed(road_right_side)]
  512. polygon = [world_to_pixel(x) for x in polygon]
  513. if len(polygon) > 2:
  514. pygame.draw.polygon(map_surface, COLOR_ALUMINIUM_5, polygon, 5)
  515. pygame.draw.polygon(map_surface, COLOR_ALUMINIUM_5, polygon)
  516. # Draw Shoulders and Parkings
  517. PARKING_COLOR = COLOR_ALUMINIUM_4_5
  518. SHOULDER_COLOR = COLOR_ALUMINIUM_5
  519. final_color = SHOULDER_COLOR
  520. # Draw Right
  521. shoulder = []
  522. for w in waypoints:
  523. r = w.get_right_lane()
  524. if r is not None and (
  525. r.lane_type == carla.LaneType.Shoulder or r.lane_type == carla.LaneType.Parking):
  526. if r.lane_type == carla.LaneType.Parking:
  527. final_color = PARKING_COLOR
  528. shoulder.append(r)
  529. shoulder_left_side = [lateral_shift(w.transform, -w.lane_width * 0.5) for w in shoulder]
  530. shoulder_right_side = [lateral_shift(w.transform, w.lane_width * 0.5) for w in shoulder]
  531. polygon = shoulder_left_side + [x for x in reversed(shoulder_right_side)]
  532. polygon = [world_to_pixel(x) for x in polygon]
  533. if len(polygon) > 2:
  534. pygame.draw.polygon(map_surface, final_color, polygon, 5)
  535. pygame.draw.polygon(map_surface, final_color, polygon)
  536. draw_lane_marking(
  537. map_surface,
  538. shoulder,
  539. False)
  540. # Draw Left
  541. shoulder = []
  542. for w in waypoints:
  543. r = w.get_left_lane()
  544. if r is not None and (
  545. r.lane_type == carla.LaneType.Shoulder or r.lane_type == carla.LaneType.Parking):
  546. if r.lane_type == carla.LaneType.Parking:
  547. final_color = PARKING_COLOR
  548. shoulder.append(r)
  549. shoulder_left_side = [lateral_shift(w.transform, -w.lane_width * 0.5) for w in shoulder]
  550. shoulder_right_side = [lateral_shift(w.transform, w.lane_width * 0.5) for w in shoulder]
  551. polygon = shoulder_left_side + [x for x in reversed(shoulder_right_side)]
  552. polygon = [world_to_pixel(x) for x in polygon]
  553. if len(polygon) > 2:
  554. pygame.draw.polygon(map_surface, final_color, polygon, 5)
  555. pygame.draw.polygon(map_surface, final_color, polygon)
  556. draw_lane_marking(
  557. map_surface,
  558. shoulder,
  559. True)
  560. # Draw Lane Markings and Arrows
  561. if not waypoint.is_intersection:
  562. draw_lane_marking(
  563. map_surface,
  564. waypoints,
  565. True)
  566. draw_lane_marking(
  567. map_surface,
  568. waypoints,
  569. False)
  570. for n, wp in enumerate(waypoints):
  571. if ((n + 1) % 400) == 0:
  572. draw_arrow(map_surface, wp.transform)
  573. topology = carla_map.get_topology()
  574. draw_topology(topology, 0)
  575. draw_topology(topology, 1)
  576. if self.show_spawn_points:
  577. for sp in carla_map.get_spawn_points():
  578. draw_arrow(map_surface, sp, color=COLOR_CHOCOLATE_0)
  579. if self.show_connections:
  580. dist = 1.5
  581. to_pixel = lambda wp: world_to_pixel(wp.transform.location)
  582. for wp in carla_map.generate_waypoints(dist):
  583. col = (0, 255, 255) if wp.is_intersection else (0, 255, 0)
  584. for nxt in wp.next(dist):
  585. pygame.draw.line(map_surface, col, to_pixel(wp), to_pixel(nxt), 2)
  586. if wp.lane_change & carla.LaneChange.Right:
  587. r = wp.get_right_lane()
  588. if r and r.lane_type == carla.LaneType.Driving:
  589. pygame.draw.line(map_surface, col, to_pixel(wp), to_pixel(r), 2)
  590. if wp.lane_change & carla.LaneChange.Left:
  591. l = wp.get_left_lane()
  592. if l and l.lane_type == carla.LaneType.Driving:
  593. pygame.draw.line(map_surface, col, to_pixel(wp), to_pixel(l), 2)
  594. actors = carla_world.get_actors()
  595. # Draw Traffic Signs
  596. font_size = world_to_pixel_width(1)
  597. font = pygame.font.SysFont('Arial', font_size, True)
  598. stops = [actor for actor in actors if 'stop' in actor.type_id]
  599. yields = [actor for actor in actors if 'yield' in actor.type_id]
  600. stop_font_surface = font.render("STOP", False, COLOR_ALUMINIUM_2)
  601. stop_font_surface = pygame.transform.scale(
  602. stop_font_surface, (stop_font_surface.get_width(), stop_font_surface.get_height() * 2))
  603. yield_font_surface = font.render("YIELD", False, COLOR_ALUMINIUM_2)
  604. yield_font_surface = pygame.transform.scale(
  605. yield_font_surface, (yield_font_surface.get_width(), yield_font_surface.get_height() * 2))
  606. for ts_stop in stops:
  607. draw_traffic_signs(map_surface, stop_font_surface, ts_stop, trigger_color=COLOR_SCARLET_RED_1)
  608. for ts_yield in yields:
  609. draw_traffic_signs(map_surface, yield_font_surface, ts_yield, trigger_color=COLOR_ORANGE_1)
  610. def world_to_pixel(self, location, offset=(0, 0)):
  611. x = self.scale * self._pixels_per_meter * (location.x - self._world_offset[0])
  612. y = self.scale * self._pixels_per_meter * (location.y - self._world_offset[1])
  613. return [int(x - offset[0]), int(y - offset[1])]
  614. def world_to_pixel_width(self, width):
  615. return int(self.scale * self._pixels_per_meter * width)
  616. def scale_map(self, scale):
  617. if scale != self.scale:
  618. self.scale = scale
  619. width = int(self.big_map_surface.get_width() * self.scale)
  620. self.surface = pygame.transform.smoothscale(self.big_map_surface, (width, width))
  621. class ModuleWorld(object):
  622. def __init__(self, name, args, timeout):
  623. self.client = None
  624. self.name = name
  625. self.args = args
  626. self.timeout = timeout
  627. self.server_fps = 0.0
  628. self.simulation_time = 0
  629. self.server_clock = pygame.time.Clock()
  630. # World data
  631. self.world = None
  632. self.town_map = None
  633. self.actors_with_transforms = []
  634. # Store necessary modules
  635. self.module_hud = None
  636. self.module_input = None
  637. self.surface_size = [0, 0]
  638. self.prev_scaled_size = 0
  639. self.scaled_size = 0
  640. # Hero actor
  641. self.hero_actor = None
  642. self.spawned_hero = None
  643. self.hero_transform = None
  644. self.scale_offset = [0, 0]
  645. self.vehicle_id_surface = None
  646. self.result_surface = None
  647. self.traffic_light_surfaces = TrafficLightSurfaces()
  648. self.affected_traffic_light = None
  649. # Map info
  650. self.map_image = None
  651. self.border_round_surface = None
  652. self.original_surface_size = None
  653. self.hero_surface = None
  654. self.actors_surface = None
  655. def _get_data_from_carla(self):
  656. try:
  657. self.client = carla.Client(self.args.host, self.args.port)
  658. self.client.set_timeout(self.timeout)
  659. if self.args.map is None:
  660. world = self.client.get_world()
  661. else:
  662. world = self.client.load_world(self.args.map)
  663. town_map = world.get_map()
  664. return (world, town_map)
  665. except RuntimeError as ex:
  666. logging.error(ex)
  667. exit_game()
  668. def start(self):
  669. self.world, self.town_map = self._get_data_from_carla()
  670. # Create Surfaces
  671. self.map_image = MapImage(
  672. carla_world=self.world,
  673. carla_map=self.town_map,
  674. pixels_per_meter=PIXELS_PER_METER,
  675. show_triggers=self.args.show_triggers,
  676. show_connections=self.args.show_connections,
  677. show_spawn_points=self.args.show_spawn_points)
  678. # Store necessary modules
  679. self.module_hud = module_manager.get_module(MODULE_HUD)
  680. self.module_input = module_manager.get_module(MODULE_INPUT)
  681. self.original_surface_size = min(self.module_hud.dim[0], self.module_hud.dim[1])
  682. self.surface_size = self.map_image.big_map_surface.get_width()
  683. self.scaled_size = int(self.surface_size)
  684. self.prev_scaled_size = int(self.surface_size)
  685. # Render Actors
  686. self.actors_surface = pygame.Surface((self.map_image.surface.get_width(), self.map_image.surface.get_height()))
  687. self.actors_surface.set_colorkey(COLOR_BLACK)
  688. self.vehicle_id_surface = pygame.Surface((self.surface_size, self.surface_size)).convert()
  689. self.vehicle_id_surface.set_colorkey(COLOR_BLACK)
  690. self.border_round_surface = pygame.Surface(self.module_hud.dim, pygame.SRCALPHA).convert()
  691. self.border_round_surface.set_colorkey(COLOR_WHITE)
  692. self.border_round_surface.fill(COLOR_BLACK)
  693. center_offset = (int(self.module_hud.dim[0] / 2), int(self.module_hud.dim[1] / 2))
  694. pygame.draw.circle(self.border_round_surface, COLOR_ALUMINIUM_1, center_offset, int(self.module_hud.dim[1] / 2))
  695. pygame.draw.circle(self.border_round_surface, COLOR_WHITE, center_offset, int((self.module_hud.dim[1] - 8) / 2))
  696. scaled_original_size = self.original_surface_size * (1.0 / 0.9)
  697. self.hero_surface = pygame.Surface((scaled_original_size, scaled_original_size)).convert()
  698. self.result_surface = pygame.Surface((self.surface_size, self.surface_size)).convert()
  699. self.result_surface.set_colorkey(COLOR_BLACK)
  700. # Start hero mode by default
  701. self.select_hero_actor()
  702. self.hero_actor.set_autopilot(False)
  703. self.module_input.wheel_offset = HERO_DEFAULT_SCALE
  704. self.module_input.control = carla.VehicleControl()
  705. weak_self = weakref.ref(self)
  706. self.world.on_tick(lambda timestamp: ModuleWorld.on_world_tick(weak_self, timestamp))
  707. def select_hero_actor(self):
  708. hero_vehicles = [actor for actor in self.world.get_actors(
  709. ) if 'vehicle' in actor.type_id and actor.attributes['role_name'] == 'hero']
  710. if len(hero_vehicles) > 0:
  711. self.hero_actor = random.choice(hero_vehicles)
  712. self.hero_transform = self.hero_actor.get_transform()
  713. else:
  714. self._spawn_hero()
  715. def _spawn_hero(self):
  716. # Get a random blueprint.
  717. blueprint = random.choice(self.world.get_blueprint_library().filter(self.args.filter))
  718. blueprint.set_attribute('role_name', 'hero')
  719. if blueprint.has_attribute('color'):
  720. color = random.choice(blueprint.get_attribute('color').recommended_values)
  721. blueprint.set_attribute('color', color)
  722. # Spawn the player.
  723. while self.hero_actor is None:
  724. spawn_points = self.world.get_map().get_spawn_points()
  725. spawn_point = random.choice(spawn_points) if spawn_points else carla.Transform()
  726. self.hero_actor = self.world.try_spawn_actor(blueprint, spawn_point)
  727. self.hero_transform = self.hero_actor.get_transform()
  728. # Save it in order to destroy it when closing program
  729. self.spawned_hero = self.hero_actor
  730. def tick(self, clock):
  731. actors = self.world.get_actors()
  732. self.actors_with_transforms = [(actor, actor.get_transform()) for actor in actors]
  733. if self.hero_actor is not None:
  734. self.hero_transform = self.hero_actor.get_transform()
  735. self.update_hud_info(clock)
  736. def update_hud_info(self, clock):
  737. hero_mode_text = []
  738. if self.hero_actor is not None:
  739. hero_speed = self.hero_actor.get_velocity()
  740. hero_speed_text = 3.6 * math.sqrt(hero_speed.x ** 2 + hero_speed.y ** 2 + hero_speed.z ** 2)
  741. affected_traffic_light_text = 'None'
  742. if self.affected_traffic_light is not None:
  743. state = self.affected_traffic_light.state
  744. if state == carla.TrafficLightState.Green:
  745. affected_traffic_light_text = 'GREEN'
  746. elif state == carla.TrafficLightState.Yellow:
  747. affected_traffic_light_text = 'YELLOW'
  748. else:
  749. affected_traffic_light_text = 'RED'
  750. affected_speed_limit_text = self.hero_actor.get_speed_limit()
  751. hero_mode_text = [
  752. 'Hero Mode: ON',
  753. 'Hero ID: %7d' % self.hero_actor.id,
  754. 'Hero Vehicle: %14s' % get_actor_display_name(self.hero_actor, truncate=14),
  755. 'Hero Speed: %3d km/h' % hero_speed_text,
  756. 'Hero Affected by:',
  757. ' Traffic Light: %12s' % affected_traffic_light_text,
  758. ' Speed Limit: %3d km/h' % affected_speed_limit_text
  759. ]
  760. else:
  761. hero_mode_text = ['Hero Mode: OFF']
  762. self.server_fps = self.server_clock.get_fps()
  763. self.server_fps = 'inf' if self.server_fps == float('inf') else round(self.server_fps)
  764. module_info_text = [
  765. 'Server: % 16s FPS' % self.server_fps,
  766. 'Client: % 16s FPS' % round(clock.get_fps()),
  767. 'Simulation Time: % 12s' % datetime.timedelta(seconds=int(self.simulation_time)),
  768. 'Map Name: %10s' % self.town_map.name.split('/')[-1],
  769. ]
  770. module_info_text = module_info_text
  771. module_hud = module_manager.get_module(MODULE_HUD)
  772. module_hud.add_info(self.name, module_info_text)
  773. module_hud.add_info('HERO', hero_mode_text)
  774. @staticmethod
  775. def on_world_tick(weak_self, timestamp):
  776. self = weak_self()
  777. if not self:
  778. return
  779. self.server_clock.tick()
  780. self.server_fps = self.server_clock.get_fps()
  781. self.simulation_time = timestamp.elapsed_seconds
  782. def _split_actors(self):
  783. vehicles = []
  784. traffic_lights = []
  785. speed_limits = []
  786. walkers = []
  787. for actor_with_transform in self.actors_with_transforms:
  788. actor = actor_with_transform[0]
  789. if 'vehicle' in actor.type_id:
  790. vehicles.append(actor_with_transform)
  791. elif 'traffic_light' in actor.type_id:
  792. traffic_lights.append(actor_with_transform)
  793. elif 'speed_limit' in actor.type_id:
  794. speed_limits.append(actor_with_transform)
  795. elif 'walker' in actor.type_id:
  796. walkers.append(actor_with_transform)
  797. info_text = []
  798. if self.hero_actor is not None and len(vehicles) > 1:
  799. location = self.hero_transform.location
  800. vehicle_list = [x[0] for x in vehicles if x[0].id != self.hero_actor.id]
  801. def distance(v): return location.distance(v.get_location())
  802. for n, vehicle in enumerate(sorted(vehicle_list, key=distance)):
  803. if n > 15:
  804. break
  805. vehicle_type = get_actor_display_name(vehicle, truncate=22)
  806. info_text.append('% 5d %s' % (vehicle.id, vehicle_type))
  807. module_manager.get_module(MODULE_HUD).add_info(
  808. 'NEARBY VEHICLES',
  809. info_text)
  810. return (vehicles, traffic_lights, speed_limits, walkers)
  811. def _render_traffic_lights(self, surface, list_tl, world_to_pixel):
  812. self.affected_traffic_light = None
  813. for tl in list_tl:
  814. world_pos = tl.get_location()
  815. pos = world_to_pixel(world_pos)
  816. if self.args.show_triggers:
  817. corners = Util.get_bounding_box(tl)
  818. corners = [world_to_pixel(p) for p in corners]
  819. pygame.draw.lines(surface, COLOR_BUTTER_1, True, corners, 2)
  820. if self.hero_actor is not None:
  821. corners = Util.get_bounding_box(tl)
  822. corners = [world_to_pixel(p) for p in corners]
  823. tl_t = tl.get_transform()
  824. transformed_tv = tl_t.transform(tl.trigger_volume.location)
  825. hero_location = self.hero_actor.get_location()
  826. d = hero_location.distance(transformed_tv)
  827. s = Util.length(tl.trigger_volume.extent) + Util.length(self.hero_actor.bounding_box.extent)
  828. if (d <= s):
  829. # Highlight traffic light
  830. self.affected_traffic_light = tl
  831. srf = self.traffic_light_surfaces.surfaces['h']
  832. surface.blit(srf, srf.get_rect(center=pos))
  833. srf = self.traffic_light_surfaces.surfaces[tl.state]
  834. surface.blit(srf, srf.get_rect(center=pos))
  835. def _render_speed_limits(self, surface, list_sl, world_to_pixel, world_to_pixel_width):
  836. font_size = world_to_pixel_width(2)
  837. radius = world_to_pixel_width(2)
  838. font = pygame.font.SysFont('Arial', font_size)
  839. for sl in list_sl:
  840. x, y = world_to_pixel(sl.get_location())
  841. # Render speed limit
  842. white_circle_radius = int(radius * 0.75)
  843. pygame.draw.circle(surface, COLOR_SCARLET_RED_1, (x, y), radius)
  844. pygame.draw.circle(surface, COLOR_ALUMINIUM_0, (x, y), white_circle_radius)
  845. limit = sl.type_id.split('.')[2]
  846. font_surface = font.render(limit, True, COLOR_ALUMINIUM_5)
  847. if self.args.show_triggers:
  848. corners = Util.get_bounding_box(sl)
  849. corners = [world_to_pixel(p) for p in corners]
  850. pygame.draw.lines(surface, COLOR_PLUM_2, True, corners, 2)
  851. # Blit
  852. if self.hero_actor is not None:
  853. # Rotate font surface with respect to hero vehicle front
  854. angle = -self.hero_transform.rotation.yaw - 90.0
  855. font_surface = pygame.transform.rotate(font_surface, angle)
  856. offset = font_surface.get_rect(center=(x, y))
  857. surface.blit(font_surface, offset)
  858. else:
  859. surface.blit(font_surface, (x - radius / 2, y - radius / 2))
  860. def _render_walkers(self, surface, list_w, world_to_pixel):
  861. for w in list_w:
  862. color = COLOR_PLUM_0
  863. # Compute bounding box points
  864. bb = w[0].bounding_box.extent
  865. corners = [
  866. carla.Location(x=-bb.x, y=-bb.y),
  867. carla.Location(x=bb.x, y=-bb.y),
  868. carla.Location(x=bb.x, y=bb.y),
  869. carla.Location(x=-bb.x, y=bb.y)]
  870. w[1].transform(corners)
  871. corners = [world_to_pixel(p) for p in corners]
  872. pygame.draw.polygon(surface, color, corners)
  873. def _render_vehicles(self, surface, list_v, world_to_pixel):
  874. for v in list_v:
  875. color = COLOR_SKY_BLUE_0
  876. if int(v[0].attributes['number_of_wheels']) == 2:
  877. color = COLOR_CHOCOLATE_1
  878. if v[0].attributes['role_name'] == 'hero':
  879. color = COLOR_CHAMELEON_0
  880. # Compute bounding box points
  881. bb = v[0].bounding_box.extent
  882. corners = [carla.Location(x=-bb.x, y=-bb.y),
  883. carla.Location(x=bb.x - 0.8, y=-bb.y),
  884. carla.Location(x=bb.x, y=0),
  885. carla.Location(x=bb.x - 0.8, y=bb.y),
  886. carla.Location(x=-bb.x, y=bb.y),
  887. carla.Location(x=-bb.x, y=-bb.y)
  888. ]
  889. v[1].transform(corners)
  890. corners = [world_to_pixel(p) for p in corners]
  891. pygame.draw.lines(surface, color, False, corners, int(math.ceil(4.0 * self.map_image.scale)))
  892. def render_actors(self, surface, vehicles, traffic_lights, speed_limits, walkers):
  893. # Static actors
  894. self._render_traffic_lights(surface, [tl[0] for tl in traffic_lights], self.map_image.world_to_pixel)
  895. self._render_speed_limits(surface, [sl[0] for sl in speed_limits], self.map_image.world_to_pixel,
  896. self.map_image.world_to_pixel_width)
  897. # Dynamic actors
  898. self._render_vehicles(surface, vehicles, self.map_image.world_to_pixel)
  899. self._render_walkers(surface, walkers, self.map_image.world_to_pixel)
  900. def clip_surfaces(self, clipping_rect):
  901. self.actors_surface.set_clip(clipping_rect)
  902. self.vehicle_id_surface.set_clip(clipping_rect)
  903. self.result_surface.set_clip(clipping_rect)
  904. def _compute_scale(self, scale_factor):
  905. m = self.module_input.mouse_pos
  906. # Percentage of surface where mouse position is actually
  907. px = (m[0] - self.scale_offset[0]) / float(self.prev_scaled_size)
  908. py = (m[1] - self.scale_offset[1]) / float(self.prev_scaled_size)
  909. # Offset will be the previously accumulated offset added with the
  910. # difference of mouse positions in the old and new scales
  911. diff_between_scales = ((float(self.prev_scaled_size) * px) - (float(self.scaled_size) * px),
  912. (float(self.prev_scaled_size) * py) - (float(self.scaled_size) * py))
  913. self.scale_offset = (self.scale_offset[0] + diff_between_scales[0],
  914. self.scale_offset[1] + diff_between_scales[1])
  915. # Update previous scale
  916. self.prev_scaled_size = self.scaled_size
  917. # Scale performed
  918. self.map_image.scale_map(scale_factor)
  919. def render(self, display):
  920. if self.actors_with_transforms is None:
  921. return
  922. self.result_surface.fill(COLOR_BLACK)
  923. vehicles, traffic_lights, speed_limits, walkers = self._split_actors()
  924. scale_factor = self.module_input.wheel_offset
  925. self.scaled_size = int(self.map_image.width * scale_factor)
  926. if self.scaled_size != self.prev_scaled_size:
  927. self._compute_scale(scale_factor)
  928. # Render Actors
  929. self.actors_surface.fill(COLOR_BLACK)
  930. self.render_actors(
  931. self.actors_surface,
  932. vehicles,
  933. traffic_lights,
  934. speed_limits,
  935. walkers)
  936. # Render Ids
  937. self.module_hud.render_vehicles_ids(self.vehicle_id_surface, vehicles,
  938. self.map_image.world_to_pixel, self.hero_actor, self.hero_transform)
  939. # Blit surfaces
  940. surfaces = ((self.map_image.surface, (0, 0)),
  941. (self.actors_surface, (0, 0)),
  942. (self.vehicle_id_surface, (0, 0)),
  943. )
  944. angle = 0.0 if self.hero_actor is None else self.hero_transform.rotation.yaw + 90.0
  945. self.traffic_light_surfaces.rotozoom(-angle, self.map_image.scale)
  946. center_offset = (0, 0)
  947. if self.hero_actor is not None:
  948. hero_location_screen = self.map_image.world_to_pixel(self.hero_transform.location)
  949. hero_front = self.hero_transform.get_forward_vector()
  950. translation_offset = (
  951. hero_location_screen[0] -
  952. self.hero_surface.get_width() /
  953. 2 +
  954. hero_front.x *
  955. PIXELS_AHEAD_VEHICLE,
  956. (hero_location_screen[1] -
  957. self.hero_surface.get_height() /
  958. 2 +
  959. hero_front.y *
  960. PIXELS_AHEAD_VEHICLE))
  961. # Apply clipping rect
  962. clipping_rect = pygame.Rect(translation_offset[0],
  963. translation_offset[1],
  964. self.hero_surface.get_width(),
  965. self.hero_surface.get_height())
  966. self.clip_surfaces(clipping_rect)
  967. Util.blits(self.result_surface, surfaces)
  968. self.border_round_surface.set_clip(clipping_rect)
  969. self.hero_surface.fill(COLOR_ALUMINIUM_4)
  970. self.hero_surface.blit(self.result_surface, (-translation_offset[0],
  971. -translation_offset[1]))
  972. rotated_result_surface = pygame.transform.rotozoom(self.hero_surface, angle, 0.9).convert()
  973. center = (display.get_width() / 2, display.get_height() / 2)
  974. rotation_pivot = rotated_result_surface.get_rect(center=center)
  975. display.blit(rotated_result_surface, rotation_pivot)
  976. display.blit(self.border_round_surface, (0, 0))
  977. else:
  978. # Translation offset
  979. translation_offset = (self.module_input.mouse_offset[0] * scale_factor + self.scale_offset[0],
  980. self.module_input.mouse_offset[1] * scale_factor + self.scale_offset[1])
  981. center_offset = (abs(display.get_width() - self.surface_size) / 2 * scale_factor, 0)
  982. # Apply clipping rect
  983. clipping_rect = pygame.Rect(-translation_offset[0] - center_offset[0], -translation_offset[1],
  984. self.module_hud.dim[0], self.module_hud.dim[1])
  985. self.clip_surfaces(clipping_rect)
  986. Util.blits(self.result_surface, surfaces)
  987. display.blit(self.result_surface, (translation_offset[0] + center_offset[0],
  988. translation_offset[1]))
  989. def destroy(self):
  990. if self.spawned_hero is not None:
  991. self.spawned_hero.destroy()
  992. # ==============================================================================
  993. # -- Input -----------------------------------------------------------
  994. # ==============================================================================
  995. class ModuleInput(object):
  996. def __init__(self, name):
  997. self.name = name
  998. self.mouse_pos = (0, 0)
  999. self.mouse_offset = [0.0, 0.0]
  1000. self.wheel_offset = 0.1
  1001. self.wheel_amount = 0.025
  1002. self._steer_cache = 0.0
  1003. self.control = None
  1004. self._autopilot_enabled = False
  1005. def start(self):
  1006. hud = module_manager.get_module(MODULE_HUD)
  1007. hud.notification("Press 'H' or '?' for help.", seconds=4.0)
  1008. def render(self, display):
  1009. pass
  1010. def tick(self, clock):
  1011. self.parse_input(clock)
  1012. def _parse_events(self):
  1013. self.mouse_pos = pygame.mouse.get_pos()
  1014. for event in pygame.event.get():
  1015. if event.type == pygame.QUIT:
  1016. exit_game()
  1017. elif event.type == pygame.KEYUP:
  1018. if self._is_quit_shortcut(event.key):
  1019. exit_game()
  1020. elif event.key == K_h or (event.key == K_SLASH and pygame.key.get_mods() & KMOD_SHIFT):
  1021. module_hud = module_manager.get_module(MODULE_HUD)
  1022. module_hud.help.toggle()
  1023. elif event.key == K_TAB:
  1024. module_world = module_manager.get_module(MODULE_WORLD)
  1025. module_hud = module_manager.get_module(MODULE_HUD)
  1026. if module_world.hero_actor is None:
  1027. module_world.select_hero_actor()
  1028. self.wheel_offset = HERO_DEFAULT_SCALE
  1029. self.control = carla.VehicleControl()
  1030. module_hud.notification('Hero Mode')
  1031. else:
  1032. self.wheel_offset = MAP_DEFAULT_SCALE
  1033. self.mouse_offset = [0, 0]
  1034. self.mouse_pos = [0, 0]
  1035. module_world.scale_offset = [0, 0]
  1036. module_world.hero_actor = None
  1037. module_hud.notification('Map Mode')
  1038. elif event.key == K_F1:
  1039. module_hud = module_manager.get_module(MODULE_HUD)
  1040. module_hud.show_info = not module_hud.show_info
  1041. elif event.key == K_i:
  1042. module_hud = module_manager.get_module(MODULE_HUD)
  1043. module_hud.show_actor_ids = not module_hud.show_actor_ids
  1044. elif isinstance(self.control, carla.VehicleControl):
  1045. if event.key == K_q:
  1046. self.control.gear = 1 if self.control.reverse else -1
  1047. elif event.key == K_m:
  1048. self.control.manual_gear_shift = not self.control.manual_gear_shift
  1049. world = module_manager.get_module(MODULE_WORLD)
  1050. self.control.gear = world.hero_actor.get_control().gear
  1051. module_hud = module_manager.get_module(MODULE_HUD)
  1052. module_hud.notification('%s Transmission' % (
  1053. 'Manual' if self.control.manual_gear_shift else 'Automatic'))
  1054. elif self.control.manual_gear_shift and event.key == K_COMMA:
  1055. self.control.gear = max(-1, self.control.gear - 1)
  1056. elif self.control.manual_gear_shift and event.key == K_PERIOD:
  1057. self.control.gear = self.control.gear + 1
  1058. elif event.key == K_p:
  1059. world = module_manager.get_module(MODULE_WORLD)
  1060. if world.hero_actor is not None:
  1061. self._autopilot_enabled = not self._autopilot_enabled
  1062. world.hero_actor.set_autopilot(self._autopilot_enabled)
  1063. module_hud = module_manager.get_module(MODULE_HUD)
  1064. module_hud.notification('Autopilot %s' % ('On' if self._autopilot_enabled else 'Off'))
  1065. elif event.type == pygame.MOUSEBUTTONDOWN:
  1066. if event.button == 4:
  1067. self.wheel_offset += self.wheel_amount
  1068. if self.wheel_offset >= 1.0:
  1069. self.wheel_offset = 1.0
  1070. elif event.button == 5:
  1071. self.wheel_offset -= self.wheel_amount
  1072. if self.wheel_offset <= 0.1:
  1073. self.wheel_offset = 0.1
  1074. def _parse_keys(self, milliseconds):
  1075. keys = pygame.key.get_pressed()
  1076. self.control.throttle = 1.0 if keys[K_UP] or keys[K_w] else 0.0
  1077. steer_increment = 5e-4 * milliseconds
  1078. if keys[K_LEFT] or keys[K_a]:
  1079. self._steer_cache -= steer_increment
  1080. elif keys[K_RIGHT] or keys[K_d]:
  1081. self._steer_cache += steer_increment
  1082. else:
  1083. self._steer_cache = 0.0
  1084. self._steer_cache = min(0.7, max(-0.7, self._steer_cache))
  1085. self.control.steer = round(self._steer_cache, 1)
  1086. self.control.brake = 1.0 if keys[K_DOWN] or keys[K_s] else 0.0
  1087. self.control.hand_brake = keys[K_SPACE]
  1088. def _parse_mouse(self):
  1089. if pygame.mouse.get_pressed()[0]:
  1090. x, y = pygame.mouse.get_pos()
  1091. self.mouse_offset[0] += (1.0 / self.wheel_offset) * (x - self.mouse_pos[0])
  1092. self.mouse_offset[1] += (1.0 / self.wheel_offset) * (y - self.mouse_pos[1])
  1093. self.mouse_pos = (x, y)
  1094. def parse_input(self, clock):
  1095. self._parse_events()
  1096. self._parse_mouse()
  1097. if not self._autopilot_enabled:
  1098. if isinstance(self.control, carla.VehicleControl):
  1099. self._parse_keys(clock.get_time())
  1100. self.control.reverse = self.control.gear < 0
  1101. world = module_manager.get_module(MODULE_WORLD)
  1102. if (world.hero_actor is not None):
  1103. world.hero_actor.apply_control(self.control)
  1104. @staticmethod
  1105. def _is_quit_shortcut(key):
  1106. return (key == K_ESCAPE) or (key == K_q and pygame.key.get_mods() & KMOD_CTRL)
  1107. # ==============================================================================
  1108. # -- Global Objects ------------------------------------------------------------
  1109. # ==============================================================================
  1110. module_manager = ModuleManager()
  1111. # ==============================================================================
  1112. # -- Game Loop ---------------------------------------------------------------
  1113. # ==============================================================================
  1114. def game_loop(args):
  1115. try:
  1116. # Init Pygame
  1117. pygame.init()
  1118. display = pygame.display.set_mode(
  1119. (args.width, args.height),
  1120. pygame.HWSURFACE | pygame.DOUBLEBUF)
  1121. pygame.display.set_caption(args.description)
  1122. font = pygame.font.Font(pygame.font.get_default_font(), 20)
  1123. text_surface = font.render('Rendering map...', True, COLOR_WHITE)
  1124. display.blit(text_surface, text_surface.get_rect(center=(args.width / 2, args.height / 2)))
  1125. pygame.display.flip()
  1126. # Init modules
  1127. input_module = ModuleInput(MODULE_INPUT)
  1128. hud_module = ModuleHUD(MODULE_HUD, args.width, args.height)
  1129. world_module = ModuleWorld(MODULE_WORLD, args, timeout=2.0)
  1130. # Register Modules
  1131. module_manager.register_module(world_module)
  1132. module_manager.register_module(hud_module)
  1133. module_manager.register_module(input_module)
  1134. module_manager.start_modules()
  1135. clock = pygame.time.Clock()
  1136. while True:
  1137. clock.tick_busy_loop(60)
  1138. module_manager.tick(clock)
  1139. module_manager.render(display)
  1140. pygame.display.flip()
  1141. except KeyboardInterrupt:
  1142. print('\nCancelled by user. Bye!')
  1143. finally:
  1144. if world_module is not None:
  1145. world_module.destroy()
  1146. def exit_game():
  1147. module_manager.clear_modules()
  1148. pygame.quit()
  1149. sys.exit()
  1150. # ==============================================================================
  1151. # -- Main --------------------------------------------------------------------
  1152. # ==============================================================================
  1153. def main():
  1154. # Parse arguments
  1155. argparser = argparse.ArgumentParser(
  1156. description='CARLA No Rendering Mode Visualizer')
  1157. argparser.add_argument(
  1158. '-v', '--verbose',
  1159. action='store_true',
  1160. dest='debug',
  1161. help='print debug information')
  1162. argparser.add_argument(
  1163. '--host',
  1164. metavar='H',
  1165. default='127.0.0.1',
  1166. help='IP of the host server (default: 127.0.0.1)')
  1167. argparser.add_argument(
  1168. '-p', '--port',
  1169. metavar='P',
  1170. default=2000,
  1171. type=int,
  1172. help='TCP port to listen to (default: 2000)')
  1173. argparser.add_argument(
  1174. '--res',
  1175. metavar='WIDTHxHEIGHT',
  1176. default='1280x720',
  1177. help='window resolution (default: 1280x720)')
  1178. argparser.add_argument(
  1179. '--filter',
  1180. metavar='PATTERN',
  1181. default='vehicle.*',
  1182. help='actor filter (default: "vehicle.*")')
  1183. argparser.add_argument(
  1184. '--map',
  1185. metavar='TOWN',
  1186. default=None,
  1187. help='start a new episode at the given TOWN')
  1188. argparser.add_argument(
  1189. '--no-rendering',
  1190. action='store_true',
  1191. default=True,
  1192. help='switch off server rendering')
  1193. argparser.add_argument(
  1194. '--show-triggers',
  1195. action='store_true',
  1196. help='show trigger boxes of traffic signs')
  1197. argparser.add_argument(
  1198. '--show-connections',
  1199. action='store_true',
  1200. help='show waypoint connections')
  1201. argparser.add_argument(
  1202. '--show-spawn-points',
  1203. action='store_true',
  1204. help='show recommended spawn points')
  1205. args = argparser.parse_args()
  1206. args.description = argparser.description
  1207. args.width, args.height = [int(x) for x in args.res.split('x')]
  1208. log_level = logging.DEBUG if args.debug else logging.INFO
  1209. logging.basicConfig(format='%(levelname)s: %(message)s', level=log_level)
  1210. logging.info('listening to server %s:%s', args.host, args.port)
  1211. print(__doc__)
  1212. game_loop(args)
  1213. if __name__ == '__main__':
  1214. main()