From d03646dbc92430840e79a8e2eae512cdc336f085 Mon Sep 17 00:00:00 2001 From: Yuansugu123 <2451009878@qq.com> Date: Fri, 19 Dec 2025 15:53:21 +0800 Subject: [PATCH 1/7] =?UTF-8?q?feat(car=5Ftracking):=20=E6=96=B0=E5=A2=9E?= =?UTF-8?q?=E9=9A=8F=E6=9C=BA=E7=94=9F=E6=88=90=E5=9C=B0=E5=9B=BE=E5=8A=9F?= =?UTF-8?q?=E8=83=BD?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- src/car_tracking/main.py | 255 +++++++++++++++++++++++++++------------ 1 file changed, 181 insertions(+), 74 deletions(-) diff --git a/src/car_tracking/main.py b/src/car_tracking/main.py index f4de03866b..30bff646b9 100644 --- a/src/car_tracking/main.py +++ b/src/car_tracking/main.py @@ -1,47 +1,153 @@ """ CARLA 0.9.14 低画质版专用脚本 -- 适配低画质CARLA的API差异 +- 适配低画质CARLA的API差异(解决各类属性找不到问题) - 解决着色器崩溃/异常类找不到问题 +- 新增:随机地图、车辆基础避让、速度控制输出 """ import sys import os +import random import carla import cv2 import numpy as np import queue +import traceback + +# ====================== 配置区域(可根据需求修改)====================== +# CARLA根目录 +CARLA_ROOT = 'D:/123/apps/CARLA_0.9.14/WindowsNoEditor' +# 服务器连接配置 +CARLA_HOST = '127.0.0.1' +CARLA_PORT = 2000 +CARLA_TIMEOUT = 60.0 # 低画质启动慢,延长超时 +# 支持的低画质小地图列表(CARLA 0.9.14) +# 注:低画质版建议只使用小地图Town01/Town02,避免卡顿 +SUPPORTED_MAPS = ['Town01', 'Town02'] +# 车辆配置 +PREFERRED_VEHICLE = 'vehicle.seat.leon' # 轻量化车型 +# 摄像头配置(低画质参数) +CAMERA_RESOLUTION = (640, 360) # 降低分辨率 +CAMERA_FOV = 80 +CAMERA_SENSOR_TICK = 0.1 # 10fps减少压力 +CAMERA_POSITION = carla.Transform(carla.Location(x=1.5, z=1.8)) # 摄像头挂载位置 +# 窗口配置 +WINDOW_NAME = 'CARLA Low-Quality View' +WINDOW_SIZE = (640, 360) +# 速度显示配置(低画质版无法通过API限制速度,仅显示实际速度) +SPEED_PRINT_INTERVAL = 10 # 每10帧打印一次速度(减少刷屏) +# ===================================================================== # 全局变量 IMAGE_QUEUE = queue.Queue(maxsize=1) -# 替换为你的低画质CARLA实际路径 -CARLA_ROOT = 'D:/123/apps/CARLA_0.9.14/WindowsNoEditor' -# 摄像头回调函数(低画质适配:降低分辨率减少压力) +def check_carla_running(): + """检查CARLA进程(CarlaUE4.exe)是否运行""" + try: + import psutil + for proc in psutil.process_iter(['name']): + if proc.info['name'] == 'CarlaUE4.exe': + return True + return False + except ImportError: + print("⚠️ 未安装psutil,无法检查CARLA进程,默认认为进程已运行") + return True + def image_callback(image): + """摄像头回调函数(低画质适配:降低分辨率减少压力)""" try: + # 将原始数据转换为BGRA格式的numpy数组 img_bgra = np.frombuffer(image.raw_data, dtype=np.uint8) img_bgra = img_bgra.reshape((image.height, image.width, 4)) + # 转换为BGR格式(适配OpenCV) img_bgr = cv2.cvtColor(img_bgra, cv2.COLOR_BGRA2BGR) - # 低画质优化:缩小图像尺寸(减少CV窗口渲染压力) - img_bgr = cv2.resize(img_bgr, (640, 360)) - - if IMAGE_QUEUE.full(): - IMAGE_QUEUE.get_nowait() + img_bgr = cv2.resize(img_bgr, WINDOW_SIZE) + # 保证队列中只有最新的一帧图像,避免堆积 + with IMAGE_QUEUE.mutex: + IMAGE_QUEUE.queue.clear() IMAGE_QUEUE.put(img_bgr, timeout=0.1) except Exception as e: print(f"⚠️ 图像回调出错:{e}") -def main(): - camera = None +def load_random_map(client): + """随机加载支持的低画质小地图(适配0.9.14)""" + random_map = random.choice(SUPPORTED_MAPS) + try: + world = client.load_world(random_map) + world.wait_for_tick() + print(f"✅ 随机加载地图成功:{random_map}(当前地图路径:{world.get_map().name})") + return world + except Exception as e: + print(f"❌ 加载地图{random_map}失败,使用默认地图Town01:{e}") + world = client.load_world('Town01') + world.wait_for_tick() + return world + +def spawn_vehicle(world, blueprint_library, spawn_points): + """生成车辆并启用基础自动驾驶(低画质版默认支持避让,无额外配置)""" + # 筛选轻量化车型(减少低画质版性能压力) + vehicle_bps = blueprint_library.filter(PREFERRED_VEHICLE) + if not vehicle_bps: + print(f"⚠️ 未找到{PREFERRED_VEHICLE},使用默认轻量化车型") + vehicle_bps = blueprint_library.filter('vehicle.*')[:1] + vehicle_bp = vehicle_bps[0] + vehicle_bp.set_attribute('role_name', 'autopilot') + + # 尝试生成车辆(多生成点重试,解决冲突问题) + max_retry = 5 + retry_count = 0 vehicle = None + while retry_count < max_retry and vehicle is None: + try: + # 随机选择生成点(增加多样性,降低冲突概率) + spawn_point = random.choice(spawn_points) + vehicle = world.spawn_actor(vehicle_bp, spawn_point) + except Exception as e: # 低画质版无专用ActorSpawnException,捕获通用异常 + retry_count += 1 + print(f"⚠️ 生成点被占用,重试第{retry_count}次...:{e}") + if vehicle is None: + print("❌ 多次重试后仍无法生成车辆") + return None + + # 启用基础自动驾驶(低画质版此操作已包含交通避让逻辑) + vehicle.set_autopilot(True) + print(f"✅ 生成车辆:{vehicle.type_id}(已启用自动驾驶+基础避让)") + return vehicle + +def spawn_camera(world, blueprint_library, vehicle): + """挂载摄像头(低画质参数配置,适配0.9.14 API)""" + try: + camera_bp = blueprint_library.find('sensor.camera.rgb') + # 设置摄像头低画质参数(确保与0.9.14 API兼容) + camera_bp.set_attribute('image_size_x', str(CAMERA_RESOLUTION[0])) + camera_bp.set_attribute('image_size_y', str(CAMERA_RESOLUTION[1])) + camera_bp.set_attribute('fov', str(CAMERA_FOV)) + camera_bp.set_attribute('sensor_tick', str(CAMERA_SENSOR_TICK)) + # 生成摄像头并挂载到车辆 + camera = world.spawn_actor(camera_bp, CAMERA_POSITION, attach_to=vehicle) + # 注册回调函数 + camera.listen(image_callback) + print("✅ 摄像头挂载成功") + return camera + except Exception as e: + print(f"❌ 摄像头挂载失败:{e}") + return None - # 检查CARLA进程是否运行 - def check_carla_running(): - import psutil - for proc in psutil.process_iter(['name']): - if proc.info['name'] == 'CarlaUE4.exe': - return True - return False +def get_vehicle_speed(vehicle): + """获取车辆当前速度(km/h,适配CARLA 0.9.14)""" + velocity = vehicle.get_velocity() + # 计算速度:√(x² + y² + z²) (m/s),转换为km/h(×3.6) + speed_ms = np.sqrt(velocity.x**2 + velocity.y**2 + velocity.z**2) + speed_kmh = speed_ms * 3.6 + return round(speed_kmh, 1) + +def main(): + """主函数:连接CARLA、随机加载地图、生成车辆、显示画面、输出速度""" + # 初始化资源 + client = None + world = None + vehicle = None + camera = None # 前置检查 print("=" * 60) @@ -57,89 +163,90 @@ def check_carla_running(): try: # 1. 连接CARLA服务器(低画质版超时延长) - client = carla.Client('127.0.0.1', 2000) - client.set_timeout(60.0) # 低画质启动慢,延长超时 - world = client.load_world('Town01') # 低画质优先用小地图Town01 - world.wait_for_tick() - print(f"✅ 连接成功!当前地图:{world.get_map().name}") + client = carla.Client(CARLA_HOST, CARLA_PORT) + client.set_timeout(CARLA_TIMEOUT) + print("✅ 连接CARLA服务器成功") + + # 2. 随机加载低画质地图 + world = load_random_map(client) - # 2. 获取蓝图和生成点 + # 3. 获取蓝图和生成点 blueprint_library = world.get_blueprint_library() spawn_points = world.get_map().get_spawn_points() if not spawn_points: print("❌ 无可用生成点,退出") return - # 3. 生成车辆(低画质选轻量化车型) - vehicle_bps = blueprint_library.filter('vehicle.seat.leon') # 轻量化车型 - if not vehicle_bps: - vehicle_bps = blueprint_library.filter('vehicle.*')[0:1] - vehicle_bp = vehicle_bps[0] - vehicle_bp.set_attribute('role_name', 'autopilot') - - # 换生成点避免占用(低画质版生成点易冲突) - vehicle = world.spawn_actor(vehicle_bp, spawn_points[10]) - vehicle.set_autopilot(True) - print(f"✅ 生成车辆:{vehicle.type_id}") + # 4. 生成车辆(带基础避让功能) + vehicle = spawn_vehicle(world, blueprint_library, spawn_points) + if vehicle is None: + return - # 4. 挂载摄像头(低画质参数) - camera_bp = blueprint_library.find('sensor.camera.rgb') - camera_bp.set_attribute('image_size_x', '640') # 降低分辨率 - camera_bp.set_attribute('image_size_y', '360') - camera_bp.set_attribute('fov', '80') - camera_bp.set_attribute('sensor_tick', '0.1') # 10fps减少压力 - camera_transform = carla.Transform(carla.Location(x=1.5, z=1.8)) - camera = world.spawn_actor(camera_bp, camera_transform, attach_to=vehicle) - camera.listen(image_callback) - print("✅ 摄像头挂载成功") + # 5. 挂载摄像头 + camera = spawn_camera(world, blueprint_library, vehicle) + if camera is None: + return - # 5. 显示画面 - print("\n📌 按 'q' 退出 | 低画质模式已启用") - cv2.namedWindow('CARLA Low-Quality View', cv2.WINDOW_NORMAL) - cv2.resizeWindow('CARLA Low-Quality View', 640, 360) + # 6. 显示画面+输出速度 + print("\n📌 操作说明:") + print(f" - 按 'q' 退出程序") + print(f" - 车辆已启用自动驾驶+基础避让功能") + cv2.namedWindow(WINDOW_NAME, cv2.WINDOW_NORMAL) + cv2.resizeWindow(WINDOW_NAME, *WINDOW_SIZE) + frame_count = 0 while True: + # 显示摄像头画面 if not IMAGE_QUEUE.empty(): img = IMAGE_QUEUE.get(timeout=0.5) - cv2.imshow('CARLA Low-Quality View', img) + cv2.imshow(WINDOW_NAME, img) + # 实时输出车辆速度(间隔打印,避免刷屏) + frame_count += 1 + if frame_count % SPEED_PRINT_INTERVAL == 0 and vehicle is not None: + current_speed = get_vehicle_speed(vehicle) + print(f"\r当前车辆速度:{current_speed} km/h", end="") + frame_count = 0 + # 按q退出 if cv2.waitKey(1) & 0xFF == ord('q'): + print("\n📌 收到退出指令,正在清理资源...") break - # 修复:低画质版CARLA异常类直接在carla模块下(无exceptions子模块) - except carla.CarlaConnectionError: - print("\n❌ 连接失败!") - print(" 解决:1. 确认CarlaUE4.exe已启动 2. 关闭防火墙 3. 检查端口2000") - except carla.ActorSpawnException: - print("\n❌ 车辆生成失败!") - print(" 解决:换生成点(如spawn_points[20])或重启CARLA") + # 异常处理(完全适配低画质版CARLA 0.9.14,无专用异常类) + except RuntimeError as e: + error_msg = str(e).lower() + if "connection" in error_msg or "timeout" in error_msg: + print("\n❌ 连接失败!") + print(f" 解决:1. 确认CarlaUE4.exe已启动 2. 关闭防火墙 3. 检查端口{CARLA_PORT}") + elif "spawn" in error_msg: + print("\n❌ 车辆生成失败(无可用生成点)!") + print(" 解决:重启CARLA或更换生成点") + else: + print(f"\n❌ 运行时错误:{e}") except AttributeError as e: print(f"\n❌ API属性错误:{e}") - print(" 解决:重新安装对应版本的whl包(低画质版CARLA需匹配whl)") + print(" 解决:1. 确认使用的是CARLA 0.9.14低画质版whl包 2. 重启CARLA和脚本") except Exception as e: print(f"\n❌ 未知错误:{e}") - import traceback traceback.print_exc() - # 清理资源 + # 清理资源(确保低画质版资源正常释放) finally: print("\n--- [清理资源] ---") if camera: - camera.stop() - camera.destroy() - print("✅ 销毁摄像头") + try: + camera.stop() + camera.destroy() + print("✅ 销毁摄像头") + except Exception as e: + print(f"⚠️ 销毁摄像头失败:{e}") if vehicle: - vehicle.destroy() - print("✅ 销毁车辆") + try: + vehicle.destroy() + print("✅ 销毁车辆") + except Exception as e: + print(f"⚠️ 销毁车辆失败:{e}") cv2.destroyAllWindows() print("✅ 程序结束") if __name__ == '__main__': - # 低画质版需额外导入psutil检查进程(可选) - try: - import psutil - except ImportError: - print("⚠️ 未安装psutil,跳过CARLA进程检查") - # 注释掉进程检查相关代码 - def check_carla_running(): - return True - main() + main() \ No newline at end of file From a7b9f47a323f7c6070caaf4b87c17341bbf423cb Mon Sep 17 00:00:00 2001 From: Yuansugu123 <2451009878@qq.com> Date: Fri, 19 Dec 2025 17:14:01 +0800 Subject: [PATCH 2/7] =?UTF-8?q?=E6=9B=B4=E6=94=B9carla=E8=B7=AF=E5=BE=84?= =?UTF-8?q?=EF=BC=8C=E9=81=BF=E8=AE=A9=E8=BD=A6=E8=BE=86=EF=BC=8C=E7=81=B5?= =?UTF-8?q?=E6=B4=BB=E6=8E=A7=E5=88=B6=E8=BD=A6=E9=80=9F?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- src/2d-carla-tracking | 1 - src/car_tracking/main.py | 176 +++++++++++++++++++++++++++++++++++---- 2 files changed, 161 insertions(+), 16 deletions(-) delete mode 160000 src/2d-carla-tracking diff --git a/src/2d-carla-tracking b/src/2d-carla-tracking deleted file mode 160000 index 713a7a59df..0000000000 --- a/src/2d-carla-tracking +++ /dev/null @@ -1 +0,0 @@ -Subproject commit 713a7a59dff78b269ea8cd2c2e140f103a7f24c8 diff --git a/src/car_tracking/main.py b/src/car_tracking/main.py index 30bff646b9..9a5e0db174 100644 --- a/src/car_tracking/main.py +++ b/src/car_tracking/main.py @@ -1,8 +1,9 @@ """ CARLA 0.9.14 低画质版专用脚本 -- 适配低画质CARLA的API差异(解决各类属性找不到问题) +- 适配低画质CARLA的API差异(解决各类属性找不到问题,包括雷达数据属性) - 解决着色器崩溃/异常类找不到问题 -- 新增:随机地图、车辆基础避让、速度控制输出 +- 新增:随机地图、车辆基础避让+主动识别车辆绕行、速度控制输出 +- 优化:CARLA路径使用环境变量+默认路径,避免绝对路径 """ import sys import os @@ -12,10 +13,28 @@ import numpy as np import queue import traceback +import math # ====================== 配置区域(可根据需求修改)====================== -# CARLA根目录 -CARLA_ROOT = 'D:/123/apps/CARLA_0.9.14/WindowsNoEditor' +# CARLA根目录:优先读取系统环境变量CARLA_ROOT,未配置则使用默认相对路径(可自行调整) +# 方案1:环境变量(推荐):在系统中配置CARLA_ROOT为你的CARLA安装路径 +# 方案2:默认路径:改为你项目的相对路径或通用默认路径 +def get_carla_root(): + """获取CARLA根目录(优先环境变量,次之用默认路径)""" + # 从环境变量读取 + carla_root = os.getenv('CARLA_ROOT') + if carla_root and os.path.exists(os.path.join(carla_root, 'CarlaUE4.exe')): + return carla_root + # 未配置环境变量时,使用默认路径(可改为相对路径,如'./CARLA_0.9.14') + default_carla_root = './CARLA_0.9.14/WindowsNoEditor' # 相对路径示例 + # 额外兜底:若相对路径不存在,使用当前工作目录的父目录(可选) + if not os.path.exists(os.path.join(default_carla_root, 'CarlaUE4.exe')): + # 也可以提示用户配置环境变量,这里暂时返回默认路径 + print(f"⚠️ 未配置CARLA_ROOT环境变量,且默认路径{default_carla_root}无效,请检查路径!") + return default_carla_root + +CARLA_ROOT = get_carla_root() + # 服务器连接配置 CARLA_HOST = '127.0.0.1' CARLA_PORT = 2000 @@ -30,6 +49,9 @@ CAMERA_FOV = 80 CAMERA_SENSOR_TICK = 0.1 # 10fps减少压力 CAMERA_POSITION = carla.Transform(carla.Location(x=1.5, z=1.8)) # 摄像头挂载位置 +# 避让配置(改用Actor距离检测,放弃雷达属性依赖) +SAFE_DISTANCE = 10.0 # 安全距离(小于此距离触发避让) +AVOIDANCE_ANGLE = 10.0 # 避让时的方向微调角度(度) # 窗口配置 WINDOW_NAME = 'CARLA Low-Quality View' WINDOW_SIZE = (640, 360) @@ -39,6 +61,8 @@ # 全局变量 IMAGE_QUEUE = queue.Queue(maxsize=1) +# 存储当前车辆的控制对象(用于避让微调) +vehicle_control = carla.VehicleControl() def check_carla_running(): """检查CARLA进程(CarlaUE4.exe)是否运行""" @@ -115,22 +139,24 @@ def spawn_vehicle(world, blueprint_library, spawn_points): return vehicle def spawn_camera(world, blueprint_library, vehicle): - """挂载摄像头(低画质参数配置,适配0.9.14 API)""" + """生成摄像头传感器(移除雷达,改用Actor直接检测,适配0.9.14低画质版)""" + camera = None try: + # 生成摄像头 camera_bp = blueprint_library.find('sensor.camera.rgb') - # 设置摄像头低画质参数(确保与0.9.14 API兼容) + # 设置摄像头低画质参数 camera_bp.set_attribute('image_size_x', str(CAMERA_RESOLUTION[0])) camera_bp.set_attribute('image_size_y', str(CAMERA_RESOLUTION[1])) camera_bp.set_attribute('fov', str(CAMERA_FOV)) camera_bp.set_attribute('sensor_tick', str(CAMERA_SENSOR_TICK)) - # 生成摄像头并挂载到车辆 camera = world.spawn_actor(camera_bp, CAMERA_POSITION, attach_to=vehicle) - # 注册回调函数 camera.listen(image_callback) print("✅ 摄像头挂载成功") return camera except Exception as e: print(f"❌ 摄像头挂载失败:{e}") + if camera: + camera.destroy() return None def get_vehicle_speed(vehicle): @@ -141,8 +167,85 @@ def get_vehicle_speed(vehicle): speed_kmh = speed_ms * 3.6 return round(speed_kmh, 1) +def detect_nearby_vehicles(world, vehicle): + """直接通过Actor检测周围车辆(放弃雷达,适配低画质版API),返回最近车辆的距离、方位和是否有车""" + if not vehicle: + return None, None, False + + # 获取当前车辆的位置和朝向 + vehicle_transform = vehicle.get_transform() + vehicle_location = vehicle_transform.location + vehicle_rotation = vehicle_transform.rotation + vehicle_yaw = math.radians(vehicle_rotation.yaw) # 车辆朝向的偏航角(弧度) + + # 存储最近的车辆信息 + min_distance = float('inf') + target_azimuth = 0.0 # 目标车辆的方位角(-180~180,正前方为0) + has_vehicle = False + + # 遍历世界中所有车辆Actor + for actor in world.get_actors().filter('vehicle.*'): + if actor.id == vehicle.id: + continue # 跳过自己 + + # 获取其他车辆的位置 + actor_location = actor.get_transform().location + # 计算两车之间的直线距离 + distance = vehicle_location.distance(actor_location) + + # 只处理安全距离内的车辆 + if distance < SAFE_DISTANCE: + # 计算目标车辆相对于当前车辆的方位角(前后左右) + # 步骤1:计算向量 + dx = actor_location.x - vehicle_location.x + dy = actor_location.y - vehicle_location.y + # 步骤2:计算目标角度(弧度) + target_angle = math.atan2(dy, dx) + # 步骤3:转换为相对于车辆朝向的方位角(度) + azimuth = math.degrees(target_angle - vehicle_yaw) + # 归一化到-180~180度 + azimuth = (azimuth + 180) % 360 - 180 + + # 只关注前方±60度的车辆(避免检测后方车辆) + if abs(azimuth) < 60.0: + min_distance = distance + target_azimuth = azimuth + has_vehicle = True + + if has_vehicle: + return min_distance, target_azimuth, True + else: + return None, None, False + +def avoid_vehicle(vehicle, distance, azimuth): + """根据检测到的车辆方位,执行避让操作(向宽敞方向微调)""" + global vehicle_control + # 获取车辆当前的控制状态(保留自动驾驶的油门/刹车,只改转向) + vehicle_control = vehicle.get_control() + + # 计算避让方向:根据方位角调整转向(-1~1之间,1为右,-1为左) + steer_strength = (AVOIDANCE_ANGLE / 60.0) * 0.2 # 控制转向强度,避免过度打方向 + if azimuth > 0: # 车辆在右侧(相对于当前车辆前方) + vehicle_control.steer = -steer_strength # 向左微调 + elif azimuth < 0: # 车辆在左侧 + vehicle_control.steer = steer_strength # 向右微调 + else: # 正前方 + vehicle_control.steer = steer_strength # 默认向右微调(宽敞方向) + + # 距离越近,转向强度稍大,同时轻微降速 + if distance < SAFE_DISTANCE / 2: + vehicle_control.steer *= 1.5 + vehicle_control.throttle = max(0.4, vehicle_control.throttle) + else: + vehicle_control.throttle = max(0.5, vehicle_control.throttle) + vehicle_control.brake = 0.0 + + # 应用控制指令(覆盖自动驾驶的转向,保留油门/刹车) + vehicle.apply_control(vehicle_control) + print(f"\n⚠️ 检测到前方车辆:距离{distance:.1f}m,方位{azimuth:.1f}度,正在向{'右' if azimuth <=0 else '左'}避让...") + def main(): - """主函数:连接CARLA、随机加载地图、生成车辆、显示画面、输出速度""" + """主函数:连接CARLA、随机加载地图、生成车辆、显示画面、检测并避让车辆""" # 初始化资源 client = None world = None @@ -177,20 +280,39 @@ def main(): print("❌ 无可用生成点,退出") return - # 4. 生成车辆(带基础避让功能) + # 4. 生成主车辆(带基础避让功能) vehicle = spawn_vehicle(world, blueprint_library, spawn_points) if vehicle is None: return - # 5. 挂载摄像头 + # 5. 生成其他车辆(增加环境中的车辆,用于测试避让) + other_vehicle_count = 5 # 生成5辆其他车辆 + spawned_count = 0 + for spawn_point in spawn_points: + if spawned_count >= other_vehicle_count: + break + # 随机选择其他车型,跳过主车辆的车型(可选) + other_vehicle_bps = [bp for bp in blueprint_library.filter('vehicle.*') if bp.id != PREFERRED_VEHICLE] + if not other_vehicle_bps: + other_vehicle_bps = blueprint_library.filter('vehicle.*') + other_vehicle_bp = random.choice(other_vehicle_bps) + try: + other_vehicle = world.spawn_actor(other_vehicle_bp, spawn_point) + other_vehicle.set_autopilot(True) + spawned_count += 1 + except Exception as e: + continue + print(f"✅ 生成了{spawned_count}辆其他车辆,用于测试避让功能") + + # 6. 挂载摄像头传感器(移除雷达,避免属性错误) camera = spawn_camera(world, blueprint_library, vehicle) if camera is None: return - # 6. 显示画面+输出速度 + # 7. 显示画面+检测车辆+避让逻辑 print("\n📌 操作说明:") print(f" - 按 'q' 退出程序") - print(f" - 车辆已启用自动驾驶+基础避让功能") + print(f" - 车辆已启用自动驾驶+主动识别车辆避让功能") cv2.namedWindow(WINDOW_NAME, cv2.WINDOW_NORMAL) cv2.resizeWindow(WINDOW_NAME, *WINDOW_SIZE) @@ -200,12 +322,25 @@ def main(): if not IMAGE_QUEUE.empty(): img = IMAGE_QUEUE.get(timeout=0.5) cv2.imshow(WINDOW_NAME, img) - # 实时输出车辆速度(间隔打印,避免刷屏) + + # 每隔一定帧数检测车辆(降低性能消耗,适配低画质版) frame_count += 1 + if frame_count % 5 == 0: # 每5帧检测一次 + distance, azimuth, has_vehicle = detect_nearby_vehicles(world, vehicle) + if has_vehicle: + avoid_vehicle(vehicle, distance, azimuth) + else: + # 无车辆时,恢复默认转向(直行) + if vehicle_control.steer != 0.0: + vehicle_control.steer = 0.0 + vehicle.apply_control(vehicle_control) + frame_count = 0 + + # 实时输出车辆速度(间隔打印,避免刷屏) if frame_count % SPEED_PRINT_INTERVAL == 0 and vehicle is not None: current_speed = get_vehicle_speed(vehicle) print(f"\r当前车辆速度:{current_speed} km/h", end="") - frame_count = 0 + # 按q退出 if cv2.waitKey(1) & 0xFF == ord('q'): print("\n📌 收到退出指令,正在清理资源...") @@ -245,6 +380,17 @@ def main(): print("✅ 销毁车辆") except Exception as e: print(f"⚠️ 销毁车辆失败:{e}") + # 销毁其他生成的车辆 + try: + if world: + for actor in world.get_actors().filter('vehicle.*'): + try: + actor.destroy() + except Exception: + pass + print("✅ 销毁所有其他车辆") + except Exception as e: + print(f"⚠️ 销毁其他车辆失败:{e}") cv2.destroyAllWindows() print("✅ 程序结束") From 0a144d2ed21245916514b667e462d4c3b0adc898 Mon Sep 17 00:00:00 2001 From: Yuansugu123 <2451009878@qq.com> Date: Fri, 19 Dec 2025 22:01:10 +0800 Subject: [PATCH 3/7] =?UTF-8?q?feat:=20=E5=8D=AB=E6=98=9F=E8=A7=86?= =?UTF-8?q?=E8=A7=92=E8=BF=BD=E8=B8=AA=E8=BD=A6=E8=BE=86=EF=BC=8C=E6=A8=A1?= =?UTF-8?q?=E6=8B=9F=E4=BA=A4=E9=80=9A=E8=BF=90=E8=A1=8C?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- src/car_tracking/README.md | 48 +++-- src/car_tracking/car_2d_ground_truth.py | 224 ++++++++++++++++++++++++ 2 files changed, 262 insertions(+), 10 deletions(-) create mode 100644 src/car_tracking/car_2d_ground_truth.py diff --git a/src/car_tracking/README.md b/src/car_tracking/README.md index 884d28e461..27a3595748 100644 --- a/src/car_tracking/README.md +++ b/src/car_tracking/README.md @@ -1,14 +1,42 @@ -# Carla-LowQuality-Viewer -CARLA 0.9.14 低画质版专用可视化脚本,针对低配置设备优化,适配低画质 CARLA 的 API 差异,解决着色器崩溃、异常类找不到等问题,实现轻量化的摄像头画面可视化+车辆自动识别交通信号灯(红灯停、绿灯行)与自动行驶功能。 + Carla 工具集 +本目录包含两个独立的Carla脚本,可直接运行,互不依赖。 -## 功能说明 -### 核心功能 -1. **2D低画质可视化**:针对低配置设备优化,适配CARLA 0.9.14低画质版的API差异,解决着色器崩溃、异常类找不到等问题。 -2. **交通信号灯识别**:车辆可自动识别交通信号灯状态,实现红灯停、绿灯行的逻辑。 -3. **车辆自动行驶**:结合信号灯识别功能,完成轻量化的自动行驶控制。 +## 程序1:carla_2d_ground_truth.py +### 功能 +将车辆3D包围盒投影为2D地面真值边界框并可视化,适用于目标检测算法验证与数据集采集。 + +### 环境要求 +- Carla 版本:0.9.10~0.9.15 +- Python 版本:3.7 +- 依赖包:`opencv-python` `numpy` `carla`(版本需与Carla模拟器匹配) + +### 运行命令 +```bash +# 第一步:启动对应版本的Carla模拟器 +# 第二步:执行脚本 +python car_2d_ground_truth.py +``` + +### 操作说明 +- 可视化窗口显示相机画面与车辆2D边界框 +- 按 **q** 键退出程序 + +## 程序2:carla_low_quality_viewer.py +### 功能 +Carla 0.9.14低画质版专用脚本,实现车辆自动行驶+提前避让(车辆/障碍物/红绿灯),修复低配置设备进程卡顿问题,降低资源占用。 + +### 环境要求 +- Carla 版本:**仅限0.9.14低画质版** +- Python 版本:3.7 +- 依赖包:`opencv-python` `numpy` `carla==0.9.14`(可选`psutil`用于进程检测) -## 快速开始 ### 运行命令 -在项目目录下执行以下命令,启动可视化与自动行驶功能: ```bash -python main.py \ No newline at end of file +# 第一步:启动Carla 0.9.14低画质版模拟器 +# 第二步:执行脚本 +python main.py +``` + +### 操作说明 +- 可视化窗口显示摄像头画面,终端实时输出车速与避让提示 +- 按 **q** 键退出程序 \ No newline at end of file diff --git a/src/car_tracking/car_2d_ground_truth.py b/src/car_tracking/car_2d_ground_truth.py new file mode 100644 index 0000000000..b6a4987a40 --- /dev/null +++ b/src/car_tracking/car_2d_ground_truth.py @@ -0,0 +1,224 @@ +import carla +import queue +import random +import cv2 +import numpy as np +import math +import colorsys + +# ===================== 核心工具函数(整合原utils的核心功能,无需外部依赖) ===================== +# 1. 绘制边界框(原utils.box_utils.draw_bounding_boxes) +def draw_bounding_boxes(image, boxes, labels, class_names, ids=None, scores=None): + """在图像上绘制边界框,包含类别、ID、置信度""" + num_classes = len(class_names) + # 为每个类别生成唯一颜色 + hsv_tuples = [(x / num_classes, 1., 1.) for x in range(num_classes)] + colors = list(map(lambda x: colorsys.hsv_to_rgb(*x), hsv_tuples)) + colors = list(map(lambda x: (int(x[0]*255), int(x[1]*255), int(x[2]*255)), colors)) + + image_copy = image.copy() + for i, box in enumerate(boxes): + x1, y1, x2, y2 = box.astype(int) + label = labels[i] + class_name = class_names[label] if label < len(class_names) else 'unknown' + color = colors[label % num_classes] + + # 绘制矩形框 + cv2.rectangle(image_copy, (x1, y1), (x2, y2), color, 2) + # 准备文本(类别+ID) + text = f"{class_name} (ID: {ids[i]})" if ids and i < len(ids) else class_name + if scores and i < len(scores): + text += f" {scores[i]:.2f}" + + # 绘制文本背景和文字 + text_size = cv2.getTextSize(text, cv2.FONT_HERSHEY_SIMPLEX, 0.5, 1)[0] + text_y = y1 - 10 if y1 - 10 > 10 else y1 + text_size[1] + 10 + cv2.rectangle(image_copy, (x1, text_y - text_size[1] - 2), + (x1 + text_size[0], text_y + 2), color, -1) + cv2.putText(image_copy, text, (x1, text_y), cv2.FONT_HERSHEY_SIMPLEX, + 0.5, (255, 255, 255), 1) + + return image_copy + +# 2. 构建相机投影矩阵(原utils.projection.build_projection_matrix) +def build_projection_matrix(w, h, fov, is_behind_camera=False): + """将3D世界坐标投影到2D图像的矩阵""" + focal = w / (2.0 * math.tan(fov * math.pi / 360.0)) + K = np.identity(3) + K[0, 0] = K[1, 1] = focal + K[0, 2] = w / 2.0 + K[1, 2] = h / 2.0 + if is_behind_camera: + K[2, 2] = -1 # 反转z轴处理相机后方的点 + return K + +# 3. 3D点转2D图像点(原utils.projection.get_image_point) +def get_image_point(loc, K, w2c): + """将Carla的3D位置转换为2D图像坐标""" + point = np.array([loc.x, loc.y, loc.z, 1.0]) + point_camera = np.dot(w2c, point) # 世界→相机 + point_img = np.dot(K, point_camera[:3]) # 相机→图像 + point_img = point_img / point_img[2] # 归一化 + return (point_img[0], point_img[1]) + +# 4. 从3D边缘生成2D边界框(原utils.projection.get_2d_box_from_3d_edges) +def get_2d_box_from_3d_edges(points_2d, edges, h, w): + """从3D点的2D投影生成最小包围框""" + x_coords = [p[0] for p in points_2d] + y_coords = [p[1] for p in points_2d] + x_min = max(0, min(x_coords)) + x_max = min(w, max(x_coords)) + y_min = max(0, min(y_coords)) + y_max = min(h, max(y_coords)) + return x_min, x_max, y_min, y_max + +# 5. 判断点是否在图像内(原utils.projection.point_in_canvas) +def point_in_canvas(point, h, w): + """检查2D点是否在图像画布范围内""" + x, y = point + return 0 <= x < w and 0 <= y < h + +# 6. 清理Carla中的车辆(原utils.world.clear_npc/clear_static_vehicle/clear) +def clear_actors(world, camera=None): + """销毁所有车辆和相机传感器""" + if camera: + camera.destroy() + for actor in world.get_actors().filter('*vehicle*'): + actor.destroy() + +# ===================== COCO类别名称(对应车辆类别) ===================== +# 无需依赖what库,直接定义核心类别 +COCO_CLASS_NAMES = [ + 'person', 'bicycle', 'car', 'motorcycle', 'airplane', + 'bus', 'train', 'truck', 'boat', 'traffic light' +] + +# ===================== 相机回调函数 ===================== +def camera_callback(image, rgb_image_queue): + """将相机图像存入队列""" + rgb_image_queue.put(np.reshape(np.copy(image.raw_data), (image.height, image.width, 4))) + +# ===================== 主程序 ===================== +if __name__ == '__main__': + # 1. 连接Carla客户端 + client = carla.Client('localhost', 2000) + client.set_timeout(20.0) + world = client.get_world() + + # 2. 设置Carla同步模式(稳定控制帧率) + settings = world.get_settings() + settings.synchronous_mode = True + settings.fixed_delta_seconds = 0.05 # 20 FPS + world.apply_settings(settings) + + # 3. 获取核心资源:蓝图库、出生点、观察者 + bp_lib = world.get_blueprint_library() + spawn_points = world.get_map().get_spawn_points() + spectator = world.get_spectator() + + # 4. 生成自车(林肯MKZ) + vehicle_bp = bp_lib.find('vehicle.lincoln.mkz_2020') + vehicle = world.try_spawn_actor(vehicle_bp, random.choice(spawn_points)) + # 防止出生点冲突,重试生成 + while not vehicle: + vehicle = world.try_spawn_actor(vehicle_bp, random.choice(spawn_points)) + + # 5. 生成相机传感器(挂载在自车前方) + camera_bp = bp_lib.find('sensor.camera.rgb') + camera_bp.set_attribute('image_size_x', '640') + camera_bp.set_attribute('image_size_y', '640') + camera_transform = carla.Transform(carla.Location(x=1.0, z=2.0)) # 前视相机 + camera = world.spawn_actor(camera_bp, camera_transform, attach_to=vehicle) + + # 6. 初始化图像队列,监听相机数据 + image_queue = queue.Queue() + camera.listen(lambda img: camera_callback(img, image_queue)) + + # 7. 清理原有车辆,生成50辆NPC车辆(开启自动驾驶) + clear_actors(world) + for i in range(50): + # 筛选4轮车辆,排除自行车 + vehicle_bps = [bp for bp in bp_lib.filter('vehicle') if int(bp.get_attribute('number_of_wheels')) == 4] + npc = world.try_spawn_actor(random.choice(vehicle_bps), random.choice(spawn_points)) + if npc: + npc.set_autopilot(True) + vehicle.set_autopilot(True) # 自车也开启自动驾驶 + + # 8. 初始化3D→2D投影的参数 + edges = [[0,1],[1,3],[3,2],[2,0],[0,4],[4,5],[5,1],[5,7],[7,6],[6,4],[6,2],[7,3]] # 车辆包围盒边缘 + image_w = 640 + image_h = 640 + fov = camera_bp.get_attribute('fov').as_float() + K = build_projection_matrix(image_w, image_h, fov) + K_b = build_projection_matrix(image_w, image_h, fov, is_behind_camera=True) + + # ===================== 主循环:实时可视化地面真值 ===================== + print("程序已启动,按q键退出...") + try: + while True: + # 推进Carla仿真一帧 + world.tick() + + # 移动观察者到自车上方(俯视视角) + spectator_transform = carla.Transform( + vehicle.get_transform().transform(carla.Location(x=-4, z=50)), + carla.Rotation(yaw=-180, pitch=-90) + ) + spectator.set_transform(spectator_transform) + + # 获取相机图像 + image = image_queue.get() + # 获取世界→相机的变换矩阵 + world_2_camera = np.array(camera.get_transform().get_inverse_matrix()) + + # 遍历所有车辆,计算2D边界框 + boxes = [] + ids = [] + for npc in world.get_actors().filter('*vehicle*'): + # 跳过自车 + if npc.id == vehicle.id: + continue + + # 筛选50米内、自车前方的车辆 + dist = npc.get_transform().location.distance(vehicle.get_transform().location) + forward_vec = vehicle.get_transform().get_forward_vector() + ray = npc.get_transform().location - vehicle.get_transform().location + if dist < 50 and forward_vec.dot(ray) > 0: + # 获取车辆包围盒的3D顶点 + bb_verts = [v for v in npc.bounding_box.get_world_vertices(npc.get_transform())] + points_2d = [] + # 将每个3D顶点投影到2D图像 + for vert in bb_verts: + ray_cam = vert - camera.get_transform().location + cam_forward = camera.get_transform().get_forward_vector() + if cam_forward.dot(ray_cam) > 0: + p = get_image_point(vert, K, world_2_camera) + else: + p = get_image_point(vert, K_b, world_2_camera) + points_2d.append(p) + # 生成2D边界框 + x_min, x_max, y_min, y_max = get_2d_box_from_3d_edges(points_2d, edges, image_h, image_w) + # 过滤过小的边界框 + if (y_max - y_min)*(x_max - x_min) > 100 and (x_max - x_min) > 20: + if point_in_canvas((x_min, y_min), image_h, image_w) and point_in_canvas((x_max, y_max), image_h, image_w): + boxes.append(np.array([x_min, y_min, x_max, y_max])) + ids.append(npc.id) + + # 绘制边界框(标签2对应COCO的car类别) + boxes = np.array(boxes) + labels = np.array([2] * len(boxes)) # 2 = car(对应COCO_CLASS_NAMES[2]) + output_image = draw_bounding_boxes(image, boxes, labels, COCO_CLASS_NAMES, ids) + + # 显示图像 + cv2.imshow('Carla 2D Ground Truth (Vehicles)', output_image) + # 按q键退出 + if cv2.waitKey(1) & 0xFF == ord('q'): + break + + except KeyboardInterrupt: + pass + finally: + # 清理资源 + clear_actors(world, camera) + cv2.destroyAllWindows() + print("程序已退出,资源已清理") \ No newline at end of file From c300df080829dccc3e5b98d41c34b53305e1e385 Mon Sep 17 00:00:00 2001 From: Yuansugu123 <2451009878@qq.com> Date: Fri, 19 Dec 2025 22:48:56 +0800 Subject: [PATCH 4/7] =?UTF-8?q?feat(car=5Ftracking):=20=E4=BD=A0=E7=9A=84?= =?UTF-8?q?=E6=96=B0=E4=BF=AE=E6=94=B9=E6=8F=8F=E8=BF=B0?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- src/car_tracking/car_2d_ground_truth.py | 224 -------- src/car_tracking/main.py | 691 +++++++++++++----------- 2 files changed, 370 insertions(+), 545 deletions(-) delete mode 100644 src/car_tracking/car_2d_ground_truth.py diff --git a/src/car_tracking/car_2d_ground_truth.py b/src/car_tracking/car_2d_ground_truth.py deleted file mode 100644 index b6a4987a40..0000000000 --- a/src/car_tracking/car_2d_ground_truth.py +++ /dev/null @@ -1,224 +0,0 @@ -import carla -import queue -import random -import cv2 -import numpy as np -import math -import colorsys - -# ===================== 核心工具函数(整合原utils的核心功能,无需外部依赖) ===================== -# 1. 绘制边界框(原utils.box_utils.draw_bounding_boxes) -def draw_bounding_boxes(image, boxes, labels, class_names, ids=None, scores=None): - """在图像上绘制边界框,包含类别、ID、置信度""" - num_classes = len(class_names) - # 为每个类别生成唯一颜色 - hsv_tuples = [(x / num_classes, 1., 1.) for x in range(num_classes)] - colors = list(map(lambda x: colorsys.hsv_to_rgb(*x), hsv_tuples)) - colors = list(map(lambda x: (int(x[0]*255), int(x[1]*255), int(x[2]*255)), colors)) - - image_copy = image.copy() - for i, box in enumerate(boxes): - x1, y1, x2, y2 = box.astype(int) - label = labels[i] - class_name = class_names[label] if label < len(class_names) else 'unknown' - color = colors[label % num_classes] - - # 绘制矩形框 - cv2.rectangle(image_copy, (x1, y1), (x2, y2), color, 2) - # 准备文本(类别+ID) - text = f"{class_name} (ID: {ids[i]})" if ids and i < len(ids) else class_name - if scores and i < len(scores): - text += f" {scores[i]:.2f}" - - # 绘制文本背景和文字 - text_size = cv2.getTextSize(text, cv2.FONT_HERSHEY_SIMPLEX, 0.5, 1)[0] - text_y = y1 - 10 if y1 - 10 > 10 else y1 + text_size[1] + 10 - cv2.rectangle(image_copy, (x1, text_y - text_size[1] - 2), - (x1 + text_size[0], text_y + 2), color, -1) - cv2.putText(image_copy, text, (x1, text_y), cv2.FONT_HERSHEY_SIMPLEX, - 0.5, (255, 255, 255), 1) - - return image_copy - -# 2. 构建相机投影矩阵(原utils.projection.build_projection_matrix) -def build_projection_matrix(w, h, fov, is_behind_camera=False): - """将3D世界坐标投影到2D图像的矩阵""" - focal = w / (2.0 * math.tan(fov * math.pi / 360.0)) - K = np.identity(3) - K[0, 0] = K[1, 1] = focal - K[0, 2] = w / 2.0 - K[1, 2] = h / 2.0 - if is_behind_camera: - K[2, 2] = -1 # 反转z轴处理相机后方的点 - return K - -# 3. 3D点转2D图像点(原utils.projection.get_image_point) -def get_image_point(loc, K, w2c): - """将Carla的3D位置转换为2D图像坐标""" - point = np.array([loc.x, loc.y, loc.z, 1.0]) - point_camera = np.dot(w2c, point) # 世界→相机 - point_img = np.dot(K, point_camera[:3]) # 相机→图像 - point_img = point_img / point_img[2] # 归一化 - return (point_img[0], point_img[1]) - -# 4. 从3D边缘生成2D边界框(原utils.projection.get_2d_box_from_3d_edges) -def get_2d_box_from_3d_edges(points_2d, edges, h, w): - """从3D点的2D投影生成最小包围框""" - x_coords = [p[0] for p in points_2d] - y_coords = [p[1] for p in points_2d] - x_min = max(0, min(x_coords)) - x_max = min(w, max(x_coords)) - y_min = max(0, min(y_coords)) - y_max = min(h, max(y_coords)) - return x_min, x_max, y_min, y_max - -# 5. 判断点是否在图像内(原utils.projection.point_in_canvas) -def point_in_canvas(point, h, w): - """检查2D点是否在图像画布范围内""" - x, y = point - return 0 <= x < w and 0 <= y < h - -# 6. 清理Carla中的车辆(原utils.world.clear_npc/clear_static_vehicle/clear) -def clear_actors(world, camera=None): - """销毁所有车辆和相机传感器""" - if camera: - camera.destroy() - for actor in world.get_actors().filter('*vehicle*'): - actor.destroy() - -# ===================== COCO类别名称(对应车辆类别) ===================== -# 无需依赖what库,直接定义核心类别 -COCO_CLASS_NAMES = [ - 'person', 'bicycle', 'car', 'motorcycle', 'airplane', - 'bus', 'train', 'truck', 'boat', 'traffic light' -] - -# ===================== 相机回调函数 ===================== -def camera_callback(image, rgb_image_queue): - """将相机图像存入队列""" - rgb_image_queue.put(np.reshape(np.copy(image.raw_data), (image.height, image.width, 4))) - -# ===================== 主程序 ===================== -if __name__ == '__main__': - # 1. 连接Carla客户端 - client = carla.Client('localhost', 2000) - client.set_timeout(20.0) - world = client.get_world() - - # 2. 设置Carla同步模式(稳定控制帧率) - settings = world.get_settings() - settings.synchronous_mode = True - settings.fixed_delta_seconds = 0.05 # 20 FPS - world.apply_settings(settings) - - # 3. 获取核心资源:蓝图库、出生点、观察者 - bp_lib = world.get_blueprint_library() - spawn_points = world.get_map().get_spawn_points() - spectator = world.get_spectator() - - # 4. 生成自车(林肯MKZ) - vehicle_bp = bp_lib.find('vehicle.lincoln.mkz_2020') - vehicle = world.try_spawn_actor(vehicle_bp, random.choice(spawn_points)) - # 防止出生点冲突,重试生成 - while not vehicle: - vehicle = world.try_spawn_actor(vehicle_bp, random.choice(spawn_points)) - - # 5. 生成相机传感器(挂载在自车前方) - camera_bp = bp_lib.find('sensor.camera.rgb') - camera_bp.set_attribute('image_size_x', '640') - camera_bp.set_attribute('image_size_y', '640') - camera_transform = carla.Transform(carla.Location(x=1.0, z=2.0)) # 前视相机 - camera = world.spawn_actor(camera_bp, camera_transform, attach_to=vehicle) - - # 6. 初始化图像队列,监听相机数据 - image_queue = queue.Queue() - camera.listen(lambda img: camera_callback(img, image_queue)) - - # 7. 清理原有车辆,生成50辆NPC车辆(开启自动驾驶) - clear_actors(world) - for i in range(50): - # 筛选4轮车辆,排除自行车 - vehicle_bps = [bp for bp in bp_lib.filter('vehicle') if int(bp.get_attribute('number_of_wheels')) == 4] - npc = world.try_spawn_actor(random.choice(vehicle_bps), random.choice(spawn_points)) - if npc: - npc.set_autopilot(True) - vehicle.set_autopilot(True) # 自车也开启自动驾驶 - - # 8. 初始化3D→2D投影的参数 - edges = [[0,1],[1,3],[3,2],[2,0],[0,4],[4,5],[5,1],[5,7],[7,6],[6,4],[6,2],[7,3]] # 车辆包围盒边缘 - image_w = 640 - image_h = 640 - fov = camera_bp.get_attribute('fov').as_float() - K = build_projection_matrix(image_w, image_h, fov) - K_b = build_projection_matrix(image_w, image_h, fov, is_behind_camera=True) - - # ===================== 主循环:实时可视化地面真值 ===================== - print("程序已启动,按q键退出...") - try: - while True: - # 推进Carla仿真一帧 - world.tick() - - # 移动观察者到自车上方(俯视视角) - spectator_transform = carla.Transform( - vehicle.get_transform().transform(carla.Location(x=-4, z=50)), - carla.Rotation(yaw=-180, pitch=-90) - ) - spectator.set_transform(spectator_transform) - - # 获取相机图像 - image = image_queue.get() - # 获取世界→相机的变换矩阵 - world_2_camera = np.array(camera.get_transform().get_inverse_matrix()) - - # 遍历所有车辆,计算2D边界框 - boxes = [] - ids = [] - for npc in world.get_actors().filter('*vehicle*'): - # 跳过自车 - if npc.id == vehicle.id: - continue - - # 筛选50米内、自车前方的车辆 - dist = npc.get_transform().location.distance(vehicle.get_transform().location) - forward_vec = vehicle.get_transform().get_forward_vector() - ray = npc.get_transform().location - vehicle.get_transform().location - if dist < 50 and forward_vec.dot(ray) > 0: - # 获取车辆包围盒的3D顶点 - bb_verts = [v for v in npc.bounding_box.get_world_vertices(npc.get_transform())] - points_2d = [] - # 将每个3D顶点投影到2D图像 - for vert in bb_verts: - ray_cam = vert - camera.get_transform().location - cam_forward = camera.get_transform().get_forward_vector() - if cam_forward.dot(ray_cam) > 0: - p = get_image_point(vert, K, world_2_camera) - else: - p = get_image_point(vert, K_b, world_2_camera) - points_2d.append(p) - # 生成2D边界框 - x_min, x_max, y_min, y_max = get_2d_box_from_3d_edges(points_2d, edges, image_h, image_w) - # 过滤过小的边界框 - if (y_max - y_min)*(x_max - x_min) > 100 and (x_max - x_min) > 20: - if point_in_canvas((x_min, y_min), image_h, image_w) and point_in_canvas((x_max, y_max), image_h, image_w): - boxes.append(np.array([x_min, y_min, x_max, y_max])) - ids.append(npc.id) - - # 绘制边界框(标签2对应COCO的car类别) - boxes = np.array(boxes) - labels = np.array([2] * len(boxes)) # 2 = car(对应COCO_CLASS_NAMES[2]) - output_image = draw_bounding_boxes(image, boxes, labels, COCO_CLASS_NAMES, ids) - - # 显示图像 - cv2.imshow('Carla 2D Ground Truth (Vehicles)', output_image) - # 按q键退出 - if cv2.waitKey(1) & 0xFF == ord('q'): - break - - except KeyboardInterrupt: - pass - finally: - # 清理资源 - clear_actors(world, camera) - cv2.destroyAllWindows() - print("程序已退出,资源已清理") \ No newline at end of file diff --git a/src/car_tracking/main.py b/src/car_tracking/main.py index 9a5e0db174..65e3f003cc 100644 --- a/src/car_tracking/main.py +++ b/src/car_tracking/main.py @@ -1,262 +1,163 @@ """ CARLA 0.9.14 低画质版专用脚本 -- 适配低画质CARLA的API差异(解决各类属性找不到问题,包括雷达数据属性) -- 解决着色器崩溃/异常类找不到问题 -- 新增:随机地图、车辆基础避让+主动识别车辆绕行、速度控制输出 -- 优化:CARLA路径使用环境变量+默认路径,避免绝对路径 +- 适配0.9.14 API差异(移除road_type、修正Vector2D.is_zero、修正异常处理) +- 实现严格的红灯停、绿灯行逻辑 +- 车辆沿道路正常行驶,路口自然拐弯(兼容0.9.14) +- 支持地图随机生成 +- 新增:3D车辆转2D边界框可视化(含车辆ID、距离筛选) """ import sys import os -import random import carla import cv2 import numpy as np import queue -import traceback import math - -# ====================== 配置区域(可根据需求修改)====================== -# CARLA根目录:优先读取系统环境变量CARLA_ROOT,未配置则使用默认相对路径(可自行调整) -# 方案1:环境变量(推荐):在系统中配置CARLA_ROOT为你的CARLA安装路径 -# 方案2:默认路径:改为你项目的相对路径或通用默认路径 -def get_carla_root(): - """获取CARLA根目录(优先环境变量,次之用默认路径)""" - # 从环境变量读取 - carla_root = os.getenv('CARLA_ROOT') - if carla_root and os.path.exists(os.path.join(carla_root, 'CarlaUE4.exe')): - return carla_root - # 未配置环境变量时,使用默认路径(可改为相对路径,如'./CARLA_0.9.14') - default_carla_root = './CARLA_0.9.14/WindowsNoEditor' # 相对路径示例 - # 额外兜底:若相对路径不存在,使用当前工作目录的父目录(可选) - if not os.path.exists(os.path.join(default_carla_root, 'CarlaUE4.exe')): - # 也可以提示用户配置环境变量,这里暂时返回默认路径 - print(f"⚠️ 未配置CARLA_ROOT环境变量,且默认路径{default_carla_root}无效,请检查路径!") - return default_carla_root - -CARLA_ROOT = get_carla_root() - -# 服务器连接配置 -CARLA_HOST = '127.0.0.1' -CARLA_PORT = 2000 -CARLA_TIMEOUT = 60.0 # 低画质启动慢,延长超时 -# 支持的低画质小地图列表(CARLA 0.9.14) -# 注:低画质版建议只使用小地图Town01/Town02,避免卡顿 -SUPPORTED_MAPS = ['Town01', 'Town02'] -# 车辆配置 -PREFERRED_VEHICLE = 'vehicle.seat.leon' # 轻量化车型 -# 摄像头配置(低画质参数) -CAMERA_RESOLUTION = (640, 360) # 降低分辨率 -CAMERA_FOV = 80 -CAMERA_SENSOR_TICK = 0.1 # 10fps减少压力 -CAMERA_POSITION = carla.Transform(carla.Location(x=1.5, z=1.8)) # 摄像头挂载位置 -# 避让配置(改用Actor距离检测,放弃雷达属性依赖) -SAFE_DISTANCE = 10.0 # 安全距离(小于此距离触发避让) -AVOIDANCE_ANGLE = 10.0 # 避让时的方向微调角度(度) -# 窗口配置 -WINDOW_NAME = 'CARLA Low-Quality View' -WINDOW_SIZE = (640, 360) -# 速度显示配置(低画质版无法通过API限制速度,仅显示实际速度) -SPEED_PRINT_INTERVAL = 10 # 每10帧打印一次速度(减少刷屏) -# ===================================================================== +import random +import colorsys # 用于生成边界框颜色 # 全局变量 -IMAGE_QUEUE = queue.Queue(maxsize=1) -# 存储当前车辆的控制对象(用于避让微调) -vehicle_control = carla.VehicleControl() - -def check_carla_running(): - """检查CARLA进程(CarlaUE4.exe)是否运行""" - try: - import psutil - for proc in psutil.process_iter(['name']): - if proc.info['name'] == 'CarlaUE4.exe': - return True - return False - except ImportError: - print("⚠️ 未安装psutil,无法检查CARLA进程,默认认为进程已运行") - return True - +IMAGE_QUEUE = queue.Queue(maxsize=2) # 增大队列,避免图像丢失 +LATEST_IMAGE = None # 存储最新图像,防止黑屏 +# 替换为你的低画质CARLA实际路径 +CARLA_ROOT = 'D:/123/apps/CARLA_0.9.14/WindowsNoEditor' + +# ===================== 3D转2D可视化核心工具函数(适配0.9.14) ===================== +def build_projection_matrix(w, h, fov, is_behind_camera=False): + """构建相机投影矩阵(将3D世界坐标投影到2D图像)""" + focal = w / (2.0 * math.tan(fov * math.pi / 360.0)) + K = np.identity(3) + K[0, 0] = K[1, 1] = focal + K[0, 2] = w / 2.0 + K[1, 2] = h / 2.0 + if is_behind_camera: + K[2, 2] = -1 # 反转z轴处理相机后方的点 + return K + +def get_image_point(loc, K, w2c): + """将Carla的3D位置转换为2D图像坐标(适配0.9.14)""" + point = np.array([loc.x, loc.y, loc.z, 1.0]) + point_camera = np.dot(w2c, point) # 世界→相机 + point_img = np.dot(K, point_camera[:3]) # 相机→图像 + if point_img[2] != 0: # 避免除零错误(适配低画质版) + point_img = point_img / point_img[2] # 归一化 + return (point_img[0], point_img[1]) + +def get_2d_box_from_3d_edges(points_2d, h, w): + """从3D点的2D投影生成最小包围框""" + x_coords = [p[0] for p in points_2d] + y_coords = [p[1] for p in points_2d] + x_min = max(0, min(x_coords)) + x_max = min(w, max(x_coords)) + y_min = max(0, min(y_coords)) + y_max = min(h, max(y_coords)) + return x_min, y_min, x_max, y_max # 调整返回顺序,匹配cv2.rectangle + +def point_in_canvas(point, h, w): + """检查2D点是否在图像画布范围内""" + x, y = point + return 0 <= x < w and 0 <= y < h + +def generate_color(class_id, num_classes=1): + """为类别生成唯一颜色(这里仅车辆,固定颜色)""" + hsv_tuples = [(x / num_classes, 1., 1.) for x in range(num_classes)] + colors = list(map(lambda x: colorsys.hsv_to_rgb(*x), hsv_tuples)) + colors = list(map(lambda x: (int(x[0]*255), int(x[1]*255), int(x[2]*255)), colors)) + return colors[class_id % num_classes] + +def draw_bounding_boxes(image, boxes, ids, class_names): + """在图像上绘制边界框(含车辆ID)""" + image_copy = image.copy() + color = generate_color(0) # 车辆类别固定为0,颜色为红色系 + for i, box in enumerate(boxes): + x1, y1, x2, y2 = box.astype(int) + # 绘制矩形框 + cv2.rectangle(image_copy, (x1, y1), (x2, y2), color, 2) + # 准备文本(车辆ID) + text = f"Car (ID: {ids[i]})" if ids and i < len(ids) else "Car" + # 绘制文本背景和文字 + text_size = cv2.getTextSize(text, cv2.FONT_HERSHEY_SIMPLEX, 0.5, 1)[0] + text_y = y1 - 10 if y1 - 10 > 10 else y1 + text_size[1] + 10 + cv2.rectangle(image_copy, (x1, text_y - text_size[1] - 2), + (x1 + text_size[0], text_y + 2), color, -1) + cv2.putText(image_copy, text, (x1, text_y), cv2.FONT_HERSHEY_SIMPLEX, + 0.5, (255, 255, 255), 1) + return image_copy + +# ===================== 原脚本核心功能函数 ===================== +# 摄像头回调函数(低画质适配,新增图像存储) def image_callback(image): - """摄像头回调函数(低画质适配:降低分辨率减少压力)""" + global LATEST_IMAGE try: - # 将原始数据转换为BGRA格式的numpy数组 img_bgra = np.frombuffer(image.raw_data, dtype=np.uint8) img_bgra = img_bgra.reshape((image.height, image.width, 4)) - # 转换为BGR格式(适配OpenCV) img_bgr = cv2.cvtColor(img_bgra, cv2.COLOR_BGRA2BGR) - # 低画质优化:缩小图像尺寸(减少CV窗口渲染压力) - img_bgr = cv2.resize(img_bgr, WINDOW_SIZE) - # 保证队列中只有最新的一帧图像,避免堆积 - with IMAGE_QUEUE.mutex: - IMAGE_QUEUE.queue.clear() + img_bgr = cv2.resize(img_bgr, (640, 360)) # 统一分辨率 + + # 存储最新图像,防止黑屏 + LATEST_IMAGE = img_bgr.copy() + + # 处理队列,避免满溢 + if IMAGE_QUEUE.full(): + IMAGE_QUEUE.get_nowait() IMAGE_QUEUE.put(img_bgr, timeout=0.1) except Exception as e: print(f"⚠️ 图像回调出错:{e}") -def load_random_map(client): - """随机加载支持的低画质小地图(适配0.9.14)""" - random_map = random.choice(SUPPORTED_MAPS) - try: - world = client.load_world(random_map) - world.wait_for_tick() - print(f"✅ 随机加载地图成功:{random_map}(当前地图路径:{world.get_map().name})") - return world - except Exception as e: - print(f"❌ 加载地图{random_map}失败,使用默认地图Town01:{e}") - world = client.load_world('Town01') - world.wait_for_tick() - return world - -def spawn_vehicle(world, blueprint_library, spawn_points): - """生成车辆并启用基础自动驾驶(低画质版默认支持避让,无额外配置)""" - # 筛选轻量化车型(减少低画质版性能压力) - vehicle_bps = blueprint_library.filter(PREFERRED_VEHICLE) - if not vehicle_bps: - print(f"⚠️ 未找到{PREFERRED_VEHICLE},使用默认轻量化车型") - vehicle_bps = blueprint_library.filter('vehicle.*')[:1] - vehicle_bp = vehicle_bps[0] - vehicle_bp.set_attribute('role_name', 'autopilot') - - # 尝试生成车辆(多生成点重试,解决冲突问题) - max_retry = 5 - retry_count = 0 - vehicle = None - while retry_count < max_retry and vehicle is None: - try: - # 随机选择生成点(增加多样性,降低冲突概率) - spawn_point = random.choice(spawn_points) - vehicle = world.spawn_actor(vehicle_bp, spawn_point) - except Exception as e: # 低画质版无专用ActorSpawnException,捕获通用异常 - retry_count += 1 - print(f"⚠️ 生成点被占用,重试第{retry_count}次...:{e}") - if vehicle is None: - print("❌ 多次重试后仍无法生成车辆") - return None - - # 启用基础自动驾驶(低画质版此操作已包含交通避让逻辑) - vehicle.set_autopilot(True) - print(f"✅ 生成车辆:{vehicle.type_id}(已启用自动驾驶+基础避让)") - return vehicle - -def spawn_camera(world, blueprint_library, vehicle): - """生成摄像头传感器(移除雷达,改用Actor直接检测,适配0.9.14低画质版)""" - camera = None - try: - # 生成摄像头 - camera_bp = blueprint_library.find('sensor.camera.rgb') - # 设置摄像头低画质参数 - camera_bp.set_attribute('image_size_x', str(CAMERA_RESOLUTION[0])) - camera_bp.set_attribute('image_size_y', str(CAMERA_RESOLUTION[1])) - camera_bp.set_attribute('fov', str(CAMERA_FOV)) - camera_bp.set_attribute('sensor_tick', str(CAMERA_SENSOR_TICK)) - camera = world.spawn_actor(camera_bp, CAMERA_POSITION, attach_to=vehicle) - camera.listen(image_callback) - print("✅ 摄像头挂载成功") - return camera - except Exception as e: - print(f"❌ 摄像头挂载失败:{e}") - if camera: - camera.destroy() - return None - -def get_vehicle_speed(vehicle): - """获取车辆当前速度(km/h,适配CARLA 0.9.14)""" - velocity = vehicle.get_velocity() - # 计算速度:√(x² + y² + z²) (m/s),转换为km/h(×3.6) - speed_ms = np.sqrt(velocity.x**2 + velocity.y**2 + velocity.z**2) - speed_kmh = speed_ms * 3.6 - return round(speed_kmh, 1) - -def detect_nearby_vehicles(world, vehicle): - """直接通过Actor检测周围车辆(放弃雷达,适配低画质版API),返回最近车辆的距离、方位和是否有车""" - if not vehicle: - return None, None, False - - # 获取当前车辆的位置和朝向 - vehicle_transform = vehicle.get_transform() - vehicle_location = vehicle_transform.location - vehicle_rotation = vehicle_transform.rotation - vehicle_yaw = math.radians(vehicle_rotation.yaw) # 车辆朝向的偏航角(弧度) - - # 存储最近的车辆信息 - min_distance = float('inf') - target_azimuth = 0.0 # 目标车辆的方位角(-180~180,正前方为0) - has_vehicle = False - - # 遍历世界中所有车辆Actor - for actor in world.get_actors().filter('vehicle.*'): - if actor.id == vehicle.id: - continue # 跳过自己 - - # 获取其他车辆的位置 - actor_location = actor.get_transform().location - # 计算两车之间的直线距离 - distance = vehicle_location.distance(actor_location) - - # 只处理安全距离内的车辆 - if distance < SAFE_DISTANCE: - # 计算目标车辆相对于当前车辆的方位角(前后左右) - # 步骤1:计算向量 - dx = actor_location.x - vehicle_location.x - dy = actor_location.y - vehicle_location.y - # 步骤2:计算目标角度(弧度) - target_angle = math.atan2(dy, dx) - # 步骤3:转换为相对于车辆朝向的方位角(度) - azimuth = math.degrees(target_angle - vehicle_yaw) - # 归一化到-180~180度 - azimuth = (azimuth + 180) % 360 - 180 - - # 只关注前方±60度的车辆(避免检测后方车辆) - if abs(azimuth) < 60.0: - min_distance = distance - target_azimuth = azimuth - has_vehicle = True - - if has_vehicle: - return min_distance, target_azimuth, True - else: - return None, None, False - -def avoid_vehicle(vehicle, distance, azimuth): - """根据检测到的车辆方位,执行避让操作(向宽敞方向微调)""" - global vehicle_control - # 获取车辆当前的控制状态(保留自动驾驶的油门/刹车,只改转向) - vehicle_control = vehicle.get_control() - - # 计算避让方向:根据方位角调整转向(-1~1之间,1为右,-1为左) - steer_strength = (AVOIDANCE_ANGLE / 60.0) * 0.2 # 控制转向强度,避免过度打方向 - if azimuth > 0: # 车辆在右侧(相对于当前车辆前方) - vehicle_control.steer = -steer_strength # 向左微调 - elif azimuth < 0: # 车辆在左侧 - vehicle_control.steer = steer_strength # 向右微调 - else: # 正前方 - vehicle_control.steer = steer_strength # 默认向右微调(宽敞方向) - - # 距离越近,转向强度稍大,同时轻微降速 - if distance < SAFE_DISTANCE / 2: - vehicle_control.steer *= 1.5 - vehicle_control.throttle = max(0.4, vehicle_control.throttle) - else: - vehicle_control.throttle = max(0.5, vehicle_control.throttle) - vehicle_control.brake = 0.0 - - # 应用控制指令(覆盖自动驾驶的转向,保留油门/刹车) - vehicle.apply_control(vehicle_control) - print(f"\n⚠️ 检测到前方车辆:距离{distance:.1f}m,方位{azimuth:.1f}度,正在向{'右' if azimuth <=0 else '左'}避让...") +# 计算两个向量的夹角(用于转向控制,平滑转向) +# 适配0.9.14:移除Vector2D.is_zero(),改用长度判断 +def calculate_angle(current_transform, target_location): + # 获取车辆的前进方向向量(仅平面,忽略z轴) + forward = current_transform.get_forward_vector() + forward_flat = carla.Vector2D(forward.x, forward.y) + # 计算车辆到目标点的方向向量 + target_flat = carla.Vector2D( + target_location.x - current_transform.location.x, + target_location.y - current_transform.location.y + ) + # 归一化向量(避免长度影响夹角计算) + if forward_flat.length() > 0: + forward_flat = forward_flat / forward_flat.length() + if target_flat.length() > 0: + target_flat = target_flat / target_flat.length() + # 计算夹角(弧度),范围[-π, π] + dot = forward_flat.x * target_flat.x + forward_flat.y * target_flat.y + cross = forward_flat.x * target_flat.y - forward_flat.y * target_flat.x + angle = math.atan2(cross, dot) + return angle + +# 选择路口的路径点(适配0.9.14:不使用road_type,优先选直走/主方向) +def choose_main_waypoint(waypoint): + # 获取下一组路径点(间距5米,更远的距离能更好识别路口) + next_waypoints = waypoint.next(5.0) + if not next_waypoints: + return waypoint + # 优先选择第一个路径点(主方向,避免拐入小巷) + main_waypoint = next_waypoints[0] + return main_waypoint def main(): - """主函数:连接CARLA、随机加载地图、生成车辆、显示画面、检测并避让车辆""" - # 初始化资源 - client = None - world = None - vehicle = None camera = None + vehicle = None + current_waypoint = None # 动态更新的当前目标路径点 + + # 检查CARLA进程是否运行 + def check_carla_running(): + try: + import psutil + for proc in psutil.process_iter(['name']): + if proc.info['name'] == 'CarlaUE4.exe': + return True + return False + except ImportError: + print("⚠️ 未安装psutil,默认认为CARLA进程已运行") + return True # 前置检查 print("=" * 60) print("--- [低画质CARLA环境检查] ---") if not check_carla_running(): - print("❌ 错误:未检测到CarlaUE4.exe进程!") + print("❌ 错误:未检测到CARLA服务器运行!") print(f" 请先启动:{os.path.join(CARLA_ROOT, 'CarlaUE4.exe')}") print(" (建议使用低画质快捷方式启动)") return @@ -266,133 +167,281 @@ def main(): try: # 1. 连接CARLA服务器(低画质版超时延长) - client = carla.Client(CARLA_HOST, CARLA_PORT) - client.set_timeout(CARLA_TIMEOUT) - print("✅ 连接CARLA服务器成功") + client = carla.Client('127.0.0.1', 2000) + client.set_timeout(60.0) - # 2. 随机加载低画质地图 - world = load_random_map(client) + # 随机选择地图 + available_maps = ['Town01', 'Town02', 'Town03', 'Town04', 'Town05'] + random_map = random.choice(available_maps) + world = client.load_world(random_map) + carla_map = world.get_map() + world.wait_for_tick() + print(f"✅ 随机加载地图成功!当前地图:{carla_map.name}(随机选择:{random_map})") - # 3. 获取蓝图和生成点 + # 2. 获取蓝图和生成点 blueprint_library = world.get_blueprint_library() - spawn_points = world.get_map().get_spawn_points() + spawn_points = carla_map.get_spawn_points() if not spawn_points: print("❌ 无可用生成点,退出") return - # 4. 生成主车辆(带基础避让功能) - vehicle = spawn_vehicle(world, blueprint_library, spawn_points) - if vehicle is None: - return - - # 5. 生成其他车辆(增加环境中的车辆,用于测试避让) - other_vehicle_count = 5 # 生成5辆其他车辆 - spawned_count = 0 + # 3. 生成车辆(低画质选轻量化车型) + vehicle_bps = blueprint_library.filter('vehicle.seat.leon') + if not vehicle_bps: + vehicle_bps = blueprint_library.filter('vehicle.*')[0:1] + vehicle_bp = vehicle_bps[0] + vehicle_bp.set_attribute('role_name', 'autopilot') + + # 选择生成点 + spawn_idx = random.choice([5, 12, 15, 20]) if len(spawn_points) > 20 else random.randint(0, len(spawn_points)-1) + spawn_point = spawn_points[spawn_idx] + vehicle = world.spawn_actor(vehicle_bp, spawn_point) + vehicle.set_autopilot(False) + print(f"✅ 生成车辆:{vehicle.type_id} | 生成点:{spawn_point.location}") + + # 4. 生成NPC车辆(可选,用于测试边界框可视化) + npc_count = 10 # 生成10辆NPC车辆 + spawned_npcs = 0 for spawn_point in spawn_points: - if spawned_count >= other_vehicle_count: + if spawned_npcs >= npc_count: break - # 随机选择其他车型,跳过主车辆的车型(可选) - other_vehicle_bps = [bp for bp in blueprint_library.filter('vehicle.*') if bp.id != PREFERRED_VEHICLE] - if not other_vehicle_bps: - other_vehicle_bps = blueprint_library.filter('vehicle.*') - other_vehicle_bp = random.choice(other_vehicle_bps) - try: - other_vehicle = world.spawn_actor(other_vehicle_bp, spawn_point) - other_vehicle.set_autopilot(True) - spawned_count += 1 - except Exception as e: + npc_bps = [bp for bp in blueprint_library.filter('vehicle.*') if int(bp.get_attribute('number_of_wheels')) == 4] + if not npc_bps: continue - print(f"✅ 生成了{spawned_count}辆其他车辆,用于测试避让功能") - - # 6. 挂载摄像头传感器(移除雷达,避免属性错误) - camera = spawn_camera(world, blueprint_library, vehicle) - if camera is None: - return + npc_bp = random.choice(npc_bps) + npc = world.try_spawn_actor(npc_bp, spawn_point) + if npc: + npc.set_autopilot(True) + spawned_npcs += 1 + print(f"✅ 生成{spawned_npcs}辆NPC车辆") + + # 5. 初始化第一个路径点 + current_waypoint = carla_map.get_waypoint(vehicle.get_location(), project_to_road=True) + current_waypoint = choose_main_waypoint(current_waypoint) + print(f"✅ 初始化路径点完成") + + # 6. 挂载摄像头(低画质参数) + camera_bp = blueprint_library.find('sensor.camera.rgb') + camera_bp.set_attribute('image_size_x', '640') + camera_bp.set_attribute('image_size_y', '360') + camera_bp.set_attribute('fov', '80') + camera_bp.set_attribute('sensor_tick', '0.1') + camera_transform = carla.Transform(carla.Location(x=1.5, z=1.8)) + camera = world.spawn_actor(camera_bp, camera_transform, attach_to=vehicle) + camera.listen(image_callback) + print("✅ 摄像头挂载成功") - # 7. 显示画面+检测车辆+避让逻辑 - print("\n📌 操作说明:") - print(f" - 按 'q' 退出程序") - print(f" - 车辆已启用自动驾驶+主动识别车辆避让功能") - cv2.namedWindow(WINDOW_NAME, cv2.WINDOW_NORMAL) - cv2.resizeWindow(WINDOW_NAME, *WINDOW_SIZE) + # 7. 初始化3D转2D投影参数(适配0.9.14) + image_w = 640 + image_h = 360 + fov = camera_bp.get_attribute('fov').as_float() + K = build_projection_matrix(image_w, image_h, fov) + K_b = build_projection_matrix(image_w, image_h, fov, is_behind_camera=True) + # 车辆包围盒边缘(简化版,不影响边界框生成) + edges = [[0,1],[1,3],[3,2],[2,0],[0,4],[4,5],[5,1],[5,7],[7,6],[6,4],[6,2],[7,3]] + # COCO类别名称(仅保留车辆) + COCO_CLASS_NAMES = ['car'] + + # 8. 核心逻辑:红灯停+绿灯行+沿道路行驶+3D可视化 + print("\n📌 按 'q' 退出 | 红灯停绿灯行+沿道路行驶+3D车辆边界框可视化") + cv2.namedWindow('CARLA Low-Quality View (3D Bounding Box)', cv2.WINDOW_NORMAL) + cv2.resizeWindow('CARLA Low-Quality View (3D Bounding Box)', 640, 360) + + # 初始化控制参数 + vehicle_control = carla.VehicleControl() + TARGET_SPEED = 30 # 目标车速(km/h) + TARGET_SPEED_MS = TARGET_SPEED / 3.6 + STANDSTILL_THRESHOLD = 0.1 + last_traffic_light_state = None - frame_count = 0 while True: - # 显示摄像头画面 - if not IMAGE_QUEUE.empty(): - img = IMAGE_QUEUE.get(timeout=0.5) - cv2.imshow(WINDOW_NAME, img) - - # 每隔一定帧数检测车辆(降低性能消耗,适配低画质版) - frame_count += 1 - if frame_count % 5 == 0: # 每5帧检测一次 - distance, azimuth, has_vehicle = detect_nearby_vehicles(world, vehicle) - if has_vehicle: - avoid_vehicle(vehicle, distance, azimuth) + # ========== 步骤1:获取车辆当前状态 ========== + current_location = vehicle.get_location() + current_transform = vehicle.get_transform() + current_speed = vehicle.get_velocity().length() + is_standstill = current_speed < STANDSTILL_THRESHOLD + need_brake = False + traffic_light_info = "无交通灯" + + # ========== 步骤2:红绿灯识别(原逻辑保留) ========== + traffic_light = vehicle.get_traffic_light() + if traffic_light is not None: + tl_state = traffic_light.state + traffic_light_info = f"交通灯状态:{tl_state}" + if tl_state in [carla.TrafficLightState.Red, carla.TrafficLightState.Yellow]: + need_brake = True + elif tl_state == carla.TrafficLightState.Green: + need_brake = False + + if vehicle.is_at_traffic_light(): + tl_state_alt = vehicle.get_traffic_light_state() + if tl_state_alt in [carla.TrafficLightState.Red, carla.TrafficLightState.Yellow]: + need_brake = True + traffic_light_info = f"备用检测:{tl_state_alt}" + + # ========== 步骤3:车辆控制逻辑(原逻辑保留) ========== + if need_brake: + if traffic_light_info != last_traffic_light_state: + print(f"🚦 {traffic_light_info} | 速度:{current_speed:.2f}m/s → 刹车停车") + last_traffic_light_state = traffic_light_info + + if not is_standstill: + vehicle_control.brake = 1.0 + vehicle_control.throttle = 0.0 + vehicle_control.steer = 0.0 + vehicle_control.hand_brake = False + vehicle_control.gear = 1 else: - # 无车辆时,恢复默认转向(直行) - if vehicle_control.steer != 0.0: - vehicle_control.steer = 0.0 - vehicle.apply_control(vehicle_control) - frame_count = 0 + vehicle_control.brake = 1.0 + vehicle_control.throttle = 0.0 + vehicle_control.steer = 0.0 + vehicle_control.hand_brake = True + vehicle_control.gear = 0 + else: + if last_traffic_light_state is not None: + print(f"🚦 {traffic_light_info} | 速度:{current_speed:.2f}m/s → 沿道路行驶") + last_traffic_light_state = None + + # 动态更新路径点 + distance_to_waypoint = math.hypot( + current_location.x - current_waypoint.transform.location.x, + current_location.y - current_waypoint.transform.location.y + ) + + if distance_to_waypoint < 2.0: + current_waypoint = choose_main_waypoint(current_waypoint) + TARGET_SPEED_MS = (TARGET_SPEED - 5) / 3.6 if current_waypoint.is_junction else TARGET_SPEED / 3.6 + + # 转向控制 + angle = calculate_angle(current_transform, current_waypoint.transform.location) + vehicle_control.steer = np.clip(angle * 0.8, -1.0, 1.0) + + # 速度控制 + vehicle_control.hand_brake = False + vehicle_control.brake = 0.0 + if current_speed < TARGET_SPEED_MS: + vehicle_control.throttle = min(0.7, (TARGET_SPEED_MS - current_speed) / 3 + 0.2) + else: + vehicle_control.throttle = 0.1 if current_speed < TARGET_SPEED_MS + 1 else 0.0 + vehicle_control.brake = 0.1 if current_speed > TARGET_SPEED_MS + 1 else 0.0 + vehicle_control.gear = 2 + + # 应用控制指令 + vehicle.apply_control(vehicle_control) - # 实时输出车辆速度(间隔打印,避免刷屏) - if frame_count % SPEED_PRINT_INTERVAL == 0 and vehicle is not None: - current_speed = get_vehicle_speed(vehicle) - print(f"\r当前车辆速度:{current_speed} km/h", end="") + # ========== 步骤4:3D车辆转2D边界框可视化(新增核心逻辑) ========== + # 获取摄像头图像 + current_img = None + if not IMAGE_QUEUE.empty(): + try: + current_img = IMAGE_QUEUE.get(timeout=0.5) + except queue.Empty: + pass + # 容错:使用最新图像或黑色占位图 + if current_img is None and LATEST_IMAGE is not None: + current_img = LATEST_IMAGE.copy() + if current_img is None: + current_img = np.zeros((image_h, image_w, 3), dtype=np.uint8) + + # 获取世界→相机的变换矩阵 + world_2_camera = np.array(camera.get_transform().get_inverse_matrix()) + + # 遍历所有车辆,计算2D边界框 + boxes = [] + ids = [] + for npc in world.get_actors().filter('*vehicle*'): + # 跳过自车 + if npc.id == vehicle.id: + continue + + # 筛选50米内、自车前方的车辆 + dist = npc.get_transform().location.distance(vehicle.get_transform().location) + forward_vec = vehicle.get_transform().get_forward_vector() + ray = npc.get_transform().location - vehicle.get_transform().location + if dist < 50 and forward_vec.dot(ray) > 0: + # 获取车辆包围盒的3D顶点(适配0.9.14) + bb_verts = [v for v in npc.bounding_box.get_world_vertices(npc.get_transform())] + points_2d = [] + # 将每个3D顶点投影到2D图像 + for vert in bb_verts: + ray_cam = vert - camera.get_transform().location + cam_forward = camera.get_transform().get_forward_vector() + if cam_forward.dot(ray_cam) > 0: + p = get_image_point(vert, K, world_2_camera) + else: + p = get_image_point(vert, K_b, world_2_camera) + points_2d.append(p) + # 生成2D边界框 + if points_2d: + x1, y1, x2, y2 = get_2d_box_from_3d_edges(points_2d, image_h, image_w) + # 过滤过小的边界框 + if (y2 - y1) * (x2 - x1) > 100 and (x2 - x1) > 20: + if point_in_canvas((x1, y1), image_h, image_w) and point_in_canvas((x2, y2), image_h, image_w): + boxes.append(np.array([x1, y1, x2, y2])) + ids.append(npc.id) + + # 绘制边界框 + if boxes: + boxes = np.array(boxes) + output_image = draw_bounding_boxes(current_img, boxes, ids, COCO_CLASS_NAMES) + else: + output_image = current_img + + # ========== 步骤5:显示图像 ========== + cv2.imshow('CARLA Low-Quality View (3D Bounding Box)', output_image) # 按q退出 if cv2.waitKey(1) & 0xFF == ord('q'): - print("\n📌 收到退出指令,正在清理资源...") break - # 异常处理(完全适配低画质版CARLA 0.9.14,无专用异常类) - except RuntimeError as e: - error_msg = str(e).lower() - if "connection" in error_msg or "timeout" in error_msg: + # 异常处理 + except Exception as e: + if "Connection" in str(e): print("\n❌ 连接失败!") - print(f" 解决:1. 确认CarlaUE4.exe已启动 2. 关闭防火墙 3. 检查端口{CARLA_PORT}") - elif "spawn" in error_msg: - print("\n❌ 车辆生成失败(无可用生成点)!") - print(" 解决:重启CARLA或更换生成点") + print(" 解决:1. 确认CarlaUE4.exe已启动 2. 关闭防火墙 3. 检查端口2000") + elif "Spawn" in str(e): + print("\n❌ 车辆生成失败!") + print(" 解决:换生成点或重启CARLA") + elif "AttributeError" in str(e): + print(f"\n❌ API属性错误:{e}") + print(" 解决:确认CARLA版本为0.9.14,重新安装对应版本的whl包") else: - print(f"\n❌ 运行时错误:{e}") - except AttributeError as e: - print(f"\n❌ API属性错误:{e}") - print(" 解决:1. 确认使用的是CARLA 0.9.14低画质版whl包 2. 重启CARLA和脚本") - except Exception as e: - print(f"\n❌ 未知错误:{e}") - traceback.print_exc() + print(f"\n❌ 未知错误:{e}") + import traceback + traceback.print_exc() - # 清理资源(确保低画质版资源正常释放) + # 清理资源 finally: print("\n--- [清理资源] ---") + # 销毁所有车辆(包括NPC) + if world: + for actor in world.get_actors().filter('*vehicle*'): + try: + actor.destroy() + except: + pass if camera: try: camera.stop() camera.destroy() print("✅ 销毁摄像头") - except Exception as e: - print(f"⚠️ 销毁摄像头失败:{e}") + except: + print("⚠️ 摄像头销毁失败") if vehicle: try: vehicle.destroy() print("✅ 销毁车辆") - except Exception as e: - print(f"⚠️ 销毁车辆失败:{e}") - # 销毁其他生成的车辆 - try: - if world: - for actor in world.get_actors().filter('vehicle.*'): - try: - actor.destroy() - except Exception: - pass - print("✅ 销毁所有其他车辆") - except Exception as e: - print(f"⚠️ 销毁其他车辆失败:{e}") + except: + print("⚠️ 车辆销毁失败") cv2.destroyAllWindows() print("✅ 程序结束") if __name__ == '__main__': + # 导入psutil + try: + import psutil + except ImportError: + print("⚠️ 未安装psutil,跳过CARLA进程检查") + def check_carla_running(): + return True main() \ No newline at end of file From c59107eced925aae6c33c0a0d1b38591fad621f2 Mon Sep 17 00:00:00 2001 From: Yuansugu123 <2451009878@qq.com> Date: Sat, 20 Dec 2025 10:22:11 +0800 Subject: [PATCH 5/7] feat python --- src/car_tracking/car1.py | 265 +++++++++++++++++++++++++++++++++++++++ 1 file changed, 265 insertions(+) create mode 100644 src/car_tracking/car1.py diff --git a/src/car_tracking/car1.py b/src/car_tracking/car1.py new file mode 100644 index 0000000000..6298523f1c --- /dev/null +++ b/src/car_tracking/car1.py @@ -0,0 +1,265 @@ +import carla +import queue +import random +import cv2 +import numpy as np + +# 注意:移除了所有deep_sort相关导入 +# 保留必要的工具函数导入(如果utils模块不存在,我会替换为内置实现) +from utils.projection import build_projection_matrix, get_image_point, get_2d_box_from_3d_edges, point_in_canvas +from utils.world import clear_npc, clear_static_vehicle + +# --------------------------- +# 全局配置(可根据需要调整) +# --------------------------- +MAP_NAME = "Town07" # 指定要加载的地图,可替换为Town01-Town12 +NPC_VEHICLE_NUM = 50 # NPC车辆数量 +CAMERA_RESOLUTION = (640, 640) # 相机分辨率 +SYNC_DELTA_SECONDS = 0.05 # 同步模式的固定时间步长 +VEHICLE_DISTANCE_THRESHOLD = 50 # 检测车辆的距离阈值(米) +SPEED_DISPLAY_UNIT = "km/h" # 速度单位:m/s 或 km/h + +# --------------------------- +# 替代draw_bounding_boxes的内置函数(如果utils.box_utils不存在) +# 如果你有utils.box_utils,可注释这部分,保留原导入 +# --------------------------- +def draw_bounding_boxes(image, bboxes, labels, class_names, ids): + """ + 绘制包围盒和ID标签 + :param image: 输入图像(BGR格式) + :param bboxes: 包围盒列表,格式为[[x1,y1,x2,y2], ...] + :param labels: 类别标签列表 + :param class_names: 类别名称列表 + :param ids: 跟踪ID列表 + :return: 绘制后的图像 + """ + for i, (bbox, id) in enumerate(zip(bboxes, ids)): + x1, y1, x2, y2 = bbox.astype(int) + # 绘制矩形框 + cv2.rectangle(image, (x1, y1), (x2, y2), (0, 255, 0), 2) + # 绘制标签(类别 + ID) + label = f"{class_names[labels[i]]} | ID: {id}" + cv2.putText(image, label, (x1, y1 - 10), cv2.FONT_HERSHEY_SIMPLEX, 0.5, (0, 255, 0), 2) + return image + +def camera_callback(image, rgb_image_queue): + """相机回调函数,将图像数据存入队列""" + rgb_image_queue.put(np.reshape(np.copy(image.raw_data), + (image.height, image.width, 4))) + +def calculate_vehicle_speed(velocity, unit="km/h"): + """计算车辆速度,转换为指定单位""" + speed_m_s = np.linalg.norm([velocity.x, velocity.y, velocity.z]) + if unit == "km/h": + return speed_m_s * 3.6 # 1 m/s = 3.6 km/h + return speed_m_s + +def main(): + # --------------------------- + # 1. 初始化Carla客户端,加快地图加载 + # --------------------------- + try: + client = carla.Client('localhost', 2000) + client.set_timeout(30.0) # 增加超时时间,避免地图加载时超时 + + # 方案3:查看所有可用地图 + print("Carla支持的地图列表:") + available_maps = client.get_available_maps() + print(available_maps) + + # 方案2:使用当前已加载的地图,避免map not found + world = client.get_world() + # 如果想指定地图,可从available_maps中选一个,比如: + # world = client.load_world("Town01") + + spawn_points = world.get_map().get_spawn_points() + if not spawn_points: + print("警告:未获取到地图生成点,程序将退出") + return + # 后续代码不变... + + # --------------------------- + # 2. 设置同步模式,优化仿真效率 + # --------------------------- + settings = world.get_settings() + settings.synchronous_mode = True + settings.fixed_delta_seconds = SYNC_DELTA_SECONDS + world.apply_settings(settings) + + # --------------------------- + # 3. 生成Ego车辆 + # --------------------------- + bp_lib = world.get_blueprint_library() + vehicle_bp = bp_lib.find('vehicle.lincoln.mkz_2020') + ego_vehicle = world.try_spawn_actor(vehicle_bp, random.choice(spawn_points)) + if not ego_vehicle: + print("警告:未能生成Ego车辆,程序将退出") + return + + # --------------------------- + # 4. 生成相机传感器 + # --------------------------- + camera_bp = bp_lib.find('sensor.camera.rgb') + camera_bp.set_attribute('image_size_x', str(CAMERA_RESOLUTION[0])) + camera_bp.set_attribute('image_size_y', str(CAMERA_RESOLUTION[1])) + camera_init_trans = carla.Transform(carla.Location(x=1, z=2)) + camera = world.spawn_actor(camera_bp, camera_init_trans, attach_to=ego_vehicle) + + # 图像队列(设置最大长度,避免内存溢出) + image_queue = queue.Queue(maxsize=1) + camera.listen(lambda image: camera_callback(image, image_queue)) + + # --------------------------- + # 5. 清理现有NPC和静态车辆 + # --------------------------- + clear_npc(world) + clear_static_vehicle(world) + + # --------------------------- + # 6. 预计算相机相关参数 + # --------------------------- + edges = [[0, 1], [1, 3], [3, 2], [2, 0], [0, 4], [4, 5], + [5, 1], [5, 7], [7, 6], [6, 4], [6, 2], [7, 3]] + image_w, image_h = CAMERA_RESOLUTION + fov = camera_bp.get_attribute("fov").as_float() + K = build_projection_matrix(image_w, image_h, fov) + K_b = build_projection_matrix(image_w, image_h, fov, is_behind_camera=True) + + # --------------------------- + # 7. 生成NPC车辆 + # --------------------------- + car_bp = [bp for bp in bp_lib.filter('vehicle') if int(bp.get_attribute('number_of_wheels')) == 4] + if car_bp: + for i in range(NPC_VEHICLE_NUM): + npc = world.try_spawn_actor(random.choice(car_bp), random.choice(spawn_points)) + if npc: + npc.set_autopilot(True) + + ego_vehicle.set_autopilot(True) + + # --------------------------- + # 8. 主循环(核心:用Actor ID实现跟踪) + # --------------------------- + while True: + world.tick() + + # 移动spectator到车辆上方 + spectator = world.get_spectator() + transform = carla.Transform(ego_vehicle.get_transform().transform( + carla.Location(x=-4, z=50)), carla.Rotation(yaw=-180, pitch=-90)) + spectator.set_transform(transform) + + # 获取相机图像 + if not image_queue.empty(): + image = image_queue.get() + else: + continue + + # 图像预处理(RGBA转BGR,减少一次颜色转换) + image = cv2.cvtColor(image, cv2.COLOR_RGBA2BGR) + + # 更新相机的世界到相机矩阵 + world_2_camera = np.array(camera.get_transform().get_inverse_matrix()) + + # 存储:包围盒、车辆ID、速度 + bboxes = [] + vehicle_ids = [] + vehicle_speeds = [] + + # 遍历所有车辆Actor + vehicles = world.get_actors().filter('*vehicle*') + for npc in vehicles: + if npc.id == ego_vehicle.id: + # 输出Ego车辆速度 + ego_speed = calculate_vehicle_speed(npc.get_velocity(), SPEED_DISPLAY_UNIT) + print(f"Ego车辆速度:{ego_speed:.1f} {SPEED_DISPLAY_UNIT}") + continue + + # 过滤距离过远的车辆 + dist = npc.get_transform().location.distance(ego_vehicle.get_transform().location) + if dist > VEHICLE_DISTANCE_THRESHOLD: + continue + + # 只检测车辆前方的目标 + forward_vec = ego_vehicle.get_transform().get_forward_vector() + ray = npc.get_transform().location - ego_vehicle.get_transform().location + if forward_vec.dot(ray) <= 0: + continue + + # 计算NPC车辆速度 + npc_speed = calculate_vehicle_speed(npc.get_velocity(), SPEED_DISPLAY_UNIT) + + # 计算3D包围盒的2D投影 + bb = npc.bounding_box + verts = [v for v in bb.get_world_vertices(npc.get_transform())] + points_2d = [] + + for vert in verts: + ray0 = vert - camera.get_transform().location + cam_forward_vec = camera.get_transform().get_forward_vector() + if cam_forward_vec.dot(ray0) > 0: + p = get_image_point(vert, K, world_2_camera) + else: + p = get_image_point(vert, K_b, world_2_camera) + points_2d.append(p) + + # 获取2D框 + x_min, x_max, y_min, y_max = get_2d_box_from_3d_edges(points_2d, edges, image_h, image_w) + + # 过滤过小的框和超出画布的框 + box_area = (y_max - y_min) * (x_max - x_min) + box_width = x_max - x_min + if box_area > 100 and box_width > 20 and point_in_canvas((x_min, y_min), image_h, image_w) and point_in_canvas((x_max, y_max), image_h, image_w): + bboxes.append(np.array([x_min, y_min, x_max, y_max])) + vehicle_ids.append(npc.id) # 用Actor ID作为跟踪ID + vehicle_speeds.append(npc_speed) + + # --------------------------- + # 绘制包围盒、ID和速度(核心替代deep_sort的部分) + # --------------------------- + if len(bboxes) > 0: + # 绘制基础包围盒和ID(类别固定为car,对应COCO的2号类别) + labels = np.array([2] * len(bboxes)) + COCO_CLASS_NAMES = {2: 'car'} # 简化的类别映射,替代原导入 + image = draw_bounding_boxes(image, bboxes, labels, COCO_CLASS_NAMES, vehicle_ids) + + # 绘制速度标签 + for i, (bbox, speed) in enumerate(zip(bboxes, vehicle_speeds)): + x1, y1, x2, y2 = bbox.astype(int) + speed_label = f"Speed: {speed:.1f} {SPEED_DISPLAY_UNIT}" + # 在框下方绘制速度 + cv2.putText(image, speed_label, (x1, y2 + 20), cv2.FONT_HERSHEY_SIMPLEX, 0.5, (255, 0, 0), 2) + + # 显示图像 + cv2.imshow('2D Tracking with Actor ID (no deep_sort)', image) + + # 按q退出 + if cv2.waitKey(1) & 0xFF == ord('q'): + break + + except KeyboardInterrupt: + print("\n程序被用户中断") + except Exception as e: + print(f"程序出错:{e}") + finally: + # --------------------------- + # 资源清理 + # --------------------------- + print("正在清理资源...") + if 'world' in locals(): + settings = world.get_settings() + settings.synchronous_mode = False + world.apply_settings(settings) + if 'camera' in locals(): + camera.destroy() + if 'ego_vehicle' in locals(): + ego_vehicle.destroy() + # 清理所有NPC车辆 + if 'world' in locals(): + for vehicle in world.get_actors().filter('*vehicle*'): + vehicle.destroy() + cv2.destroyAllWindows() + print("资源清理完成") + +if __name__ == "__main__": + main() \ No newline at end of file From ec12c8695827836101ea4ab78cf91fdc00378269 Mon Sep 17 00:00:00 2001 From: Yuansugu123 <2451009878@qq.com> Date: Sat, 20 Dec 2025 18:32:39 +0800 Subject: [PATCH 6/7] =?UTF-8?q?=E6=96=B0=E5=A2=9EREADME.md=E5=86=85?= =?UTF-8?q?=E5=AE=B9?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- src/car_tracking/README.md | 47 ++++++++++++++++++++++++++++---------- 1 file changed, 35 insertions(+), 12 deletions(-) diff --git a/src/car_tracking/README.md b/src/car_tracking/README.md index 27a3595748..38036674c4 100644 --- a/src/car_tracking/README.md +++ b/src/car_tracking/README.md @@ -1,5 +1,6 @@ - Carla 工具集 -本目录包含两个独立的Carla脚本,可直接运行,互不依赖。 +# Carla 工具集 运行步骤汇总 + +本汇总包含三个独立Carla脚本的完整运行步骤,所有运行命令均封装在可一键复制的bash代码框内。 ## 程序1:carla_2d_ground_truth.py ### 功能 @@ -10,33 +11,55 @@ - Python 版本:3.7 - 依赖包:`opencv-python` `numpy` `carla`(版本需与Carla模拟器匹配) -### 运行命令 +### 操作说明 +- 可视化窗口显示相机画面与车辆2D边界框 +- 按 **q** 键退出程序 + +### 完整运行步骤 ```bash # 第一步:启动对应版本的Carla模拟器 # 第二步:执行脚本 python car_2d_ground_truth.py ``` -### 操作说明 -- 可视化窗口显示相机画面与车辆2D边界框 -- 按 **q** 键退出程序 - -## 程序2:carla_low_quality_viewer.py +## 程序2:carla_viewer.py ### 功能 Carla 0.9.14低画质版专用脚本,实现车辆自动行驶+提前避让(车辆/障碍物/红绿灯),修复低配置设备进程卡顿问题,降低资源占用。 ### 环境要求 -- Carla 版本:**仅限0.9.14低画质版** +- Carla 版本:仅限0.9.14低画质版 - Python 版本:3.7 - 依赖包:`opencv-python` `numpy` `carla==0.9.14`(可选`psutil`用于进程检测) -### 运行命令 +### 操作说明 +- 可视化窗口显示摄像头画面,终端实时输出车速与避让提示 +- 按 **q** 键退出程序 + +### 完整运行步骤 ```bash # 第一步:启动Carla 0.9.14低画质版模拟器 # 第二步:执行脚本 python main.py ``` +## 程序3:car1.py +### 功能 +基于Carla Actor ID实现轻量化车辆检测、跟踪与速度可视化,利用3D包围盒投影生成2D边界框,替代Deep SORT依赖,优化地图加载与仿真同步效率。 + +### 环境要求 +- Carla 版本:0.9.10~0.9.15(兼容主流版本,避免非内置地图名) +- Python 版本:3.7 +- 依赖包:`opencv-python` `numpy` `carla`(版本需与Carla模拟器匹配) + ### 操作说明 -- 可视化窗口显示摄像头画面,终端实时输出车速与避让提示 -- 按 **q** 键退出程序 \ No newline at end of file +- 可视化窗口显示相机画面、车辆2D边界框、Actor ID及实时速度 +- 终端输出主车辆行驶速度 +- 按 **q** 键退出程序 +- 程序启动时自动清理现有NPC,生成50辆自动驾驶NPC模拟交通场景 + +### 完整运行步骤 +```bash +# 第一步:启动对应版本的Carla模拟器(建议确认可用地图:如Town01/Town06) +# 第二步:执行脚本 +python car1.py +``` \ No newline at end of file From fafbd7e31e4040a05b5b222c945aba14e87ff04f Mon Sep 17 00:00:00 2001 From: Yuansugu123 <2451009878@qq.com> Date: Sat, 20 Dec 2025 20:06:13 +0800 Subject: [PATCH 7/7] =?UTF-8?q?=E5=9F=BA=E4=BA=8Eyolo=E7=9A=84=E8=BD=A6?= =?UTF-8?q?=E8=BE=86=E6=A3=80=E6=B5=8B?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- src/car_tracking/caryolo.py | 391 ++++++++++++++++++++++++++++++++++++ 1 file changed, 391 insertions(+) create mode 100644 src/car_tracking/caryolo.py diff --git a/src/car_tracking/caryolo.py b/src/car_tracking/caryolo.py new file mode 100644 index 0000000000..206173e502 --- /dev/null +++ b/src/car_tracking/caryolo.py @@ -0,0 +1,391 @@ +import os +import sys +import traceback # 用于打印错误堆栈 +# ====================== 关键:添加src/2d-carla-tracking-master到Python搜索路径 ====================== +# 替换为你本地的src/2d-carla-tracking-master绝对路径(例如:D:\nn\src\2d-carla-tracking-master) +CARLA_TRACKING_ROOT = r"D:\nn\src\2d-carla-tracking-master" # 必须修改为你的实际路径 +sys.path.append(CARLA_TRACKING_ROOT) + +import queue +import random +import time +import numpy as np +import cv2 +import carla # 确保carla库已安装且版本与模拟器匹配 + +# ====================== 直接导入utils模块(保留原名,去掉多余路径) ====================== +# 1. 导入oc_sort(在src/2d-carla-tracking-master下) +from oc_sort.ocsort import OCSort +# 2. 导入utils模块中的函数(模块名就是utils,完全保留原名,确保utils在2d-carla-tracking-master下) +from utils.box_utils import draw_bounding_boxes +from utils.projection import build_projection_matrix +from utils.world import clear_npc, clear_static_vehicle, clear + +# ====================== 全局配置项(可根据需求修改) ====================== +# Carla连接配置 +CARLA_HOST = 'localhost' +CARLA_PORT = 2000 +CARLA_TIMEOUT = 10.0 # 单次连接超时时间(秒) +CARLA_RETRY_MAX = 3 # Carla连接最大重试次数 +CARLA_RETRY_DELAY = 2 # 重试间隔(秒) +SYNC_DELTA_SECONDS = 0.05 # 同步模式的固定时间步长 + +# 摄像头配置 +CAMERA_WIDTH = 640 +CAMERA_HEIGHT = 640 +CAMERA_FOV = 90 # 摄像头视场角 +CAMERA_POSITION = carla.Transform(carla.Location(x=1, z=2)) # 摄像头挂载位置 + +# 目标检测配置 +YOLO_MODEL_INDEX = 0 # 0: YOLOv4, 1: YOLOv4 Tiny +DETECT_CLASSES = [2, 5, 7] # COCO类别:2-汽车,5-公交车,7-卡车 +DETECT_CLASS_ID = 2 # 统一标注为汽车类别用于绘制 + +# OC-SORT跟踪器配置 +OCSORT_DET_THRESH = 0.6 # 检测置信度阈值 +OCSORT_IOU_THRESH = 0.3 # IOU匹配阈值 +OCSORT_USE_BYTE = False # 是否使用ByteTrack + +# NPC车辆配置 +NPC_VEHICLE_NUM = 50 # 生成的NPC车辆数量 + +# ====================== 抑制TensorFlow冗余警告(可选) ====================== +import tensorflow as tf +tf.get_logger().setLevel('ERROR') # 只显示ERROR级别日志,屏蔽WARNING +os.environ['TF_CPP_MIN_LOG_LEVEL'] = '2' # 屏蔽TensorFlow的GPU库警告 + +# ====================== 目标检测模型加载 ====================== +def load_yolov4_model(): + """加载YOLOv4目标检测模型(包含异常处理)""" + try: + from what.models.detection.datasets.coco import COCO_CLASS_NAMES + from what.models.detection.yolo.yolov4 import YOLOV4 + from what.cli.model import ( + what_model_list, WHAT_MODEL_FILE_INDEX, WHAT_MODEL_URL_INDEX, + WHAT_MODEL_HASH_INDEX, WHAT_MODEL_PATH + ) + from what.utils.file import get_file + + # 获取模型列表(YOLOv4和YOLOv4 Tiny) + what_yolov4_model_list = what_model_list[4:6] + model_info = what_yolov4_model_list[YOLO_MODEL_INDEX] + + # 下载模型(如果不存在) + model_file = model_info[WHAT_MODEL_FILE_INDEX] + model_url = model_info[WHAT_MODEL_URL_INDEX] + model_hash = model_info[WHAT_MODEL_HASH_INDEX] + model_path = os.path.join(WHAT_MODEL_PATH, model_file) + + if not os.path.isfile(model_path): + print(f"正在下载YOLOv4模型:{model_file}") + get_file(model_file, WHAT_MODEL_PATH, model_url, model_hash) + print("模型下载完成") + + # 初始化YOLOv4模型 + model = YOLOV4(COCO_CLASS_NAMES, model_path) + return model, COCO_CLASS_NAMES + except ImportError as e: + raise ImportError(f"缺少what库或相关依赖:{e}") + except Exception as e: + raise RuntimeError(f"模型加载失败:{e}") + +# ====================== Carla环境初始化(含重试逻辑) ====================== +def init_carla_environment(): + """初始化Carla环境:连接服务器、设置同步模式、生成主车辆和摄像头""" + # 连接Carla服务器(带重试逻辑) + client = carla.Client(CARLA_HOST, CARLA_PORT) + client.set_timeout(CARLA_TIMEOUT) + world = None + retry_count = 0 + + while retry_count < CARLA_RETRY_MAX and world is None: + try: + print(f"尝试连接Carla模拟器({retry_count + 1}/{CARLA_RETRY_MAX})...") + world = client.get_world() + except RuntimeError as e: + retry_count += 1 + print(f"连接失败:{e}") + if retry_count < CARLA_RETRY_MAX: + print(f"{CARLA_RETRY_DELAY}秒后重试...") + time.sleep(CARLA_RETRY_DELAY) + else: + raise RuntimeError( + f"多次重试后仍无法连接Carla模拟器,请检查:\n" + f"1. Carla模拟器是否已启动(运行CarlaUE4.exe)\n" + f"2. 模拟器端口是否为{CARLA_PORT}\n" + f"3. Python的carla库版本是否与模拟器一致" + ) + + print("Carla模拟器连接成功!") + + # 设置同步模式 + settings = world.get_settings() + settings.synchronous_mode = True + settings.fixed_delta_seconds = SYNC_DELTA_SECONDS + world.apply_settings(settings) + print("Carla同步模式已启用") + + # 获取蓝图库和生成点 + bp_lib = world.get_blueprint_library() + spawn_points = world.get_map().get_spawn_points() + if not spawn_points: + raise RuntimeError("未找到地图生成点,请检查Carla地图是否加载正常") + + # 生成主车辆(林肯MKZ 2020,确保生成成功) + vehicle_bp = bp_lib.find('vehicle.lincoln.mkz_2020') + vehicle = None + vehicle_retry = 0 + while not vehicle and vehicle_retry < 10: # 最多重试10次 + vehicle = world.try_spawn_actor(vehicle_bp, random.choice(spawn_points)) + vehicle_retry += 1 + if not vehicle: + raise RuntimeError("无法生成主车辆,请检查地图生成点是否可用") + print("主车辆生成成功") + + # 生成RGB摄像头传感器 + camera_bp = bp_lib.find('sensor.camera.rgb') + camera_bp.set_attribute('image_size_x', str(CAMERA_WIDTH)) + camera_bp.set_attribute('image_size_y', str(CAMERA_HEIGHT)) + camera_bp.set_attribute('fov', str(CAMERA_FOV)) + + camera = world.spawn_actor(camera_bp, CAMERA_POSITION, attach_to=vehicle) + print("摄像头传感器生成成功") + + # 创建图像队列,用于存储摄像头数据 + image_queue = queue.Queue(maxsize=1) # 限制队列大小,避免堆积 + def camera_callback(image, queue_obj): + """摄像头回调函数:将图像数据存入队列(确保数据连续且类型正确)""" + try: + # 转换原始数据为RGB图像(HWC,去除alpha通道) + img_data = np.reshape( + np.copy(image.raw_data), + (image.height, image.width, 4) + )[:, :, :3] # 去掉第四个alpha通道 + # 关键:转换为uint8的连续数组,避免OpenCV布局不兼容 + img_data = np.ascontiguousarray(img_data, dtype=np.uint8) + # 队列满时丢弃旧数据 + if not queue_obj.full(): + queue_obj.put(img_data) + except Exception as e: + print(f"摄像头回调函数出错:{e}") + traceback.print_exc() + + camera.listen(lambda image: camera_callback(image, image_queue)) + + # 清除现有NPC和静态车辆 + clear_npc(world) + clear_static_vehicle(world) + print("现有NPC和静态车辆已清除") + + # 生成NPC车辆 + spawn_npc_vehicles(world, bp_lib, spawn_points) + print(f"成功生成{NPC_VEHICLE_NUM}辆NPC车辆(尽可能)") + + # 开启主车辆自动驾驶 + vehicle.set_autopilot(True) + print("主车辆自动驾驶已开启") + + return world, client, vehicle, camera, image_queue + +def spawn_npc_vehicles(world, bp_lib, spawn_points): + """生成指定数量的NPC车辆并开启自动驾驶(容错处理)""" + npc_count = 0 + # 过滤四轮车辆(排除自行车、摩托车) + car_bps = [ + bp for bp in bp_lib.filter('vehicle') + if int(bp.get_attribute('number_of_wheels')) == 4 + ] + if not car_bps: + print("警告:未找到四轮车辆蓝图") + return + + # 循环生成,直到达到数量或无生成点 + max_spawn_retry = len(spawn_points) * 2 # 最大生成重试次数 + spawn_retry = 0 + while npc_count < NPC_VEHICLE_NUM and spawn_retry < max_spawn_retry: + bp = random.choice(car_bps) + spawn_point = random.choice(spawn_points) + npc = world.try_spawn_actor(bp, spawn_point) + if npc: + npc.set_autopilot(True) + npc_count += 1 + spawn_retry += 1 + + if npc_count < NPC_VEHICLE_NUM: + print(f"警告:仅生成了{npc_count}辆NPC车辆,未达到目标数量{NPC_VEHICLE_NUM}") + +# ====================== 目标检测与跟踪处理(修复数组索引+OpenCV图像兼容问题) ====================== +def process_detection_and_tracking(image, model, tracker, coco_names, detect_classes, detect_class_id): + """处理单帧图像的目标检测和OC-SORT跟踪(修复数组索引+OpenCV图像兼容问题)""" + try: + # ====================== 修复1:确保图像是连续的uint8数组,维度正确 ====================== + # 转换为连续数组,避免OpenCV布局不兼容 + img = np.ascontiguousarray(image, dtype=np.uint8) + # 校验图像维度:必须是(H, W, 3)的RGB图像 + if len(img.shape) != 3 or img.shape[2] != 3: + print(f"图像维度错误:{img.shape},预期(H, W, 3)") + return image + # 图像预处理:BGR转RGB(YOLO模型输入要求,注意OpenCV默认是BGR,这里确认转换) + img_rgb = cv2.cvtColor(img, cv2.COLOR_BGR2RGB) + + # YOLOv4推理 + _, boxes, labels, probs = model.predict(img_rgb) + + # ====================== 核心修复:数据维度与类型校验 ====================== + # 1. 确保labels是一维数组(处理模型返回的多维labels情况) + labels = np.ravel(labels) # 展平为一维数组 + # 2. 确保boxes是二维数组(形状为(n, 4),n为检测框数量) + if len(boxes.shape) != 2 or boxes.shape[-1] != 4: + # 若维度异常,直接返回原始图像(无检测结果) + return img + # 3. 确保probs是一维数组,且长度与labels/boxes一致 + probs = np.ravel(probs) + # 过滤掉长度不匹配的情况 + min_len = min(len(boxes), len(labels), len(probs)) + boxes = boxes[:min_len] + labels = labels[:min_len] + probs = probs[:min_len] + + # 过滤目标:只保留汽车、公交车、卡车(用整数索引替代np.isin,避免掩码问题) + keep_indices = [i for i, label in enumerate(labels) if label in detect_classes] + # 按整数索引过滤数组(确保索引是整数标量) + boxes = boxes[keep_indices] + probs = probs[keep_indices] + # 确保labels的长度与检测框一致 + labels = np.full(len(boxes), detect_class_id, dtype=np.int32) # 统一标注为汽车类别,指定int32类型 + + # 转换检测框格式:(xc, yc, w, h) -> (x1, y1, x2, y2)(OC-SORT要求) + dets = np.empty((0, 5), dtype=np.float32) # 初始化检测结果:(x1, y1, x2, y2, score) + if len(boxes) > 0: + height, width = img.shape[:2] + # 反归一化:将0-1的坐标转换为像素坐标(转为float32避免整数溢出) + boxes = boxes.astype(np.float32) + boxes[:, 0] *= width # xc(中心x) + boxes[:, 1] *= height # yc(中心y) + boxes[:, 2] *= width # w(宽度) + boxes[:, 3] *= height # h(高度) + + # 中心坐标转左上角、右下角坐标(显式重塑维度,避免拼接错误) + x1 = boxes[:, 0] - boxes[:, 2] / 2 + y1 = boxes[:, 1] - boxes[:, 3] / 2 + x2 = boxes[:, 0] + boxes[:, 2] / 2 + y2 = boxes[:, 1] + boxes[:, 3] / 2 + scores = probs.reshape(-1, 1) # 形状为(n, 1) + + # 拼接为(n, 5)的检测结果 + dets = np.hstack(( + x1.reshape(-1, 1), + y1.reshape(-1, 1), + x2.reshape(-1, 1), + y2.reshape(-1, 1), + scores + )).astype(np.float32) + + # 更新OC-SORT跟踪器 + height, width = img.shape[:2] + track_results = tracker.update(dets, [height, width], (height, width)) + + # 绘制检测框和跟踪ID + if len(track_results) > 0: + # ====================== 修复2:确保跟踪结果和标签长度匹配 ====================== + # 提取检测框和跟踪ID + track_boxes = track_results[:, :4].astype(np.int32) # 转换为整数坐标(OpenCV需要) + track_ids = track_results[:, 4].astype(np.int32) # 跟踪ID转为整数 + # 确保标签长度与跟踪框一致(若不足则补全) + if len(labels) < len(track_boxes): + labels = np.pad(labels, (0, len(track_boxes) - len(labels)), mode='constant', constant_values=detect_class_id) + else: + labels = labels[:len(track_boxes)] + # 调用绘制函数,传入正确的参数 + img = draw_bounding_boxes(img, track_boxes, labels, coco_names, track_ids) + + return img + except Exception as e: + print(f"检测与跟踪处理出错:{e}") + traceback.print_exc() # 打印完整错误堆栈,便于定位具体行 + return image # 出错时返回原始图像 + +# ====================== 主函数 ====================== +def main(): + """主函数:整合所有流程,包含完整的异常处理""" + # 初始化资源变量 + world = None + camera = None + try: + # 1. 加载YOLOv4模型 + print("开始加载YOLOv4模型...") + model, coco_class_names = load_yolov4_model() + print("YOLOv4模型加载成功") + + # 2. 初始化Carla环境 + print("开始初始化Carla环境...") + world, client, vehicle, camera, image_queue = init_carla_environment() + + # 3. 初始化OC-SORT跟踪器 + mot_tracker = OCSort( + det_thresh=OCSORT_DET_THRESH, + iou_threshold=OCSORT_IOU_THRESH, + use_byte=OCSORT_USE_BYTE + ) + print("OC-SORT跟踪器初始化成功") + + # 获取spectator(用于视角跟随) + spectator = world.get_spectator() + + # 4. 主循环 + print("进入主循环,按'q'退出...") + while True: + # 同步步进到下一帧 + world.tick() + + # 移动spectator到车辆上方的俯视视角 + vehicle_transform = vehicle.get_transform() + spectator_transform = carla.Transform( + vehicle_transform.transform(carla.Location(x=-4, z=50)), + carla.Rotation(yaw=-180, pitch=-90) + ) + spectator.set_transform(spectator_transform) + + # 获取摄像头图像(非阻塞,避免队列堆积) + if not image_queue.empty(): + origin_image = image_queue.get() + else: + continue + + # 处理检测和跟踪 + output_image = process_detection_and_tracking( + origin_image, model, mot_tracker, coco_class_names, + DETECT_CLASSES, DETECT_CLASS_ID + ) + + # 显示结果 + cv2.imshow('2D YOLOv4 OC-SORT Vehicle Tracking', output_image) + + # 按q退出(cv2.waitKey不能省略,否则窗口卡死) + if cv2.waitKey(1) & 0xFF == ord('q'): + print("用户按下'q',退出主循环") + break + + except KeyboardInterrupt: + print("\n用户中断程序(Ctrl+C)") + except Exception as e: + print(f"\n程序运行出错:{e}") + traceback.print_exc() + finally: + # 释放资源:恢复Carla设置、销毁演员、关闭窗口 + print("开始释放资源...") + if world is not None: + # 恢复Carla异步模式 + settings = world.get_settings() + settings.synchronous_mode = False + world.apply_settings(settings) + print("Carla同步模式已关闭") + if camera is not None: + clear(world, camera) # 销毁摄像头等演员 + print("Carla演员已销毁") + # 关闭OpenCV窗口 + cv2.destroyAllWindows() + print("资源释放完成,程序退出") + +if __name__ == '__main__': + main() \ No newline at end of file