diff --git a/src/carla_driving_car_lane/Add_weather_switching.py b/src/carla_driving_car_lane/Add_weather_switching.py new file mode 100644 index 0000000000..28cb478900 --- /dev/null +++ b/src/carla_driving_car_lane/Add_weather_switching.py @@ -0,0 +1,342 @@ +#!/usr/bin/env python + +# Copyright (c) 2018 Intel Labs. +# authors: German Ros (german.ros@gmail.com) +# +# This work is licensed under the terms of the MIT license. +# For a copy, see + +"""Example of automatic vehicle control from client side.""" + +from __future__ import print_function + +import argparse +import collections +import datetime +import glob +import logging +import math +import os +import random +import re +import sys +import weakref + +# ========================================== +# 适配你的路径 D:\carla0.9.15 +# ========================================== +CARLA_ROOT = r"D:\carla0.9.15" +PYTHON_API = os.path.join(CARLA_ROOT, "PythonAPI") +sys.path.append(PYTHON_API) +sys.path.append(os.path.join(PYTHON_API, "carla")) + +try: + eggs = glob.glob(os.path.join(PYTHON_API, "carla", "dist", "*.egg")) + for e in eggs: + sys.path.append(e) +except: + pass + +# ================= 依赖导入 ================= +try: + import pygame + from pygame.locals import * +except ImportError: + raise RuntimeError('请安装:pip install pygame') + +try: + import numpy as np +except ImportError: + raise RuntimeError('请安装:pip install numpy') + +import carla +from carla import ColorConverter as cc + +from agents.navigation.behavior_agent import BehaviorAgent + +# ============================================================================== +# -- Global functions +# ============================================================================== +def find_weather_presets(): + rgx = re.compile('.+?(?:(?<=[a-z])(?=[A-Z])|(?=[A-Z])(?=[A-Z][a-z])|$)') + def name(x): return ' '.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] + +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 +# ============================================================================== +class World(object): + def __init__(self, carla_world, hud, args): + self.world = carla_world + 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.camera_manager = None + self._weather_presets = find_weather_presets() + self._weather_index = 0 + self._actor_filter = args.filter + self._gamma = 2.2 + self.restart(args) + self.world.on_tick(hud.on_world_tick) + + def restart(self, args): + cam_index = self.camera_manager.index if self.camera_manager else 0 + cam_pos_id = self.camera_manager.transform_index if self.camera_manager else 0 + + blueprint = random.choice(self.world.get_blueprint_library().filter(self._actor_filter)) + blueprint.set_attribute('role_name', 'hero') + + if self.player is not None: + spawn_point = self.player.get_transform() + spawn_point.location.z += 2.0 + self.destroy() + self.player = self.world.try_spawn_actor(blueprint, spawn_point) + + while self.player is None: + spawn_point = random.choice(self.map.get_spawn_points()) + self.player = self.world.try_spawn_actor(blueprint, spawn_point) + + self.collision_sensor = CollisionSensor(self.player, self.hud) + self.lane_invasion_sensor = LaneInvasionSensor(self.player, self.hud) + self.gnss_sensor = GnssSensor(self.player) + self.camera_manager = CameraManager(self.player, self.hud, self._gamma) + self.camera_manager.transform_index = cam_pos_id + self.camera_manager.set_sensor(cam_index, notify=False) + + def next_weather(self, reverse=False): + self._weather_index += -1 if reverse else 1 + self._weather_index %= len(self._weather_presets) + preset = self._weather_presets[self._weather_index] + self.hud.notification('Weather: %s' % preset[1]) + self.world.set_weather(preset[0]) + + def tick(self, clock): + self.hud.tick(clock) + + def render(self, display): + self.camera_manager.render(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.player] + for actor in actors: + if actor is not None: + actor.destroy() + +# ============================================================================== +# -- KeyboardControl +# ============================================================================== +class KeyboardControl(object): + def __init__(self, world): + world.hud.notification("Press ESC or Ctrl+Q to quit", seconds=2) + world.hud.notification("PageUp / PageDown: Change Weather", seconds=2) + + def parse_events(self, world): + for event in pygame.event.get(): + if event.type == pygame.QUIT: + return True + if event.type == pygame.KEYUP: + if self._is_quit_shortcut(event.key): + return True + if event.key == K_PAGEUP: + world.next_weather(reverse=True) + if event.key == K_PAGEDOWN: + world.next_weather() + return False + + @staticmethod + def _is_quit_shortcut(key): + return key == K_ESCAPE or (key == K_q and pygame.key.get_mod() & KMOD_CTRL) + +# ============================================================================== +# -- HUD +# ============================================================================== +class HUD(object): + def __init__(self, width, height): + self.dim = (width, height) + font = pygame.font.Font(pygame.font.get_default_font(), 16) + self._notifications = FadingText(font, (width, 40), (0, height - 40)) + self.server_fps = 0 + self.frame = 0 + self.simulation_time = 0 + self._show_info = True + + def on_world_tick(self, timestamp): + self.server_fps = 1.0 / timestamp.delta_seconds + self.frame = timestamp.frame_count + self.simulation_time = timestamp.elapsed_seconds + + def tick(self, clock): + self._notifications.tick(clock) + + def notification(self, text, seconds=2): + self._notifications.set_text(text, seconds=seconds) + + def error(self, text): + self._notifications.set_text('Error: %s' % text, (255, 0, 0)) + + def render(self, display): + self._notifications.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): + self.seconds_left = seconds + self.surface = self.font.render(text, True, color) + + def tick(self, clock): + self.seconds_left = max(0.0, self.seconds_left - clock.get_time() / 1000.0) + + def render(self, display): + display.blit(self.surface, self.pos) + +# ============================================================================== +# -- CollisionSensor +# ============================================================================== +class CollisionSensor(object): + def __init__(self, parent_actor, hud): + self.sensor = None + self._parent = parent_actor + self.hud = hud + bp = parent_actor.get_world().get_blueprint_library().find('sensor.other.collision') + self.sensor = parent_actor.get_world().spawn_actor(bp, carla.Transform(), attach_to=parent_actor) + self.sensor.listen(lambda e: self.hud.notification(f"碰撞: {get_actor_display_name(e.other_actor)}")) + +# ============================================================================== +# -- LaneInvasionSensor +# ============================================================================== +class LaneInvasionSensor(object): + def __init__(self, parent_actor, hud): + self.sensor = None + bp = parent_actor.get_world().get_blueprint_library().find('sensor.other.lane_invasion') + self.sensor = parent_actor.get_world().spawn_actor(bp, carla.Transform(), attach_to=parent_actor) + +# ============================================================================== +# -- GnssSensor +# ============================================================================== +class GnssSensor(object): + def __init__(self, parent_actor): + self.sensor = None + self.lat = self.lon = 0.0 + bp = parent_actor.get_world().get_blueprint_library().find('sensor.other.gnss') + self.sensor = parent_actor.get_world().spawn_actor(bp, carla.Transform(carla.Location(z=2.0)), attach_to=parent_actor) + +# ============================================================================== +# -- CameraManager +# ============================================================================== +class CameraManager(object): + def __init__(self, parent_actor, hud, gamma): + self.sensor = None + self.surface = None + self._parent = parent_actor + self.hud = hud + self.transform_index = 0 + self._camera_transforms = [ + (carla.Transform(carla.Location(x=-5.5, z=2.5), carla.Rotation(pitch=8)), carla.AttachmentType.SpringArm) + ] + self.sensors = [['sensor.camera.rgb', cc.Raw, 'RGB']] + world = parent_actor.get_world() + for item in self.sensors: + b = world.get_blueprint_library().find(item[0]) + b.set_attribute('image_size_x', str(hud.dim[0])) + b.set_attribute('image_size_y', str(hud.dim[1])) + item.append(b) + self.index = None + + def set_sensor(self, index, notify=True): + if self.sensor: + self.sensor.destroy() + self.sensor = self._parent.get_world().spawn_actor( + self.sensors[0][-1], + self._camera_transforms[0][0], + attach_to=self._parent + ) + self.sensor.listen(lambda img: self._parse_image(img)) + self.index = 0 + + def _parse_image(self, image): + array = np.frombuffer(image.raw_data, dtype=np.uint8) + array = array.reshape(image.height, image.width, 4)[:, :, :3] + self.surface = pygame.surfarray.make_surface(array.swapaxes(0, 1)[:, :, ::-1]) + + def render(self, display): + if self.surface: + display.blit(self.surface, (0, 0)) + +# ============================================================================== +# -- Game Loop +# ============================================================================== +def game_loop(args): + pygame.init() + pygame.font.init() + world = None + + try: + client = carla.Client(args.host, args.port) + client.set_timeout(10.0) + display = pygame.display.set_mode((args.width, args.height), pygame.HWSURFACE | pygame.DOUBLEBUF) + hud = HUD(args.width, args.height) + world = World(client.get_world(), hud, args) + controller = KeyboardControl(world) + + agent = BehaviorAgent(world.player, behavior=args.behavior) + spawn_points = world.map.get_spawn_points() + agent.set_destination(random.choice(spawn_points).location) + + clock = pygame.time.Clock() + + while True: + clock.tick_busy_loop(60) + if controller.parse_events(world): + return + + world.tick(clock) + world.render(display) + pygame.display.flip() + + control = agent.run_step() + world.player.apply_control(control) + + finally: + if world: + world.destroy() + pygame.quit() + +# ============================================================================== +# -- main() +# ============================================================================== +def main(): + argparser = argparse.ArgumentParser(description='CARLA Automatic Control Client') + 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('-b', '--behavior', default='normal') + args = argparser.parse_args() + args.width, args.height = map(int, args.res.split('x')) + game_loop(args) + +if __name__ == '__main__': + main() diff --git a/src/carla_driving_car_lane/README.md b/src/carla_driving_car_lane/README.md new file mode 100644 index 0000000000..abeadf0f5a --- /dev/null +++ b/src/carla_driving_car_lane/README.md @@ -0,0 +1,45 @@ + +--- + +## CARLA 0.9.15 交通灯检测与自动驾驶控制系统 + + +本项目基于 CARLA 仿真器(0.9.15 版本)实现了一套简易自动驾驶控制系统。系统通过摄像头数据检测交通信号灯,并控制车辆在红灯时停车、绿灯时通行。 + +## 功能特性 +- **交通灯检测**:使用 RGB 摄像头与目标包围框实现交通灯识别 +- **启停逻辑**:车辆在红灯时自动停车,绿灯时自动恢复行驶 +- **自动驾驶控制**:根据交通灯状态自动调整油门与刹车 + +## 使用技术 +- **仿真平台**:CARLA 仿真器 0.9.15 +- **编程语言**:Python 3.9 +- **依赖库**: + - pygame:用于渲染仿真界面 + - cv2(OpenCV):用于图像处理(可扩展功能) + - numpy、time、math、random:用于数值计算与时序控制 + +## 工作原理 +1. **传感器配置** + 车辆搭载前置 RGB 摄像头及其他 CARLA 内置传感器。 + +2. **交通灯检测** + 脚本通过 CARLA 内置环境信息(目标包围框)检测交通灯, + 并判断信号灯状态(红灯/绿灯)。 + +3. **控制逻辑** + - 若前方检测到红灯,车辆自动刹车并停车 + - 若检测到绿灯或无有效红灯,车辆继续行驶 + +## 运行方法 +1. 安装 Python 依赖库: + ```bash + pip install pygame opencv-python numpy + ``` +2. 使用方式: + 启动 `CarlaUE4.exe`(Windows 系统) + +## 预期效果 +车辆在前方检测到红灯时平稳停车,绿灯亮起后恢复行驶,所有行为均由控制逻辑自动完成。 + +--- diff --git a/src/carla_driving_car_lane/Requirements.txt b/src/carla_driving_car_lane/Requirements.txt new file mode 100644 index 0000000000..8437a15171 --- /dev/null +++ b/src/carla_driving_car_lane/Requirements.txt @@ -0,0 +1,9 @@ + +carla==0.9.15 +pygame==2.1.0 +numpy==1.23.5 +opencv-python==4.8.0.76 +matplotlib==3.7.1 +torch==2.0.0 # Only if you're using PyTorch for detection +torchvision==0.15.1 +Pillow==9.4.0 diff --git a/src/carla_driving_car_lane/automatic_control.py b/src/carla_driving_car_lane/automatic_control.py new file mode 100644 index 0000000000..078bf9e2aa --- /dev/null +++ b/src/carla_driving_car_lane/automatic_control.py @@ -0,0 +1,291 @@ +#!/usr/bin/env python + +from __future__ import print_function + +import argparse +import collections +import datetime +import glob +import logging +import math +import os +import random +import re +import sys +import weakref + +# --------------- Python 3.9 专用修复 --------------- +# 清理冲突路径 +for path in list(sys.path): + if "carla/dist" in path and path.endswith(".egg"): + sys.path.remove(path) + +# 正确添加 CARLA PythonAPI 路径 +CARLA_ROOT = r"D:\carla0.9.15" +sys.path.append(os.path.join(CARLA_ROOT, "PythonAPI")) +sys.path.append(os.path.join(CARLA_ROOT, "PythonAPI", "carla")) + +# ---------------- 依赖 ---------------- +try: + import pygame + from pygame.locals import KMOD_CTRL + from pygame.locals import K_ESCAPE + from pygame.locals import K_q +except ImportError: + raise RuntimeError('请安装: pip install pygame') + +try: + import numpy as np +except ImportError: + raise RuntimeError('请安装: pip install numpy') + +import carla +from carla import ColorConverter as cc + +# 导入自动驾驶代理 +from agents.navigation.behavior_agent import BehaviorAgent + + +# ============================================================================== +# -- 工具函数 +# ============================================================================== +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 场景管理 +# ============================================================================== +class World(object): + def __init__(self, carla_world, hud, args): + self.world = carla_world + 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.camera_manager = None + self._actor_filter = args.filter + self.restart(args) + self.world.on_tick(hud.on_world_tick) + + def restart(self, args): + # 清理旧车辆 + if self.player is not None: + self.destroy() + + # 生成车辆 + blueprint = random.choice(self.world.get_blueprint_library().filter(self._actor_filter)) + blueprint.set_attribute('role_name', 'hero') + + while self.player is None: + spawn_point = random.choice(self.map.get_spawn_points()) + self.player = self.world.try_spawn_actor(blueprint, spawn_point) + + # 传感器(全部修复完成) + self.collision_sensor = CollisionSensor(self.player, self.hud) + self.lane_invasion_sensor = LaneInvasionSensor(self.player, self.hud) + self.gnss_sensor = GnssSensor(self.player) + self.camera_manager = CameraManager(self.player, self.hud) + self.camera_manager.set_sensor(0) + + def tick(self, clock): + self.hud.tick(clock) + + def render(self, display): + self.camera_manager.render(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.player] + for actor in actors: + if actor is not None and actor.is_alive: + actor.destroy() + + +# ============================================================================== +# -- 键盘控制 +# ============================================================================== +class KeyboardControl(object): + @staticmethod + def parse_events(): + for event in pygame.event.get(): + if event.type == pygame.QUIT: + return True + if event.type == pygame.KEYUP: + if event.key == K_ESCAPE or (event.key == K_q and pygame.key.get_mod() & KMOD_CTRL): + return True + return False + + +# ============================================================================== +# -- HUD 显示 +# ============================================================================== +class HUD(object): + def __init__(self, width, height): + self.dim = (width, height) + font = pygame.font.Font(pygame.font.get_default_font(), 16) + self._notifications = FadingText(font, (width, 40), (0, height - 40)) + self.server_fps = 0 + + def on_world_tick(self, timestamp): + self.server_fps = 1.0 / max(timestamp.delta_seconds, 0.01) + + def tick(self, clock): + self._notifications.tick(clock) + + def notification(self, text, seconds=2): + self._notifications.set_text(text, seconds=seconds) + + def render(self, display): + self._notifications.render(display) + + +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): + self.seconds_left = seconds + self.surface = self.font.render(text, True, color) + + def tick(self, clock): + self.seconds_left = max(0, self.seconds_left - clock.get_time() / 1000) + + def render(self, display): + display.blit(self.surface, self.pos) + + +# ============================================================================== +# -- 传感器(全部修复完成) +# ============================================================================== +class CollisionSensor(object): + def __init__(self, parent, hud): + self.sensor = parent.get_world().spawn_actor( + parent.get_world().get_blueprint_library().find('sensor.other.collision'), + carla.Transform(), attach_to=parent) + self.sensor.listen(lambda e: hud.notification(f"碰撞: {get_actor_display_name(e.other_actor)}")) + + +class LaneInvasionSensor(object): + def __init__(self, parent, hud): + self.sensor = parent.get_world().spawn_actor( + parent.get_world().get_blueprint_library().find('sensor.other.lane_invasion'), + carla.Transform(), attach_to=parent) + + +class GnssSensor(object): + 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(z=2.0)), attach_to=parent) + + +# ============================================================================== +# -- 相机 +# ============================================================================== +class CameraManager(object): + def __init__(self, parent, hud): + self.sensor = None + self.surface = None + self._parent = parent + self.hud = hud + self.index = 0 + + def set_sensor(self, index): + if self.sensor: self.sensor.destroy() + bp = self._parent.get_world().get_blueprint_library().find('sensor.camera.rgb') + bp.set_attribute('image_size_x', str(self.hud.dim[0])) + bp.set_attribute('image_size_y', str(self.hud.dim[1])) + + transform = carla.Transform(carla.Location(x=-5.5, z=2.5), carla.Rotation(pitch=8)) + self.sensor = self._parent.get_world().spawn_actor( + bp, transform, attach_to=self._parent, attachment_type=carla.AttachmentType.SpringArm) + self.sensor.listen(lambda img: self._parse(img)) + + def _parse(self, image): + array = np.frombuffer(image.raw_data, dtype=np.uint8) + array = array.reshape(image.height, image.width, 4)[:, :, :3] + self.surface = pygame.surfarray.make_surface(array.swapaxes(0, 1)[:, :, ::-1]) + + def render(self, display): + if self.surface: display.blit(self.surface, (0, 0)) + + +# ============================================================================== +# -- 主循环 +# ============================================================================== +def game_loop(args): + pygame.init() + pygame.font.init() + world = None + + try: + client = carla.Client(args.host, args.port) + client.set_timeout(10.0) + display = pygame.display.set_mode((args.width, args.height)) + hud = HUD(args.width, args.height) + world = World(client.get_world(), hud, args) + + # 自动导航 + agent = BehaviorAgent(world.player, behavior=args.behavior) + spawn_points = world.map.get_spawn_points() + target = random.choice(spawn_points).location + agent.set_destination(target) + hud.notification("自动导航已启动", 3) + + clock = pygame.time.Clock() + + while True: + clock.tick_busy_loop(60) + if KeyboardControl.parse_events(): + return + + world.tick(clock) + world.render(display) + pygame.display.flip() + + # 自动驾驶控制 + control = agent.run_step() + world.player.apply_control(control) + + # 到达目标后自动换新目标 + if agent.done(): + new_target = random.choice(spawn_points).location + agent.set_destination(new_target) + hud.notification("已到达!前往下一个目标", 2) + + finally: + if world: + world.destroy() + pygame.quit() + + +# ============================================================================== +# -- 主函数 +# ============================================================================== +def main(): + argparser = argparse.ArgumentParser() + 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('--behavior', default='normal') + args = argparser.parse_args() + args.width, args.height = map(int, args.res.split('x')) + game_loop(args) + + +if __name__ == '__main__': + main() diff --git a/src/carla_driving_car_lane/main.py b/src/carla_driving_car_lane/main.py new file mode 100644 index 0000000000..73752cec9d --- /dev/null +++ b/src/carla_driving_car_lane/main.py @@ -0,0 +1,341 @@ +#!/usr/bin/env python + +# Copyright (c) 2018 Intel Labs. +# authors: German Ros (german.ros@gmail.com) +# +# This work is licensed under the terms of the MIT license. +# For a copy, see + +"""Example of automatic vehicle control from client side.""" + +from __future__ import print_function + +import argparse +import collections +import datetime +import glob +import logging +import math +import os +import random +import re +import sys +import weakref + +# ========================================== +# 适配你的路径 D:\carla0.9.15 +# ========================================== +CARLA_ROOT = r"D:\carla0.9.15" +PYTHON_API = os.path.join(CARLA_ROOT, "PythonAPI") +sys.path.append(PYTHON_API) +sys.path.append(os.path.join(PYTHON_API, "carla")) + +try: + eggs = glob.glob(os.path.join(PYTHON_API, "carla", "dist", "*.egg")) + for e in eggs: + sys.path.append(e) +except: + pass + +# ================= 依赖导入 ================= +try: + import pygame + from pygame.locals import KMOD_CTRL + from pygame.locals import K_ESCAPE + from pygame.locals import K_q +except ImportError: + raise RuntimeError('请安装:pip install pygame') + +try: + import numpy as np +except ImportError: + raise RuntimeError('请安装:pip install numpy') + +import carla +from carla import ColorConverter as cc + +# 已删除不存在的 RoamingAgent +from agents.navigation.behavior_agent import BehaviorAgent +from agents.navigation.basic_agent import BasicAgent + +# ============================================================================== +# -- Global functions +# ============================================================================== +def find_weather_presets(): + rgx = re.compile('.+?(?:(?<=[a-z])(?=[A-Z])|(?=[A-Z])(?=[A-Z][a-z])|$)') + def name(x): return ' '.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] + +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 +# ============================================================================== +class World(object): + def __init__(self, carla_world, hud, args): + self.world = carla_world + 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.camera_manager = None + self._weather_presets = find_weather_presets() + self._weather_index = 0 + self._actor_filter = args.filter + self._gamma = 2.2 + self.restart(args) + self.world.on_tick(hud.on_world_tick) + + def restart(self, args): + cam_index = self.camera_manager.index if self.camera_manager else 0 + cam_pos_id = self.camera_manager.transform_index if self.camera_manager else 0 + + blueprint = random.choice(self.world.get_blueprint_library().filter(self._actor_filter)) + blueprint.set_attribute('role_name', 'hero') + + if self.player is not None: + spawn_point = self.player.get_transform() + spawn_point.location.z += 2.0 + self.destroy() + self.player = self.world.try_spawn_actor(blueprint, spawn_point) + + while self.player is None: + spawn_point = random.choice(self.map.get_spawn_points()) + self.player = self.world.try_spawn_actor(blueprint, spawn_point) + + self.collision_sensor = CollisionSensor(self.player, self.hud) + self.lane_invasion_sensor = LaneInvasionSensor(self.player, self.hud) + self.gnss_sensor = GnssSensor(self.player) + self.camera_manager = CameraManager(self.player, self.hud, self._gamma) + self.camera_manager.transform_index = cam_pos_id + self.camera_manager.set_sensor(cam_index, notify=False) + + def next_weather(self, reverse=False): + self._weather_index += -1 if reverse else 1 + self._weather_index %= len(self._weather_presets) + preset = self._weather_presets[self._weather_index] + self.hud.notification('Weather: %s' % preset[1]) + self.world.set_weather(preset[0]) + + def tick(self, clock): + self.hud.tick(self, clock) + + def render(self, display): + self.camera_manager.render(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.player] + for actor in actors: + if actor is not None: + actor.destroy() + +# ============================================================================== +# -- KeyboardControl +# ============================================================================== +class KeyboardControl(object): + def __init__(self, world): + world.hud.notification("Press ESC or Ctrl+Q to quit", seconds=2) + + def parse_events(self): + for event in pygame.event.get(): + if event.type == pygame.QUIT: + return True + if event.type == pygame.KEYUP: + if self._is_quit_shortcut(event.key): + return True + return False + + @staticmethod + def _is_quit_shortcut(key): + return key == K_ESCAPE or (key == K_q and pygame.key.get_mod() & KMOD_CTRL) + +# ============================================================================== +# -- HUD +# ============================================================================== +class HUD(object): + def __init__(self, width, height): + self.dim = (width, height) + font = pygame.font.Font(pygame.font.get_default_font(), 16) + self._notifications = FadingText(font, (width, 40), (0, height - 40)) + self.server_fps = 0 + self.frame = 0 + self.simulation_time = 0 + self._show_info = True + + def on_world_tick(self, timestamp): + self.server_fps = 1.0 / timestamp.delta_seconds + self.frame = timestamp.frame_count + self.simulation_time = timestamp.elapsed_seconds + + def tick(self, world, clock): + self._notifications.tick(world, clock) + + def notification(self, text, seconds=2): + self._notifications.set_text(text, seconds=seconds) + + def error(self, text): + self._notifications.set_text('Error: %s' % text, (255, 0, 0)) + + def render(self, display): + self._notifications.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): + self.seconds_left = seconds + self.surface = self.font.render(text, True, color) + + def tick(self, _, clock): + self.seconds_left = max(0.0, self.seconds_left - clock.get_time() / 1000.0) + + def render(self, display): + display.blit(self.surface, self.pos) + +# ============================================================================== +# -- CollisionSensor +# ============================================================================== +class CollisionSensor(object): + def __init__(self, parent_actor, hud): + self.sensor = None + self._parent = parent_actor + self.hud = hud + bp = parent_actor.get_world().get_blueprint_library().find('sensor.other.collision') + self.sensor = parent_actor.get_world().spawn_actor(bp, carla.Transform(), attach_to=parent_actor) + self.sensor.listen(lambda e: self.hud.notification(f"碰撞: {get_actor_display_name(e.other_actor)}")) + +# ============================================================================== +# -- LaneInvasionSensor +# ============================================================================== +class LaneInvasionSensor(object): + def __init__(self, parent_actor, hud): + self.sensor = None + bp = parent_actor.get_world().get_blueprint_library().find('sensor.other.lane_invasion') + self.sensor = parent_actor.get_world().spawn_actor(bp, carla.Transform(), attach_to=parent_actor) + +# ============================================================================== +# -- GnssSensor +# ============================================================================== +class GnssSensor(object): + def __init__(self, parent_actor): + self.sensor = None + self.lat = self.lon = 0.0 + bp = parent_actor.get_world().get_blueprint_library().find('sensor.other.gnss') + self.sensor = parent_actor.get_world().spawn_actor(bp, carla.Transform(carla.Location(z=2.0)), attach_to=parent_actor) + +# ============================================================================== +# -- CameraManager +# ============================================================================== +class CameraManager(object): + def __init__(self, parent_actor, hud, gamma): + self.sensor = None + self.surface = None + self._parent = parent_actor + self.hud = hud + self.transform_index = 0 + self._camera_transforms = [ + (carla.Transform(carla.Location(x=-5.5, z=2.5), carla.Rotation(pitch=8)), carla.AttachmentType.SpringArm) + ] + self.sensors = [['sensor.camera.rgb', cc.Raw, 'RGB']] + world = parent_actor.get_world() + for item in self.sensors: + b = world.get_blueprint_library().find(item[0]) + b.set_attribute('image_size_x', str(hud.dim[0])) + b.set_attribute('image_size_y', str(hud.dim[1])) + item.append(b) + self.index = None + + def set_sensor(self, index, notify=True): + if self.sensor: + self.sensor.destroy() + self.sensor = self._parent.get_world().spawn_actor( + self.sensors[0][-1], + self._camera_transforms[0][0], + attach_to=self._parent + ) + self.sensor.listen(lambda img: self._parse_image(img)) + self.index = 0 + + def _parse_image(self, image): + array = np.frombuffer(image.raw_data, dtype=np.uint8) + array = array.reshape(image.height, image.width, 4)[:, :, :3] + self.surface = pygame.surfarray.make_surface(array.swapaxes(0, 1)[:, :, ::-1]) + + def render(self, display): + if self.surface: + display.blit(self.surface, (0, 0)) + +# ============================================================================== +# -- Game Loop +# ============================================================================== +def game_loop(args): + pygame.init() + pygame.font.init() + world = None + + try: + client = carla.Client(args.host, args.port) + client.set_timeout(10.0) + display = pygame.display.set_mode((args.width, args.height), pygame.HWSURFACE | pygame.DOUBLEBUF) + hud = HUD(args.width, args.height) + world = World(client.get_world(), hud, args) + controller = KeyboardControl(world) + + agent = BehaviorAgent(world.player, behavior=args.behavior) + spawn_points = world.map.get_spawn_points() + agent.set_destination(random.choice(spawn_points).location) + + clock = pygame.time.Clock() + + while True: + clock.tick_busy_loop(60) + if controller.parse_events(): + return + + world.tick(clock) + world.render(display) + pygame.display.flip() + + control = agent.run_step() + world.player.apply_control(control) + + finally: + if world: + world.destroy() + pygame.quit() + +# ============================================================================== +# -- main() +# ============================================================================== +def main(): + argparser = argparse.ArgumentParser(description='CARLA Automatic Control Client') + 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('-b', '--behavior', default='normal') + args = argparser.parse_args() + args.width, args.height = map(int, args.res.split('x')) + game_loop(args) + +if __name__ == '__main__': + main() diff --git a/src/carla_driving_car_lane/start_carla_sim.bat b/src/carla_driving_car_lane/start_carla_sim.bat new file mode 100644 index 0000000000..7383dbc8ae --- /dev/null +++ b/src/carla_driving_car_lane/start_carla_sim.bat @@ -0,0 +1,27 @@ +@echo off +chcp 65001 >nul 2>&1 +color 0A +mode con: cols=80 lines=25 +title CARLA AUTOPILOT + +echo ============================================== +echo CARLA 0.9.15 AUTOPILOT LAUNCHER +echo ============================================== +echo. + +echo Starting CARLA Simulator... +start "" "D:\carla0.9.15\CarlaUE4.exe" -quality-level=Low + +echo. +echo Waiting for CARLA server (15s)... +timeout /t 15 /nobreak >nul + +echo. +echo Starting autopilot script... +echo. + +"D:\carla_automatic1\.venv\Scripts\python.exe" "D:\carla_automatic1\automatic_control.py" + +echo. +echo Done. +pause diff --git a/src/carla_driving_car_lane/world_Coordinate.py b/src/carla_driving_car_lane/world_Coordinate.py new file mode 100644 index 0000000000..acb8ded23d --- /dev/null +++ b/src/carla_driving_car_lane/world_Coordinate.py @@ -0,0 +1,307 @@ +#!/usr/bin/env python +# -*- coding: utf-8 -*- +import os +import sys +import argparse +import math +import random +import weakref +import glob + +# ============================================== +# 强制阻断所有冲突库加载(终极方案) +# ============================================== +for lib in ['matplotlib', 'scipy', 'tensorflow', 'torch', 'keras']: + sys.modules[lib] = None + sys.modules[lib + '.core'] = None + sys.modules[lib + '.api'] = None + +# 加载 CARLA Python API +try: + current_dir = os.path.dirname(os.path.abspath(__file__)) + eggs = glob.glob(os.path.join(current_dir, "carla", "dist", "*.egg")) + for e in eggs: + sys.path.append(e) +except: + pass + +# 仅加载必要库 +import pygame +from pygame.locals import * +import numpy as np +import carla +from carla import ColorConverter as cc + + +# ============================================== +# 自己实现极简自动导航(不依赖任何第三方库) +# ============================================== +class SimpleAutoAgent: + def __init__(self, vehicle): + self.vehicle = vehicle + self.world = vehicle.get_world() + self.map = self.world.get_map() + self.target = None + + def set_destination(self, location): + self.target = location + + def run_step(self): + control = carla.VehicleControl() + if not self.target: + return control + + # 基础自动行驶逻辑 + transform = self.vehicle.get_transform() + forward = transform.get_forward_vector() + target_dir = self.target - transform.location + target_dir.z = 0 + + # 计算方向 + dot = forward.x * target_dir.x + forward.y * target_dir.y + cross = forward.x * target_dir.y - forward.y * target_dir.x + distance = math.sqrt(target_dir.x ** 2 + target_dir.y ** 2) + + # 控制 + if distance > 5.0: + control.throttle = 0.5 + control.steer = max(-0.3, min(0.3, cross * 0.15)) + else: + control.brake = 1.0 + + return control + + +# ===================== 以下是完整 CARLA 运行代码 ===================== +def find_weather_presets(): + presets = [] + for name in dir(carla.WeatherParameters): + if name[0].isupper(): + presets.append((getattr(carla.WeatherParameters, name), name)) + return presets + + +def get_actor_display_name(actor): + name = ' '.join(actor.type_id.replace('_', '.').title().split('.')[1:]) + return name + + +class World(object): + def __init__(self, carla_world, hud, args): + self.world = carla_world + self.map = self.world.get_map() + self.hud = hud + self.player = None + self.collision_sensor = None + self.lane_invasion_sensor = None + self.camera_manager = None + self._weather_presets = find_weather_presets() + self._weather_index = 0 + self._actor_filter = args.filter + self.restart(args) + self.world.on_tick(hud.on_world_tick) + + def restart(self, args): + blueprint_library = self.world.get_blueprint_library() + blueprint = random.choice(blueprint_library.filter(self._actor_filter)) + blueprint.set_attribute('role_name', 'hero') + + if self.player is not None: + spawn_point = self.player.get_transform() + spawn_point.location.z += 2.0 + self.destroy() + self.player = self.world.try_spawn_actor(blueprint, spawn_point) + + while self.player is None: + spawn_point = random.choice(self.map.get_spawn_points()) + self.player = self.world.try_spawn_actor(blueprint, spawn_point) + + self.collision_sensor = CollisionSensor(self.player, self.hud) + self.lane_invasion_sensor = LaneInvasionSensor(self.player, self.hud) + self.camera_manager = CameraManager(self.player, self.hud) + self.camera_manager.set_sensor(0) + + def next_weather(self, reverse=False): + self._weather_index += -1 if reverse else 1 + self._weather_index %= len(self._weather_presets) + preset = self._weather_presets[self._weather_index] + self.hud.notification('天气: %s' % preset[1]) + self.world.set_weather(preset[0]) + + def tick(self, clock): + self.hud.tick(clock, self) + + def render(self, display): + self.camera_manager.render(display) + self.hud.render(display) + + def destroy(self): + actors = [self.camera_manager.sensor, self.collision_sensor.sensor, + self.lane_invasion_sensor.sensor, self.player] + for actor in actors: + if actor and actor.is_alive: + actor.destroy() + + +class CollisionSensor(object): + def __init__(self, parent, hud): + self.sensor = parent.get_world().spawn_actor( + parent.get_world().get_blueprint_library().find('sensor.other.collision'), + carla.Transform(), attach_to=parent) + self.hud = hud + self.sensor.listen(lambda e: self.hud.notification(f"碰撞: {get_actor_display_name(e.other_actor)}")) + + +class LaneInvasionSensor(object): + def __init__(self, parent, hud): + self.sensor = parent.get_world().spawn_actor( + parent.get_world().get_blueprint_library().find('sensor.other.lane_invasion'), + carla.Transform(), attach_to=parent) + self.hud = hud + self.sensor.listen(lambda e: self.hud.notification("车道偏离")) + + +class CameraManager(object): + def __init__(self, parent, hud): + self.sensor = None + self.surface = None + self.parent = parent + self.hud = hud + bp = parent.get_world().get_blueprint_library().find('sensor.camera.rgb') + bp.set_attribute('image_size_x', str(hud.dim[0])) + bp.set_attribute('image_size_y', str(hud.dim[1])) + self.bp = bp + + def set_sensor(self, index): + if self.sensor: self.sensor.destroy() + v = self.parent + ext = v.bounding_box.extent + trans = carla.Transform(carla.Location(x=-ext.x * 2.5, z=ext.z * 2), carla.Rotation(pitch=8)) + self.sensor = v.get_world().spawn_actor(self.bp, trans, attach_to=v) + self.sensor.listen(lambda img: self._parse(img)) + + def _parse(self, image): + image.convert(cc.Raw) + 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)) + + def render(self, display): + if self.surface: display.blit(self.surface, (0, 0)) + + +class HUD(object): + def __init__(self, width, height): + self.dim = (width, height) + self.font = pygame.font.Font(pygame.font.get_default_font(), 16) + self._notifications = FadingText(self.font, (width, 40), (0, height - 40)) + self.server_fps = 0 + self.speed = 0 + + def on_world_tick(self, timestamp): + self.server_fps = 1.0 / max(timestamp.delta_seconds, 0.01) + + def tick(self, clock, world): + self._notifications.tick(clock) + if world.player: + v = world.player.get_velocity() + self.speed = round(3.6 * math.sqrt(v.x ** 2 + v.y ** 2 + v.z ** 2), 1) + + def notification(self, text, seconds=2): + self._notifications.set_text(text, seconds=seconds) + + def render(self, display): + info = [f"FPS: {self.server_fps:.1f}", f"车速: {self.speed:.1f} km/h", "状态: 自动行驶中"] + for i, line in enumerate(info): + surf = self.font.render(line, True, (255, 255, 255)) + display.blit(surf, (10, 10 + i * 20)) + self._notifications.render(display) + + +class FadingText(object): + def __init__(self, font, dim, pos): + self.font = font + self.dim = dim + self.pos = pos + self.seconds_left = 0 + self.surface = font.render("", True, (0, 0, 0)) + + def set_text(self, text, color=(255, 255, 255), seconds=2): + self.seconds_left = seconds + self.surface = self.font.render(text, True, color) + + def tick(self, clock): + self.seconds_left = max(0.0, self.seconds_left - clock.get_time() / 1000) + + def render(self, display): + alpha = int(255 * self.seconds_left / 2) if self.seconds_left else 0 + self.surface.set_alpha(alpha) + display.blit(self.surface, self.pos) + + +class KeyboardControl(object): + def __init__(self, world): + world.hud.notification("ESC 退出", 2) + world.hud.notification("PageUp/Down 切换天气", 2) + + def parse_events(self, world): + for event in pygame.event.get(): + if event.type == QUIT or (event.type == KEYUP and event.key == K_ESCAPE): + return True + if event.type == KEYUP: + if event.key == K_PAGEUP: + world.next_weather(True) + elif event.key == K_PAGEDOWN: + world.next_weather() + return False + + +def game_loop(args): + pygame.init() + pygame.font.init() + world = None + + try: + client = carla.Client(args.host, args.port) + client.set_timeout(10.0) + display = pygame.display.set_mode((args.width, args.height)) + hud = HUD(args.width, args.height) + world = World(client.get_world(), hud, args) + controller = KeyboardControl(world) + + # 使用我们自己的极简自动代理(无任何依赖) + agent = SimpleAutoAgent(world.player) + dest = random.choice(world.map.get_spawn_points()).location + agent.set_destination(dest) + hud.notification("自动导航已启动") + + clock = pygame.time.Clock() + while True: + clock.tick_busy_loop(60) + if controller.parse_events(world): return + world.tick(clock) + world.render(display) + pygame.display.flip() + control = agent.run_step() + world.player.apply_control(control) + + finally: + if world: world.destroy() + pygame.quit() + + +def main(): + argparser = argparse.ArgumentParser() + 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.*') + args = argparser.parse_args() + args.width, args.height = map(int, args.res.split('x')) + game_loop(args) + + +if __name__ == '__main__': + main()