diff --git a/src/carla_imu/example/data_reciever.py b/src/carla_imu/example/data_reciever.py index 5fed640759..533bd7c340 100644 --- a/src/carla_imu/example/data_reciever.py +++ b/src/carla_imu/example/data_reciever.py @@ -7,32 +7,10 @@ S : brake A/D : steer left/right Q : toggle reverse - Space : hand-brake + Space : hand-brake / 切换中英文 P : toggle autopilot M : toggle manual transmission ,/. : gear up/down - - L : toggle next light type - SHIFT + L : toggle high beam - Z/X : toggle right/left blinker - I : toggle interior light - - TAB : change sensor position - ` or N : next sensor - [1-9] : change to sensor [1-9] - G : toggle radar visualization - C : change weather (Shift+C reverse) - Backspace : change vehicle - - R : toggle recording images to disk - - CTRL + R : toggle recording of simulation (replacing any previous) - CTRL + P : start replaying last recorded simulation - CTRL + + : increments the start time of the replay by 1 second (+SHIFT = 10 seconds) - CTRL + - : decrements the start time of the replay by 1 second (+SHIFT = 10 seconds) - - F1 : toggle HUD - H/? : toggle help ESC : quit """ @@ -51,13 +29,9 @@ pass import carla -from carla import ColorConverter as cc - import argparse -import collections import math import random -import re import weakref try: @@ -71,89 +45,73 @@ except ImportError: raise RuntimeError('cannot import numpy, make sure numpy package is installed') - -# ============================================================================== -# -- Global functions ---------------------------------------------------------- -# ============================================================================== - -def find_weather_presets(): - rgx = re.compile('.+?(?:(?<=[a-z])(?=[A-Z])|(?<=[A-Z])(?=[A-Z][a-z])|$)') - name = lambda x: ' '.join(m.group(0) for m in rgx.finditer(x)) - presets = [x for x in dir(carla.WeatherParameters) if re.match('[A-Z].+', x)] - return [(getattr(carla.WeatherParameters, x), name(x)) for x in presets] - +# ====================== 多语言配置 ====================== +LANG = "en" +TEXTS = { + "en": { + "server": "Server", + "client": "Client", + "vehicle": "Vehicle", + "map": "Map", + "speed": "Speed", + "autopilot": "AUTOPILOT ENABLED", + "fps": "FPS", + "kmh": "km/h" + }, + "cn": { + "server": "服务器", + "client": "客户端", + "vehicle": "车辆", + "map": "地图", + "speed": "速度", + "autopilot": "自动驾驶已启用", + "fps": "帧率", + "kmh": "千米/小时" + } +} + +def t(key): + return TEXTS[LANG].get(key, key) + +# ====================== 工具函数 ====================== def get_actor_display_name(actor, truncate=250): name = ' '.join(actor.type_id.replace('_', '.').title().split('.')[1:]) - return (name[:truncate - 1] + u'\u2026') if len(name) > truncate else name - - -# ============================================================================== -# -- World --------------------------------------------------------------------- -# ============================================================================== + return (name[:truncate-1] + "…") if len(name) > truncate else name +# ====================== 世界管理 ====================== class World(object): def __init__(self, carla_world, hud, args): self.world = carla_world - self.actor_role_name = args.rolename - try: - self.map = self.world.get_map() - except RuntimeError as error: - print('RuntimeError: {}'.format(error)) - sys.exit(1) + self.map = self.world.get_map() self.hud = hud self.player = None self.collision_sensor = None - self.lane_invasion_sensor = None self.gnss_sensor = None self.imu_sensor = None - self.radar_sensor = None self.camera_manager = None - self._weather_presets = find_weather_presets() - self._weather_index = 0 self._actor_filter = args.filter - self._gamma = args.gamma self.restart() - self.world.on_tick(hud.on_world_tick) def restart(self): - self.player_max_speed = 1.589 - cam_index = self.camera_manager.index if self.camera_manager is not None else 0 - cam_pos_index = self.camera_manager.transform_index if self.camera_manager is not None else 0 blueprint = random.choice(self.world.get_blueprint_library().filter(self._actor_filter)) - blueprint.set_attribute('role_name', self.actor_role_name) + blueprint.set_attribute('role_name', "hero") if blueprint.has_attribute('color'): color = random.choice(blueprint.get_attribute('color').recommended_values) blueprint.set_attribute('color', color) - if blueprint.has_attribute('is_invincible'): - blueprint.set_attribute('is_invincible', 'true') - if blueprint.has_attribute('speed'): - try: - self.player_max_speed = float(blueprint.get_attribute('speed').recommended_values[1]) - except: - print("No recommended values for 'speed' attribute") - else: - print("No recommended values for 'speed' attribute") + if self.player is not None: - spawn_point = self.player.get_transform() - spawn_point.location.z += 2.0 - spawn_point.rotation.roll = 0.0 - spawn_point.rotation.pitch = 0.0 - self.destroy() - self.player = self.world.try_spawn_actor(blueprint, spawn_point) - while self.player is None: - spawn_points = self.map.get_spawn_points() - spawn_point = spawn_points[45] if spawn_points else carla.Transform() - self.player = self.world.try_spawn_actor(blueprint, spawn_point) - + self.player.destroy() + + spawn_points = self.map.get_spawn_points() + spawn_point = spawn_points[0] if spawn_points else carla.Transform() + self.player = self.world.spawn_actor(blueprint, spawn_point) self.player.set_autopilot(True) - self.collision_sensor = CollisionSensor(self.player, self.hud) - self.lane_invasion_sensor = LaneInvasionSensor(self.player, self.hud) + self.collision_sensor = CollisionSensor(self.player) self.gnss_sensor = GnssSensor(self.player) self.imu_sensor = IMUSensor(self.player) - self.camera_manager = CameraManager(self.player, self.hud, self._gamma) - actor_type = get_actor_display_name(self.player) - self.hud.notification(actor_type) + self.camera_manager = CameraManager(self.player) + self.hud.notification(t("autopilot")) def tick(self, clock): self.hud.tick(self, clock) @@ -163,315 +121,179 @@ def render(self, display): self.hud.render(display) def destroy(self): - actors = [ - self.camera_manager.sensor, - self.collision_sensor.sensor, - self.lane_invasion_sensor.sensor, - self.gnss_sensor.sensor, - self.imu_sensor.sensor, - self.player] - for actor in actors: - if actor is not None: - actor.destroy() - - -# ============================================================================== -# -- KeyboardControl ----------------------------------------------------------- -# ============================================================================== - + if self.player: + self.player.destroy() + if self.collision_sensor.sensor: + self.collision_sensor.sensor.destroy() + if self.gnss_sensor.sensor: + self.gnss_sensor.sensor.destroy() + if self.imu_sensor.sensor: + self.imu_sensor.sensor.destroy() + if self.camera_manager.sensor: + self.camera_manager.sensor.destroy() + +# ====================== 按键控制 ====================== class KeyboardControl(object): - def __init__(self, world, start_in_autopilot): - self._autopilot_enabled = True - world.player.set_autopilot(True) - world.hud.notification("AUTOPILOT ENABLED - AI DRIVING", seconds=3.0) - - def parse_events(self, client, world, clock): + def parse_events(self, world): + global LANG for event in pygame.event.get(): if event.type == pygame.QUIT: return True - elif event.type == pygame.KEYUP: + if event.type == pygame.KEYUP: if event.key == K_ESCAPE: return True - elif event.type == MOUSEMOTION and pygame.mouse.get_pressed()[0]: + # 空格键切换中英文 + if event.key == K_SPACE: + LANG = "cn" if LANG == "en" else "en" + tip = "已切换为中文" if LANG=="cn" else "Switched to English" + world.hud.notification(tip) + # 鼠标拖拽旋转视角 + if event.type == MOUSEMOTION and pygame.mouse.get_pressed()[0]: dx, dy = event.rel world.camera_manager.yaw += dx * 0.2 world.camera_manager.pitch = max(-60, min(60, world.camera_manager.pitch - dy * 0.2)) return False - -# ============================================================================== -# -- HUD ----------------------------------------------------------------------- -# ============================================================================== - +# ====================== HUD界面（已修复中文字体） ====================== class HUD(object): def __init__(self, width, height): self.dim = (width, height) - font = pygame.font.Font(pygame.font.get_default_font(), 20) - font_name = 'courier' if os.name == 'nt' else 'mono' - fonts = [x for x in pygame.font.get_fonts() if font_name in x] - default_font = 'ubuntumono' - mono = default_font if default_font in fonts else fonts[0] - mono = pygame.font.match_font(mono) - self._font_mono = pygame.font.Font(mono, 12 if os.name == 'nt' else 14) - self._notifications = FadingText(font, (width, 40), (0, height - 40)) - self.help = HelpText(pygame.font.Font(mono, 16), width, height) + # 方法1：强制加载Windows微软雅黑，完美支持中文 + font_path = r"C:\Windows\Fonts\msyh.ttc" + self._font = pygame.font.Font(font_path, 14) self.server_fps = 0 self.frame = 0 - self._show_info = True self._info_text = [] - self._server_clock = pygame.time.Clock() + self._clock = pygame.time.Clock() + self.notice_text = "" + self.notice_time = 0 - def on_world_tick(self, timestamp): - self._server_clock.tick() - self.server_fps = self._server_clock.get_fps() - self.frame = timestamp.frame + def notification(self, text, dur=2.0): + self.notice_text = text + self.notice_time = dur def tick(self, world, clock): - self._notifications.tick(world, clock) - if not self._show_info: - return + self._clock.tick() + self.server_fps = self._clock.get_fps() + if self.notice_time > 0: + self.notice_time -= 0.016 + v = world.player.get_velocity() + # 修复：Python3.7 hypot 只支持2个参数 speed = 3.6 * math.sqrt(v.x**2 + v.y**2 + v.z**2) self._info_text = [ - 'Server: % 16.0f FPS' % self.server_fps, - 'Client: % 16.0f FPS' % clock.get_fps(), - '', - 'Vehicle: % 20s' % get_actor_display_name(world.player, truncate=20), - 'Map: % 20s' % world.map.name, - 'Speed: % 15.0f km/h' % speed, + f"{t('server')}: {self.server_fps:.1f} {t('fps')}", + f"{t('client')}: {clock.get_fps():.1f} {t('fps')}", + "", + f"{t('vehicle')}: {get_actor_display_name(world.player,20)}", + f"{t('map')}: {world.map.name}", + f"{t('speed')}: {speed:.1f} {t('kmh')}" ] - def notification(self, text, seconds=2.0): - self._notifications.set_text(text, seconds=seconds) - - def render(self, display): - if self._show_info: - info_surface = pygame.Surface((220, self.dim[1])) - info_surface.set_alpha(100) - display.blit(info_surface, (0, 0)) - v_offset = 4 - for item in self._info_text: - if v_offset + 18 > self.dim[1]: - break - if isinstance(item, str): - surface = self._font_mono.render(item, True, (255, 255, 255)) - display.blit(surface, (8, v_offset)) - v_offset += 18 - self._notifications.render(display) - self.help.render(display) - - -# ============================================================================== -# -- FadingText ---------------------------------------------------------------- -# ============================================================================== - -class FadingText(object): - def __init__(self, font, dim, pos): - self.font = font - self.dim = dim - self.pos = pos - self.seconds_left = 0 - self.surface = pygame.Surface(self.dim) - - def set_text(self, text, color=(255, 255, 255), seconds=2.0): - text_texture = self.font.render(text, True, color) - self.surface = pygame.Surface(self.dim) - self.seconds_left = seconds - self.surface.fill((0, 0, 0, 0)) - self.surface.blit(text_texture, (10, 11)) - - def tick(self, _, clock): - delta_seconds = 1e-3 * clock.get_time() - self.seconds_left = max(0.0, self.seconds_left - delta_seconds) - self.surface.set_alpha(500.0 * self.seconds_left) - def render(self, display): - display.blit(self.surface, self.pos) - - -# ============================================================================== -# -- HelpText ------------------------------------------------------------------ -# ============================================================================== - -class HelpText(object): - def __init__(self, font, width, height): - lines = __doc__.split('\n') - self.font = font - self.line_space = 18 - self.dim = (780, len(lines) * self.line_space + 12) - self.pos = (0.5 * width - 0.5 * self.dim[0], 0.5 * height - 0.5 * self.dim[1]) - self._render = False - self.surface = pygame.Surface(self.dim) - self.surface.fill((0, 0, 0, 0)) - for n, line in enumerate(lines): - text_texture = self.font.render(line, True, (255, 255, 255)) - self.surface.blit(text_texture, (22, n * self.line_space)) - self.surface.set_alpha(220) - - def toggle(self): - self._render = not self._render - - def render(self, display): - if self._render: - display.blit(self.surface, self.pos) - - -# ============================================================================== -# -- Sensors ------------------------------------------------------------------- -# ============================================================================== - -class CollisionSensor(object): - def __init__(self, parent_actor, hud): - self.sensor = None - self._parent = parent_actor - world = self._parent.get_world() - bp = world.get_blueprint_library().find('sensor.other.collision') - self.sensor = world.spawn_actor(bp, carla.Transform(), attach_to=self._parent) - weak_self = weakref.ref(self) - self.sensor.listen(lambda event: CollisionSensor._on_collision(weak_self, event)) - @staticmethod - def _on_collision(weak_self, event): - pass - -class LaneInvasionSensor(object): - def __init__(self, parent_actor, hud): - self.sensor = None - self._parent = parent_actor - world = self._parent.get_world() - bp = world.get_blueprint_library().find('sensor.other.lane_invasion') - self.sensor = world.spawn_actor(bp, carla.Transform(), attach_to=self._parent) - weak_self = weakref.ref(self) - self.sensor.listen(lambda event: LaneInvasionSensor._on_invasion(weak_self, event)) - @staticmethod - def _on_invasion(weak_self, event): - pass - -class GnssSensor(object): - def __init__(self, parent_actor): - self.sensor = None - self._parent = parent_actor - self.lat = 0.0 - self.lon = 0.0 - world = self._parent.get_world() - bp = world.get_blueprint_library().find('sensor.other.gnss') - self.sensor = world.spawn_actor(bp, carla.Transform(carla.Location(x=1.0, z=2.8)), attach_to=self._parent) - weak_self = weakref.ref(self) - self.sensor.listen(lambda event: GnssSensor._on_gnss_event(weak_self, event)) - @staticmethod - def _on_gnss_event(weak_self, event): - self = weak_self() - self.lat = event.latitude - self.lon = event.longitude - -class IMUSensor(object): - def __init__(self, parent_actor): - self.sensor = None - self._parent = parent_actor - self.accelerometer = (0.0, 0.0, 0.0) - self.gyroscope = (0.0, 0.0, 0.0) - self.compass = 0.0 - world = self._parent.get_world() - bp = world.get_blueprint_library().find('sensor.other.imu') - self.sensor = world.spawn_actor(bp, carla.Transform(), attach_to=self._parent) - weak_self = weakref.ref(self) - self.sensor.listen(lambda data: IMUSensor._IMU_callback(weak_self, data)) - @staticmethod - def _IMU_callback(weak_self, sensor_data): - self = weak_self() - if not self: return - lim = (-99.9, 99.9) - self.accelerometer = ( - max(lim[0], min(lim[1], sensor_data.accelerometer.x)), - max(lim[0], min(lim[1], sensor_data.accelerometer.y)), - max(lim[0], min(lim[1], sensor_data.accelerometer.z))) - self.gyroscope = ( - max(lim[0], min(lim[1], math.degrees(sensor_data.gyroscope.x))), - max(lim[0], min(lim[1], math.degrees(sensor_data.gyroscope.y))), - max(lim[0], min(lim[1], math.degrees(sensor_data.gyroscope.z)))) - self.compass = math.degrees(sensor_data.compass) - - -# ============================================================================== -# -- CameraManager（车载可旋转视角）--------------------------------------------- -# ============================================================================== - -class CameraManager(object): - def __init__(self, parent_actor, hud, gamma_correction): + # 半透明黑底 + bg = pygame.Surface((260, self.dim[1])) + bg.set_alpha(80) + display.blit(bg, (0,0)) + + y = 5 + for line in self._info_text: + render = self._font.render(line, True, (255,255,255)) + display.blit(render, (10, y)) + y += 20 + + # 提示文字 + if self.notice_time > 0: + tip = self._font.render(self.notice_text, True, (255,255,0)) + display.blit(tip, (self.dim[0]//2 - 100, 80)) + +# ====================== 传感器 ====================== +class CollisionSensor: + def __init__(self, parent): + self.sensor = parent.get_world().spawn_actor( + parent.get_world().get_blueprint_library().find("sensor.other.collision"), + carla.Transform(), attach_to=parent) + +class GnssSensor: + def __init__(self, parent): + self.sensor = parent.get_world().spawn_actor( + parent.get_world().get_blueprint_library().find("sensor.other.gnss"), + carla.Transform(carla.Location(x=1.0, z=2.8)), attach_to=parent) + +class IMUSensor: + def __init__(self, parent): + self.sensor = parent.get_world().spawn_actor( + parent.get_world().get_blueprint_library().find("sensor.other.imu"), + carla.Transform(), attach_to=parent) + self.accelerometer = (0.0,0.0,0.0) + self.gyroscope = (0.0,0.0,0.0) + self.sensor.listen(self.callback) + def callback(self, data): + self.accelerometer = (data.accelerometer.x, data.accelerometer.y, data.accelerometer.z) + self.gyroscope = (data.gyroscope.x, data.gyroscope.y, data.gyroscope.z) + +# ====================== 摄像头 ====================== +class CameraManager: + def __init__(self, parent): + self._parent = parent self.sensor = None self.surface = None - self._parent = parent_actor - self.hud = hud self.yaw = 0.0 self.pitch = 10.0 - self.distance = 5.5 - - world = self._parent.get_world() - bp = world.get_blueprint_library().find('sensor.camera.rgb') - bp.set_attribute('image_size_x', '1280') - bp.set_attribute('image_size_y', '720') - self.bp = bp - self.set_sensor() + self.dist = 5.5 + self.reset_cam() - def set_sensor(self): + def reset_cam(self): if self.sensor: self.sensor.destroy() - trans = carla.Transform(carla.Location(x=-self.distance, z=2.3), carla.Rotation(pitch=self.pitch, yaw=self.yaw)) - self.sensor = self._parent.get_world().spawn_actor( - self.bp, trans, attach_to=self._parent, attachment_type=carla.AttachmentType.SpringArm) - weak_self = weakref.ref(self) - self.sensor.listen(lambda img: CameraManager._parse_image(weak_self, img)) + bp = self._parent.get_world().get_blueprint_library().find("sensor.camera.rgb") + bp.set_attribute("image_size_x","1280") + bp.set_attribute("image_size_y","720") + trans = carla.Transform(carla.Location(x=-self.dist,z=2.3), carla.Rotation(pitch=self.pitch,yaw=self.yaw)) + self.sensor = self._parent.get_world().spawn_actor(bp, trans, attach_to=self._parent, attachment_type=carla.AttachmentType.SpringArm) + self.sensor.listen(self.on_img) + + def on_img(self, img): + arr = np.frombuffer(img.raw_data, dtype=np.uint8).reshape(img.height,img.width,4)[:,:,:3] + self.surface = pygame.surfarray.make_surface(arr.swapaxes(0,1)) def render(self, display): if self.surface: - display.blit(self.surface, (0, 0)) - trans = carla.Transform(carla.Location(x=-self.distance, z=2.3), carla.Rotation(pitch=self.pitch, yaw=self.yaw)) - self.sensor.set_transform(trans) - - @staticmethod - def _parse_image(weak_self, image): - self = weak_self() - if not self: return - array = np.frombuffer(image.raw_data, dtype=np.uint8) - array = array.reshape((image.height, image.width, 4)) - array = array[:, :, :3] - array = array[:, :, ::-1] - self.surface = pygame.surfarray.make_surface(array.swapaxes(0, 1)) - - -# ============================================================================== -# -- game_loop() --------------------------------------------------------------- -# ============================================================================== + display.blit(self.surface, (0,0)) + new_trans = carla.Transform(carla.Location(x=-self.dist,z=2.3), carla.Rotation(pitch=self.pitch,yaw=self.yaw)) + self.sensor.set_transform(new_trans) +# ====================== 主循环 ====================== def game_loop(args): pygame.init() pygame.font.init() world = None data = pd.DataFrame() + try: client = carla.Client(args.host, args.port) - client.set_timeout(2.0) - - display = pygame.display.set_mode((args.width, args.height), pygame.HWSURFACE | pygame.DOUBLEBUF) - hud = HUD(args.width, args.height) + client.set_timeout(3.0) + display = pygame.display.set_mode((1280,720), pygame.HWSURFACE|pygame.DOUBLEBUF) + hud = HUD(1280,720) world = World(client.get_world(), hud, args) - controller = KeyboardControl(world, True) - + controller = KeyboardControl() clock = pygame.time.Clock() + while True: - clock.tick_busy_loop(60) - if controller.parse_events(client, world, clock): + clock.tick(60) + if controller.parse_events(world): break world.tick(clock) world.render(display) data = pd.concat([data, pd.DataFrame([{ - "accelX": world.imu_sensor.accelerometer[0], - "accelY": world.imu_sensor.accelerometer[1], - "accelZ": world.imu_sensor.accelerometer[2], - "gyroX": world.imu_sensor.gyroscope[0], - "gyroY": world.imu_sensor.gyroscope[1], - "gyroZ": world.imu_sensor.gyroscope[2], - "class": args.name + "accelX":world.imu_sensor.accelerometer[0], + "accelY":world.imu_sensor.accelerometer[1], + "accelZ":world.imu_sensor.accelerometer[2], + "gyroX":world.imu_sensor.gyroscope[0], + "gyroY":world.imu_sensor.gyroscope[1], + "gyroZ":world.imu_sensor.gyroscope[2], + "class":args.name }])], ignore_index=True) pygame.display.flip() @@ -479,26 +301,17 @@ def game_loop(args): if world: world.destroy() data.to_csv(f"out_{args.name}.csv", index=False) - print(f"✅ CSV SAVED: out_{args.name}.csv") + print(f"✅ CSV 已保存: out_{args.name}.csv") pygame.quit() - -# ============================================================================== -# -- main() -------------------------------------------------------------------- -# ============================================================================== - def main(): - argparser = argparse.ArgumentParser(description='CARLA Autopilot') - argparser.add_argument('--host', default='127.0.0.1') - argparser.add_argument('--port', default=2000, type=int) - argparser.add_argument('--res', default='1280x720') - argparser.add_argument('--filter', default='vehicle.*') - argparser.add_argument('--rolename', default='hero') - argparser.add_argument('--gamma', default=2.2, type=float) - argparser.add_argument('--name', default="mehdi", type=str) - args = argparser.parse_args() - args.width, args.height = [int(x) for x in args.res.split('x')] + parser = argparse.ArgumentParser() + parser.add_argument("--host",default="127.0.0.1") + parser.add_argument("--port",default=2000,type=int) + parser.add_argument("--filter",default="vehicle.*") + parser.add_argument("--name",default="mehdi") + args = parser.parse_args() game_loop(args) -if __name__ == '__main__': +if __name__ == "__main__": main()