From 94683a4f98ea03a525725977abedaee18ccfe651 Mon Sep 17 00:00:00 2001 From: Yuansugu123 <2451009878@qq.com> Date: Mon, 22 Dec 2025 21:45:43 +0800 Subject: [PATCH 1/2] =?UTF-8?q?=E5=B0=86=E5=85=B6=E4=BB=96=E4=BB=A3?= =?UTF-8?q?=E7=A0=81=E5=8A=9F=E8=83=BD=E5=90=88=E5=B9=B6=E8=BF=9Bmain,?= =?UTF-8?q?=E4=BC=98=E5=8C=96main=E4=BB=A3=E7=A0=81=EF=BC=9A=E7=BA=A2?= =?UTF-8?q?=E7=BB=BF=E7=81=AF=E7=9C=9F=E5=80=BC=E5=8F=AF=E8=A7=86=E5=8C=96?= =?UTF-8?q?=E4=B8=8E=203D=20=E4=BA=A4=E4=BA=92?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- src/car_tracking/README.md | 54 +- src/car_tracking/main.py | 1021 +++++++++++++++++++---------- src/car_tracking/requirements.txt | 15 +- 3 files changed, 724 insertions(+), 366 deletions(-) diff --git a/src/car_tracking/README.md b/src/car_tracking/README.md index 1243ab635e..1a58d46eb2 100644 --- a/src/car_tracking/README.md +++ b/src/car_tracking/README.md @@ -1,21 +1,43 @@ -# CARLA-DeepSORT-Vehicle-Tracking +# Carla 3D可视化与车辆跟踪系统 +这是基于Carla 0.9.14仿真平台的可视化项目,适配Python 3.7.9,实现2D/3D车辆可视化、交通灯显示、天气控制、数据统计及车辆跟踪功能。 -## 项目介绍 -基于CARLA仿真器和Deep SORT算法的2D车辆实时追踪程序,支持NPC生成、车辆追踪、关键信息显示,已修复文字反向、编码错误等问题。 +## 功能特点 +- **多维度可视化**:2D车辆检测框(带距离标注)+ 3D车辆边界框(距离衰减效果) +- **交通灯可视化**:投影红绿灯3D位置,显示绿/黄/红/未知状态及文字 +- **天气控制**:6种天气模式(晴/多云/雨/黄昏/雾/暴雨),支持自动/手动切换 +- **数据统计**:实时绘制最近50帧车辆数量、最大距离折线图 +- **交互控制**:键盘按键开关3D显示、交通灯显示等功能 -## 环境依赖 -- Python 3.7 -- CARLA 0.9.10+ -- 依赖库:`pip install numpy opencv-python scipy carla` +## 环境要求 +- 系统:Windows 10/11 / Linux(Ubuntu 18.04/20.04) +- Python:3.7.9 +- Carla:0.9.14(服务端与Python库版本一致) -## 快速开始 -1. 启动CARLA仿真器(建议低画质:`CarlaUE4.exe -quality-level=Low`) -2. 运行程序:`python main.py` -3. 操作:按`q`键退出 +## 安装依赖 +```bash +pip install -r requirements.txt +# 若Carla安装失败,用清华镜像: +# pip install carla==0.9.14 -i https://pypi.tuna.tsinghua.edu.cn/simple +``` -## 核心功能 -- 自动生成自车和20辆NPC(自动驾驶) -- Deep SORT实时追踪车辆,显示追踪ID和类别 -- 实时显示车速、追踪车辆数、地图名称 -- 自动清理资源,避免残留进程 +## 运行步骤 +1. 启动Carla 0.9.14服务端: + ```bash + # Windows + CarlaUE4.exe -carla-rpc-port=2000 + # Linux + ./CarlaUE4.sh -carla-rpc-port=2000 + ``` +2. 运行项目: + ```bash + python main.py + ``` +## 键盘交互 +| 按键 | 功能 | +|------|------| +| Q | 退出程序(自动清理资源) | +| 1-6 | 切换对应天气(1=晴,2=多云,3=雨,4=黄昏,5=雾,6=暴雨) | +| V | 开关车辆3D框显示 | +| T | 开关交通信号灯显示 | +| S | 开关红绿灯状态文字显示 | \ No newline at end of file diff --git a/src/car_tracking/main.py b/src/car_tracking/main.py index e90c6840ab..52e7827196 100644 --- a/src/car_tracking/main.py +++ b/src/car_tracking/main.py @@ -3,303 +3,267 @@ import random import cv2 import numpy as np +import time + +# ===================== 依赖导入(保留原始依赖,新增3D可视化相关)===================== +# 注意:如果以下导入报错,请根据实际环境调整,或注释后自行实现相关函数 +try: + from what.models.detection.datasets.coco import COCO_CLASS_NAMES + from utils.box_utils import draw_bounding_boxes + from utils.projection import get_image_point, build_projection_matrix, point_in_canvas, get_2d_box_from_3d_edges + from utils.world import clear_npc, clear_static_vehicle +except ImportError: + # 提供替代实现,避免代码无法运行 + COCO_CLASS_NAMES = ['person', 'bicycle', 'car', 'motorcycle', 'airplane', 'bus', 'train', 'truck', 'boat', 'traffic light'] + def draw_bounding_boxes(image, boxes, labels, class_names, ids=None): + img = image.copy() + for i, box in enumerate(boxes): + x1, y1, x2, y2 = map(int, box) + label = class_names[labels[i]] if labels[i] < len(class_names) else 'unknown' + cv2.rectangle(img, (x1, y1), (x2, y2), (0, 255, 0), 2) + cv2.putText(img, label, (x1, y1-5), cv2.FONT_HERSHEY_SIMPLEX, 0.5, (0, 255, 0), 1) + return img + def build_projection_matrix(w, h, fov, is_behind_camera=False): + fov_rad = np.deg2rad(fov) + fx = w / (2 * np.tan(fov_rad / 2)) + fy = h / (2 * np.tan(fov_rad / 2)) + cx = w / 2 + cy = h / 2 + matrix = np.array([[fx, 0, cx], [0, fy, cy], [0, 0, 1]]) + if is_behind_camera: + matrix[0, 0] = -fx + return matrix + def get_image_point(loc, K, w2c): + loc_vector = np.array([loc.x, loc.y, loc.z, 1]) + point_in_cam = np.dot(w2c, loc_vector) + point_in_cam = point_in_cam / point_in_cam[3] + point_in_2d = np.dot(K, point_in_cam[:3]) + point_in_2d = point_in_2d / point_in_2d[2] + return (point_in_2d[0], point_in_2d[1]) + def point_in_canvas(point, h, w): + return 0 <= point[0] <= w and 0 <= point[1] <= h + def get_2d_box_from_3d_edges(points, edges, h, w): + xs = [p[0] for p in points if point_in_canvas(p, h, w)] + ys = [p[1] for p in points if point_in_canvas(p, h, w)] + return min(xs) if xs else 0, max(xs) if xs else w, min(ys) if ys else 0, max(ys) if ys else h + def clear_npc(world): + for actor in world.get_actors().filter('*vehicle*'): + actor.destroy() + def clear_static_vehicle(world): + pass + +# ===================== 配置常量(集中管理,便于修改,新增3D可视化配置)===================== +# 相机配置 +CAMERA_WIDTH = 640 +CAMERA_HEIGHT = 640 +# 图表配置 +CHART_WIDTH = 400 +CHART_HEIGHT = CAMERA_HEIGHT # 与相机高度一致 +MAX_HISTORY_FRAMES = 50 # 最近50帧数据 +# 跟踪窗口配置 +TRACK_WINDOW_WIDTH = 300 +TRACK_WINDOW_HEIGHT = 400 +# 绘图配置 +FONT_SCALE_SMALL = 0.4 +FONT_SCALE_MEDIUM = 0.6 +LINE_THICKNESS = 2 +POINT_RADIUS = 2 +# 天气配置(完全自定义,不依赖Carla预定义属性) +WEATHER_SWITCH_INTERVAL = 10 # 随机天气切换间隔(秒,0表示不自动切换) +SUPPORTED_WEATHERS = { + 1: "ClearNoon", # 晴天正午 + 2: "CloudyNoon", # 多云正午 + 3: "RainyNoon", # 雨天正午 + 4: "Sunset", # 黄昏 + 5: "Foggy", # 雾天 + 6: "Stormy" # 暴雨 +} + +# 新增:3D可视化配置 +DISTANCE_THRESHOLD = 80 # 车辆3D框显示距离阈值(米) +TRAFFIC_LIGHT_DISTANCE = 60 # 红绿灯显示距离阈值(米) +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]] # 3D边界框边 +# 颜色定义(BGR格式) +VEHICLE_3D_COLOR = (0, 255, 0) # 车辆3D框默认颜色 +TRAFFIC_LIGHT_COLORS = { + 0: (0, 255, 0), # 绿色 + 1: (0, 255, 255), # 黄色 + 2: (0, 0, 255), # 红色 + 3: (255, 255, 255) # 白色(未知状态) +} +TRAFFIC_LIGHT_STATE_NAMES = { + 0: "GREEN", + 1: "YELLOW", + 2: "RED", + 3: "UNKNOWN" +} +SHOW_VEHICLES_3D = True # 是否显示车辆3D框 +SHOW_TRAFFIC_LIGHTS = True # 是否显示红绿灯 +SHOW_TRAFFIC_LIGHTS_STATE = True # 是否显示红绿灯状态文字 + +# ===================== 工具函数(独立封装,提升复用性,新增3D可视化函数)===================== +def get_vehicle_color(vehicle_id): + """为车辆生成固定唯一的RGB颜色(基于ID种子,保证跟踪时颜色不变)""" + np.random.seed(vehicle_id) + return tuple(np.random.randint(0, 255, 3).tolist()) + +def custom_draw_bounding_boxes(image, boxes, labels, class_names, ids=None, track_data=None): + """保留原始边界框绘制逻辑,叠加跟踪数据(距离+颜色外框)""" + # 调用原始画框函数,保证核心功能不变 + img = draw_bounding_boxes(image, boxes, labels, class_names, ids) + + # 叠加跟踪数据标注(不破坏原始框) + if ids is not None and track_data is not None and len(boxes) > 0: + for i, box in enumerate(boxes): + vid = ids[i] + if vid in track_data: + x1, y1, x2, y2 = map(int, box) + color = track_data[vid]['color'] + dist = track_data[vid]['distance'] + # 绘制距离文本(在原始标注下方) + cv2.putText( + img, f"Dist: {dist:.1f}m", (x1, y1 + 10), + cv2.FONT_HERSHEY_SIMPLEX, FONT_SCALE_SMALL, color, 1 + ) + # 绘制跟踪颜色外框(不覆盖原始绿色框) + cv2.rectangle(img, (x1 - 1, y1 - 1), (x2 + 1, y2 + 1), color, 1) + return img + +def init_chart_background(width, height): + """初始化图表背景(绘制固定元素:标题、网格、图例,避免每帧重复绘制)""" + chart = np.zeros((height, width, 3), dtype=np.uint8) + # 绘制标题 + cv2.putText( + chart, "Real-Time Statistics (Last 50 Frames)", (10, 30), + cv2.FONT_HERSHEY_SIMPLEX, FONT_SCALE_MEDIUM, (255, 255, 255), LINE_THICKNESS + ) + # 绘制网格(浅灰色,提升视觉效果) + grid_color = (50, 50, 50) + # 水平网格线 + for y in range(50, height - 30, 50): + cv2.line(chart, (50, y), (width - 50, y), grid_color, 1) + # 垂直网格线 + for x in range(50, width - 50, 50): + cv2.line(chart, (x, 30), (x, height - 30), grid_color, 1) + # 绘制图例(固定位置,提升可读性) + cv2.putText( + chart, "Current Vehicles (green)", (10, height - 10), + cv2.FONT_HERSHEY_SIMPLEX, FONT_SCALE_SMALL, (0, 255, 0), 1 + ) + cv2.putText( + chart, "Max Distance (red)", (200, height - 10), + cv2.FONT_HERSHEY_SIMPLEX, FONT_SCALE_SMALL, (0, 0, 255), 1 + ) + return chart + +def draw_dynamic_chart(history_frames, history_vehicles, history_max_dist): + """ + 绘制实时动态折线图(仅绘制变化的折线和数据点,复用固定背景) + 参数: + history_frames: 最近50帧的帧数列表 + history_vehicles: 最近50帧的车辆数量列表 + history_max_dist: 最近50帧的最大距离列表 + 返回: + 绘制完成的图表图像 + """ + # 初始化图表背景(固定元素) + chart = init_chart_background(CHART_WIDTH, CHART_HEIGHT) + + # 数据为空时直接返回背景 + if len(history_frames) == 0: + return chart + + # ========== 数据归一化(优化计算逻辑,避免除零错误)========== + # 车辆数量归一化(映射到图表y轴范围:30 ~ CHART_HEIGHT-30) + max_veh = max(history_vehicles) if history_vehicles else 1 + max_veh = max_veh if max_veh != 0 else 1 # 处理除零 + norm_veh = [(v / max_veh) * (CHART_HEIGHT - 60) for v in history_vehicles] + y_veh = np.array([CHART_HEIGHT - 30 - v for v in norm_veh], dtype=int) + + # 最大距离归一化 + max_d = max(history_max_dist) if history_max_dist else 1 + max_d = max_d if max_d != 0 else 1 # 处理除零 + norm_dist = [(d / max_d) * (CHART_HEIGHT - 60) for d in history_max_dist] + y_dist = np.array([CHART_HEIGHT - 30 - d for d in norm_dist], dtype=int) + + # x轴归一化(动态滚动,仅显示最近50帧的x坐标) + x_coords = np.array([ + 50 + (i * (CHART_WIDTH - 100) / (len(history_frames) - 1 if len(history_frames) > 1 else 1)) + for i in range(len(history_frames)) + ], dtype=int) + + # ========== 绘制折线和数据点(优化绘制逻辑,提升流畅度)========== + # 绘制车辆数量折线(绿色) + if len(x_coords) > 1: + cv2.polylines(chart, [np.column_stack((x_coords, y_veh))], isClosed=False, color=(0, 255, 0), + thickness=LINE_THICKNESS) + # 绘制车辆数量数据点 + for x, y in zip(x_coords, y_veh): + cv2.circle(chart, (x, y), POINT_RADIUS, (0, 255, 0), -1) + + # 绘制最大距离折线(红色) + if len(x_coords) > 1: + cv2.polylines(chart, [np.column_stack((x_coords, y_dist))], isClosed=False, color=(0, 0, 255), + thickness=LINE_THICKNESS) + # 绘制最大距离数据点 + for x, y in zip(x_coords, y_dist): + cv2.circle(chart, (x, y), POINT_RADIUS, (0, 0, 255), -1) + + # ========== 绘制当前数值标注(实时更新)========== + if len(history_vehicles) > 0 and len(history_max_dist) > 0: + current_veh = history_vehicles[-1] + current_dist = history_max_dist[-1] + cv2.putText( + chart, f"Now: {current_veh} cars | {current_dist:.1f}m", + (CHART_WIDTH - 200, 30), cv2.FONT_HERSHEY_SIMPLEX, FONT_SCALE_SMALL, + (255, 255, 255), 1 + ) + + return chart -from what.models.detection.datasets.coco import COCO_CLASS_NAMES -from utils.box_utils import draw_bounding_boxes -from utils.projection import * -from utils.world import * - -# -------------------------- 新增:车辆类型、距离、速度统计工具函数 -------------------------- -def get_vehicle_brand_type(actor): - """提取车辆的品牌和车型(从type_id中解析,如vehicle.lincoln.mkz → Lincoln MKZ)""" - try: - parts = actor.type_id.split('.') - if len(parts) >= 3: - brand = parts[1].capitalize() - model = parts[2].upper() - return f"{brand} {model}" - return "Unknown Vehicle" - except: - return "Unknown Vehicle" - -def get_vehicle_speed(vehicle): - """获取车辆的速度(km/h)""" - try: - velocity = vehicle.get_velocity() - # 转换为km/h:速度向量的模 × 3.6(m/s → km/h) - speed = np.sqrt(velocity.x**2 + velocity.y**2 + velocity.z**2) * 3.6 - return round(speed, 1) - except: - return 0.0 - -def calculate_vehicle_stats(vehicle_data): - """统计车辆类型、距离区间、速度区间""" - # 1. 车辆类型统计 - type_count = {} - # 2. 距离区间统计(0-10m, 10-20m, 20-30m, 30-40m, 40-50m) - distance_ranges = {"0-10m": 0, "10-20m": 0, "20-30m": 0, "30-40m": 0, "40-50m": 0} - # 3. 速度区间统计(0-10km/h, 10-20km/h, 20-30km/h, >30km/h) - speed_ranges = {"0-10km/h": 0, "10-20km/h": 0, "20-30km/h": 0, ">30km/h": 0} - - for _, v_type, dist, speed in vehicle_data: - # 类型统计 - type_count[v_type] = type_count.get(v_type, 0) + 1 - # 距离区间统计 - if dist < 10: - distance_ranges["0-10m"] += 1 - elif dist < 20: - distance_ranges["10-20m"] += 1 - elif dist < 30: - distance_ranges["20-30m"] += 1 - elif dist < 40: - distance_ranges["30-40m"] += 1 - else: - distance_ranges["40-50m"] += 1 - # 速度区间统计 - if speed < 10: - speed_ranges["0-10km/h"] += 1 - elif speed < 20: - speed_ranges["10-20km/h"] += 1 - elif speed < 30: - speed_ranges["20-30km/h"] += 1 - else: - speed_ranges[">30km/h"] += 1 - - return type_count, distance_ranges, speed_ranges - -def calculate_perception_stats(vehicle_distances, valid_boxes_count): - """基础感知统计(总车辆数、平均距离等)""" - stats = { - "total_vehicles": len(vehicle_distances), - "valid_boxes": valid_boxes_count, - "avg_distance": np.mean(vehicle_distances) if vehicle_distances else 0.0, - "max_distance": np.max(vehicle_distances) if vehicle_distances else 0.0, - "avg_speed": np.mean([d[3] for d in vehicle_data]) if vehicle_data else 0.0 # 平均速度 - } - return stats - -# -------------------------- 小框图核心函数(替换为新的统计维度) -------------------------- -def create_small_view_layout(main_img, base_stats, vehicle_data, type_count, distance_ranges, speed_ranges, CAMERA_WIDTH=640, CAMERA_HEIGHT=640): - """主视图+右侧统计面板(车辆类型、距离、速度统计)""" - canvas_width = CAMERA_WIDTH + 350 - canvas_height = CAMERA_HEIGHT - canvas = np.ones((canvas_height, canvas_width, 3), dtype=np.uint8) * 240 # 浅灰色背景 - - # 1. 主视图(左侧640x640) - canvas[:CAMERA_HEIGHT, :CAMERA_WIDTH, :] = main_img - - # 2. 右侧统计面板 - panel_x_start = CAMERA_WIDTH + 20 - # 标题 - cv2.putText(canvas, "Vehicle Perception Stats", (panel_x_start, 30), cv2.FONT_HERSHEY_SIMPLEX, 0.6, (0, 0, 0), 2) - # 基础统计数据 - base_stats_text = [ - f"Total Vehicles (50m): {base_stats['total_vehicles']}", - f"Valid 2D Boxes: {base_stats['valid_boxes']}", - f"Avg Distance: {base_stats['avg_distance']:.1f}m", - f"Max Distance: {base_stats['max_distance']:.1f}m", - f"Avg Speed: {base_stats['avg_speed']:.1f}km/h" - ] - y_start = 70 - line_height = 30 - for text in base_stats_text: - cv2.putText(canvas, text, (panel_x_start, y_start), cv2.FONT_HERSHEY_SIMPLEX, 0.5, (0, 0, 0), 1) - y_start += line_height - - # -------------------------- 统计1:车辆类型(前5种,避免过长) -------------------------- - cv2.putText(canvas, "Vehicle Type (Top5)", (panel_x_start, y_start + 10), cv2.FONT_HERSHEY_SIMPLEX, 0.6, (0, 0, 0), 2) - y_start += 40 - # 按数量排序,取前5种 - sorted_types = sorted(type_count.items(), key=lambda x: x[1], reverse=True)[:5] - for v_type, count in sorted_types: - if y_start > 220: # 为后续统计留出空间 - break - # 缩短过长的车型名称(避免超出面板) - display_type = v_type if len(v_type) <= 15 else v_type[:12] + "..." - text = f"{display_type}: {count} vehicles" - cv2.putText(canvas, text, (panel_x_start, y_start), cv2.FONT_HERSHEY_SIMPLEX, 0.45, (0, 0, 0), 1) - y_start += 25 - - # -------------------------- 统计2:距离区间(彩色进度条显示) -------------------------- - cv2.putText(canvas, "Distance Distribution", (panel_x_start, y_start + 10), cv2.FONT_HERSHEY_SIMPLEX, 0.6, (0, 0, 0), 2) - y_start += 40 - # 距离区间颜色映射(不同区间不同颜色) - dist_color_map = { - "0-10m": (0, 0, 255), "10-20m": (0, 165, 255), "20-30m": (0, 255, 255), - "30-40m": (0, 255, 0), "40-50m": (255, 0, 0) - } - for dist_range, count in distance_ranges.items(): - if y_start > 350: - break - # 绘制彩色小方块 - color = dist_color_map.get(dist_range, (128, 128, 128)) - sq_x1 = panel_x_start - sq_y1 = y_start - 8 - sq_x2 = panel_x_start + 15 - sq_y2 = y_start + 8 - cv2.rectangle(canvas, (sq_x1, sq_y1), (sq_x2, sq_y2), color, -1) - cv2.rectangle(canvas, (sq_x1, sq_y1), (sq_x2, sq_y2), (0, 0, 0), 1) - # 绘制距离区间和数量 - text = f"{dist_range}: {count} vehicles" - cv2.putText(canvas, text, (panel_x_start + 20, y_start + 5), cv2.FONT_HERSHEY_SIMPLEX, 0.45, (0, 0, 0), 1) - y_start += 25 - - # -------------------------- 统计3:速度区间(彩色进度条显示) -------------------------- - cv2.putText(canvas, "Speed Distribution", (panel_x_start, y_start + 10), cv2.FONT_HERSHEY_SIMPLEX, 0.6, (0, 0, 0), 2) - y_start += 40 - # 速度区间颜色映射 - speed_color_map = { - "0-10km/h": (128, 128, 128), "10-20km/h": (0, 255, 0), - "20-30km/h": (0, 255, 255), ">30km/h": (0, 0, 255) - } - for speed_range, count in speed_ranges.items(): - if y_start > canvas_height - 20: - break - # 绘制彩色小方块 - color = speed_color_map.get(speed_range, (128, 128, 128)) - sq_x1 = panel_x_start - sq_y1 = y_start - 8 - sq_x2 = panel_x_start + 15 - sq_y2 = y_start + 8 - cv2.rectangle(canvas, (sq_x1, sq_y1), (sq_x2, sq_y2), color, -1) - cv2.rectangle(canvas, (sq_x1, sq_y1), (sq_x2, sq_y2), (0, 0, 0), 1) - # 绘制速度区间和数量 - text = f"{speed_range}: {count} vehicles" - cv2.putText(canvas, text, (panel_x_start + 20, y_start + 5), cv2.FONT_HERSHEY_SIMPLEX, 0.45, (0, 0, 0), 1) - y_start += 25 - - return canvas - -# -------------------------- 原始代码:相机回调函数 -------------------------- def camera_callback(image, rgb_image_queue): - rgb_image_queue.put(np.reshape(np.copy(image.raw_data), - (image.height, image.width, 4))) - -# -------------------------- 主程序(替换为新的统计维度) -------------------------- -def main(): - # 1. 连接CARLA并设置超时 - client = carla.Client('localhost', 2000) - client.set_timeout(60.0) - world = client.get_world() - - # 2. 配置仿真环境(同步模式+Traffic Manager) - settings = world.get_settings() - settings.synchronous_mode = True - settings.fixed_delta_seconds = 0.05 - world.apply_settings(settings) - - # 初始化Traffic Manager(车辆移动核心) - tm = client.get_trafficmanager(8000) - tm.set_global_distance_to_leading_vehicle(2.0) - tm.set_random_device_seed(42) - tm.global_percentage_speed_difference(20) # 车辆速度80%限速 - - # 3. 获取出生点并生成主角车辆 - spawn_points = world.get_map().get_spawn_points() - if not spawn_points: - print("❌ 没有可用的出生点!") - return - - bp_lib = world.get_blueprint_library() - vehicle_bp = bp_lib.find('vehicle.lincoln.mkz_2017') # 更换为更常见的车型 - ego_vehicle = None - for sp in random.sample(spawn_points, min(10, len(spawn_points))): - ego_vehicle = world.try_spawn_actor(vehicle_bp, sp) - if ego_vehicle: - break - if not ego_vehicle: - print("❌ 主角车辆生成失败!") - return - ego_vehicle.set_autopilot(True, tm.get_port()) - - # 4. 生成相机(保留原始参数) - 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_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() - camera.listen(lambda image: camera_callback(image, image_queue)) - - # 5. 清理旧NPC并生成新NPC(生成不同品牌的车辆,丰富类型统计) - clear_npc(world) - clear_static_vehicle(world) - - # 选择不同品牌的车辆蓝图(丰富类型统计) - vehicle_blueprints = [ - bp for bp in bp_lib.filter('vehicle') - if int(bp.get_attribute('number_of_wheels')) == 4 and - not bp.id.endswith('cycle') and not bp.id.endswith('motorcycle') - ] - # 生成50辆NPC车辆(不同品牌) - for i in range(50): - if not vehicle_blueprints: - break - npc_bp = random.choice(vehicle_blueprints) - npc_vehicle = None - # 遍历多个出生点,确保NPC生成成功 - for sp in random.sample(spawn_points, min(5, len(spawn_points))): - npc_vehicle = world.try_spawn_actor(npc_bp, sp) - if npc_vehicle: - break - if npc_vehicle: - npc_vehicle.set_autopilot(True, tm.get_port()) + """相机回调函数(保留原始逻辑,简化代码)""" + rgb_image_queue.put(np.reshape(np.copy(image.raw_data), (image.height, image.width, 4))) - # 6. 初始化 spectator 视角 - spectator = world.get_spectator() - 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]] +def convert_image_format(image): + """将4通道BGRA图像转换为3通道RGB图像(提取前3通道,简化逻辑)""" + return image[..., :3] if image.shape[-1] == 4 else image.copy() - # 7. 主循环(车辆感知核心) +# 新增:3D物体(车辆、红绿灯)绘制函数 +def draw_3d_objects(image, world, camera, vehicle, K, K_b): + """在图像上绘制3D车辆边界框和交通信号灯(整合红绿灯真值数据和投影功能)""" try: - while True: - world.tick() - - # 更新 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 image_queue.empty(): - continue - image = image_queue.get() - - # 更新投影矩阵(每帧更新,确保投影准确) - world_2_camera = np.array(camera.get_transform().get_inverse_matrix()) - image_w = camera_bp.get_attribute("image_size_x").as_int() - image_h = camera_bp.get_attribute("image_size_y").as_int() - 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) - - boxes = [] - ids = [] - vehicle_data = [] # 格式:(id, type, distance, speed) - vehicle_distances = [] - - # 遍历所有车辆(筛选+投影) - for npc in world.get_actors().filter('*vehicle*'): - if npc.id == ego_vehicle.id: + img = image.copy() + height, width = CAMERA_HEIGHT, CAMERA_WIDTH + world_2_camera = np.array(camera.get_transform().get_inverse_matrix()) + + vehicle_count_3d = 0 + traffic_light_count = 0 + + # 绘制车辆3D边界框 + if SHOW_VEHICLES_3D: + vehicles = list(world.get_actors().filter('*vehicle*')) + for npc in vehicles: + if npc.id == vehicle.id: continue - bb = npc.bounding_box - dist = npc.get_transform().location.distance(ego_vehicle.get_transform().location) - - # 过滤1:50米内 - if dist > 50: + # 计算距离,过滤超出阈值的车辆 + dist = npc.get_transform().location.distance(vehicle.get_transform().location) + if dist >= DISTANCE_THRESHOLD: continue - # 过滤2:正前方(向量单位化,点积阈值0.1) - forward_vec = ego_vehicle.get_transform().get_forward_vector() - ray = npc.get_transform().location - ego_vehicle.get_transform().location - ray = ray.make_unit_vector() - dot_product = forward_vec.dot(ray) - if dot_product <= 0.1: + # 检查是否在车辆前方 + forward_vec = vehicle.get_transform().get_forward_vector() + ray = npc.get_transform().location - vehicle.get_transform().location + if forward_vec.dot(ray) <= 0: continue - # 3D转2D投影(过滤无效点) - verts = [v for v in bb.get_world_vertices(npc.get_transform())] + # 获取车辆3D边界框顶点 + bb = npc.bounding_box + verts = bb.get_world_vertices(npc.get_transform()) + + # 投影到2D图像平面(3D转2D核心逻辑) points_2d = [] for vert in verts: ray0 = vert - camera.get_transform().location @@ -308,69 +272,446 @@ def main(): p = get_image_point(vert, K, world_2_camera) else: p = get_image_point(vert, K_b, world_2_camera) - if not (np.isnan(p[0]) or np.isnan(p[1])): - points_2d.append(p) + points_2d.append(p) + + # 绘制3D边界框的边 + for edge in EDGES: + p1 = points_2d[edge[0]] + p2 = points_2d[edge[1]] + if point_in_canvas(p1, height, width) or point_in_canvas(p2, height, width): + # 距离衰减:越远越细、颜色越浅 + thickness = max(1, int(2 - dist / 50)) + color_intensity = max(50, int(255 - dist)) + color = (0, color_intensity, 0) + cv2.line(img, (int(p1[0]), int(p1[1])), + (int(p2[0]), int(p2[1])), color, thickness) + vehicle_count_3d += 1 + + # 绘制交通信号灯(整合红绿灯真值数据获取和状态绘制) + if SHOW_TRAFFIC_LIGHTS: + # 1. 获取所有红绿灯(真值数据) + traffic_lights = list(world.get_actors().filter('*traffic_light*')) + for light in traffic_lights: + # 计算距离,过滤超出阈值的红绿灯 + dist = light.get_transform().location.distance(vehicle.get_transform().location) + if dist >= TRAFFIC_LIGHT_DISTANCE: + continue + + # 检查是否在车辆前方 + forward_vec = vehicle.get_transform().get_forward_vector() + ray = light.get_transform().location - vehicle.get_transform().location + if forward_vec.dot(ray) <= 0: + continue - # 至少4个有效点才计算边界框 - if len(points_2d) < 4: + # 2. 将3D世界坐标投影到2D图像坐标(核心投影逻辑) + location = light.get_transform().location + ray0 = location - camera.get_transform().location + cam_forward_vec = camera.get_transform().get_forward_vector() + if cam_forward_vec.dot(ray0) > 0: + point_2d = get_image_point(location, K, world_2_camera) + else: + point_2d = get_image_point(location, K_b, world_2_camera) + + # 检查点是否在画布内 + if not point_in_canvas(point_2d, height, width): continue - x_min, x_max, y_min, y_max = get_2d_box_from_3d_edges(points_2d, edges, image_h, image_w) - box_width = x_max - x_min - box_height = y_max - y_min - box_area = box_width * box_height - - # 过滤无效小框(降低阈值,避免漏检) - if box_area > 50 and box_width > 10: - if point_in_canvas((x_min, y_min), image_h, image_w) and point_in_canvas((x_max, y_max), image_h, image_w): - ids.append(npc.id) - boxes.append(np.array([x_min, y_min, x_max, y_max])) - # 收集车辆类型、距离、速度(核心:新的统计数据) - v_type = get_vehicle_brand_type(npc) - v_speed = get_vehicle_speed(npc) - vehicle_data.append((npc.id, v_type, dist, v_speed)) - vehicle_distances.append(dist) - - # 绘制边界框(保留原始调用) + x, y = int(point_2d[0]), int(point_2d[1]) + + # 3. 获取红绿灯状态(真值数据)并映射颜色 + light_state = light.get_state() + # 状态映射:Green->0, Yellow->1, Red->2, 其他->3 + state_mapping = { + carla.TrafficLightState.Green: 0, + carla.TrafficLightState.Yellow: 1, + carla.TrafficLightState.Red: 2, + } + state_idx = state_mapping.get(light_state, 3) + light_color = TRAFFIC_LIGHT_COLORS[state_idx] + state_name = TRAFFIC_LIGHT_STATE_NAMES[state_idx] + + # 绘制红绿灯圆形标记(距离衰减) + radius = max(6, int(15 - dist / 20)) + cv2.circle(img, (x, y), radius, light_color, -1) + cv2.circle(img, (x, y), radius, (255, 255, 255), 1) # 白色描边 + + # 绘制红绿灯状态文字(可选) + if SHOW_TRAFFIC_LIGHTS_STATE and radius > 4: # 半径足够大时才显示文字 + text_size = cv2.getTextSize(state_name, cv2.FONT_HERSHEY_SIMPLEX, 0.5, 1)[0] + text_x = x - text_size[0] // 2 + text_y = y - radius - 5 # 文字在圆形上方 + # 绘制文字背景框,提升可读性 + cv2.rectangle(img, (text_x - 3, text_y - text_size[1] - 3), + (text_x + text_size[0] + 3, text_y + 3), + (40, 40, 40), -1) # 深色背景 + # 绘制状态文字 + cv2.putText(img, state_name, (text_x, text_y), + cv2.FONT_HERSHEY_SIMPLEX, 0.5, (255, 255, 255), 1) + + traffic_light_count += 1 + + return img, vehicle_count_3d, traffic_light_count + except Exception as e: + print(f"3D物体绘制错误:{e}") + return image, 0, 0 + +# ===================== 天气控制函数(完全自定义,不依赖任何预定义属性)===================== +def set_weather(world, weather_type): + """ + 设置Carla世界的天气(完全通过参数配置,兼容所有Carla版本) + 参数: + world: Carla的world对象 + weather_type: 天气类型(字符串,支持ClearNoon/CloudyNoon/RainyNoon/Sunset/Foggy/Stormy) + """ + # 初始化天气参数(所有参数默认值为0) + weather = carla.WeatherParameters() + + if weather_type == "ClearNoon": + # 晴天正午:无云、无雨、无雾,太阳高度高(60度) + weather.sun_altitude_angle = 60.0 # 太阳高度角(0=地平线,90=天顶) + weather.cloudiness = 0.0 # 云量(0-100) + weather.precipitation = 0.0 # 降雨量(0-100) + weather.wetness = 0.0 # 地面湿润度(0-100) + weather.fog_density = 0.0 # 雾浓度(0-100) + + elif weather_type == "CloudyNoon": + # 多云正午:高云量(80),太阳高度角60度,少量风 + weather.sun_altitude_angle = 60.0 + weather.cloudiness = 80.0 + weather.wind_intensity = 20.0 # 风速(0-100) + weather.precipitation = 0.0 + weather.wetness = 0.0 + weather.fog_density = 0.0 + + elif weather_type == "RainyNoon": + # 雨天正午:中雨(50)、高云量(80)、地面湿润(80)、风速30 + weather.sun_altitude_angle = 60.0 + weather.cloudiness = 80.0 + weather.precipitation = 50.0 # 降雨量 + weather.precipitation_deposits = 20.0 # 降水沉积(地面积水) + weather.wetness = 80.0 # 地面湿润度 + weather.wind_intensity = 30.0 + weather.fog_density = 10.0 # 少量雾 + + elif weather_type == "Sunset": + # 黄昏:太阳高度角低(10度)、橙色色调、少量云、低风速 + weather.sun_altitude_angle = 10.0 # 太阳高度角低(黄昏效果) + weather.sun_azimuth_angle = 180.0 # 太阳方位角(180=西方) + weather.cloudiness = 30.0 + weather.wind_intensity = 10.0 + weather.precipitation = 0.0 + weather.fog_density = 5.0 # 轻微雾(增强黄昏氛围) + + elif weather_type == "Foggy": + # 雾天:高雾浓度(90)、低可见距离(10)、低太阳高度、少量云 + weather.sun_altitude_angle = 30.0 + weather.cloudiness = 20.0 + weather.fog_density = 90.0 # 雾浓度(越高雾越浓) + weather.fog_distance = 10.0 # 雾的可见距离(越小雾越近) + weather.fog_falloff = 1.0 # 雾的衰减率 + weather.precipitation = 0.0 + weather.wetness = 0.0 + + elif weather_type == "Stormy": + # 暴雨:最大降雨量(100)、高云量(100)、大风(70)、地面完全湿润(100)、中度雾 + weather.sun_altitude_angle = 30.0 + weather.cloudiness = 100.0 + weather.precipitation = 100.0 # 最大降雨量 + weather.precipitation_deposits = 50.0 # 大量积水 + weather.wetness = 100.0 # 地面完全湿润 + weather.wind_intensity = 70.0 # 大风 + weather.fog_density = 30.0 # 中度雾(暴雨伴随雾) + + # 应用天气设置到世界 + world.set_weather(weather) + print(f"当前天气已切换为:{weather_type}") + +def get_random_weather(): + """获取随机的天气类型(从SUPPORTED_WEATHERS中随机选择)""" + weather_codes = list(SUPPORTED_WEATHERS.keys()) + random_code = random.choice(weather_codes) + return SUPPORTED_WEATHERS[random_code] + +# ===================== 主程序逻辑(优化结构,修复所有已知问题,整合3D可视化)===================== +def main(): + # 声明全局变量用于按键控制3D显示 + global SHOW_VEHICLES_3D, SHOW_TRAFFIC_LIGHTS, SHOW_TRAFFIC_LIGHTS_STATE + # 初始化变量,用于资源清理 + camera = None + vehicle = None + world = None + try: + # 初始化Carla客户端和世界 + client = carla.Client('localhost', 2000) + # 设置超时时间,避免连接卡顿 + client.set_timeout(10.0) + world = client.get_world() + + # 设置同步模式(保留原始逻辑) + settings = world.get_settings() + settings.synchronous_mode = True + settings.fixed_delta_seconds = 0.05 + world.apply_settings(settings) + + # 获取观众和生成点(保留原始逻辑) + spectator = world.get_spectator() + spawn_points = world.get_map().get_spawn_points() + + # 生成主车辆(保留原始逻辑,增加异常处理) + bp_lib = world.get_blueprint_library() + vehicle_bp = bp_lib.find('vehicle.lincoln.mkz_2020') + vehicle = world.try_spawn_actor(vehicle_bp, random.choice(spawn_points)) + if not vehicle: + print("警告:主车辆生成失败,程序退出!") + return + + # 生成相机(使用配置常量,简化代码) + 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_init_trans = carla.Transform(carla.Location(x=1, z=2)) + camera = world.spawn_actor(camera_bp, camera_init_trans, attach_to=vehicle) + + # 初始化图像队列(保留原始逻辑) + image_queue = queue.Queue() + # 先停止监听(防止之前的残留),再开始监听 + if camera.is_listening: + camera.stop() + camera.listen(lambda image: camera_callback(image, image_queue)) + + # 清理现有NPC(保留原始逻辑) + clear_npc(world) + clear_static_vehicle(world) + + # 2D框计算相关参数(保留原始逻辑,复用K和K_b用于3D投影) + edges = EDGES # 复用3D边界框边定义 + fov = camera_bp.get_attribute("fov").as_float() + K = build_projection_matrix(CAMERA_WIDTH, CAMERA_HEIGHT, fov) + K_b = build_projection_matrix(CAMERA_WIDTH, CAMERA_HEIGHT, fov, is_behind_camera=True) + + # 生成NPC车辆(保留原始逻辑,使用配置常量) + for _ in range(50): + vehicle_bp_list = bp_lib.filter('vehicle') + car_bp = [bp for bp in vehicle_bp_list if int(bp.get_attribute('number_of_wheels')) == 4] + if car_bp: + npc = world.try_spawn_actor(random.choice(car_bp), random.choice(spawn_points)) + if npc: + npc.set_autopilot(True) + if vehicle: + vehicle.set_autopilot(True) + + # 初始化跟踪和数据缓存变量(改为局部变量,减少全局变量) + tracked_vehicles = {} # key:车辆ID, value:{'color':颜色, 'distance':距离, 'frame':帧数} + frame_counter = 0 + history_frames = [] # 最近50帧的帧数 + history_vehicles = [] # 最近50帧的车辆数量 + history_max_dist = [] # 最近50帧的最大距离 + + # ========== 初始化天气(新增)========== + current_weather = "ClearNoon" # 默认晴天 + set_weather(world, current_weather) + last_weather_switch_time = time.time() # 记录最后一次天气切换时间 + + # 主循环(优化逻辑,提升可读性,加入3D可视化) + while True: + world.tick() + frame_counter += 1 + + # ========== 天气切换逻辑(新增)========== + # 1. 自动随机切换天气(如果配置了间隔) + auto_switch = False + if WEATHER_SWITCH_INTERVAL > 0: + current_time = time.time() + if current_time - last_weather_switch_time >= WEATHER_SWITCH_INTERVAL: + current_weather = get_random_weather() + set_weather(world, current_weather) + last_weather_switch_time = current_time + auto_switch = True + + # 2. 键盘按键处理(合并逻辑,避免重复调用cv2.waitKey,新增3D显示控制) + key = cv2.waitKey(1) & 0xFF + if key == ord('q'): + break + # 按数字键1-6切换对应天气(仅在非自动切换时响应,避免冲突) + elif not auto_switch and key in [ord(str(code)) for code in SUPPORTED_WEATHERS.keys()]: + weather_code = int(chr(key)) + current_weather = SUPPORTED_WEATHERS[weather_code] + set_weather(world, current_weather) + last_weather_switch_time = time.time() # 重置自动切换时间 + # 新增:按键控制3D显示 + elif key == ord('v'): + SHOW_VEHICLES_3D = not SHOW_VEHICLES_3D + print(f"车辆3D框显示:{'开启' if SHOW_VEHICLES_3D else '关闭'}") + elif key == ord('t'): + SHOW_TRAFFIC_LIGHTS = not SHOW_TRAFFIC_LIGHTS + print(f"红绿灯显示:{'开启' if SHOW_TRAFFIC_LIGHTS else '关闭'}") + elif key == ord('s'): + SHOW_TRAFFIC_LIGHTS_STATE = not SHOW_TRAFFIC_LIGHTS_STATE + print(f"红绿灯状态文字显示:{'开启' if SHOW_TRAFFIC_LIGHTS_STATE else '关闭'}") + + # 移动观众视角到车辆顶部(保留原始逻辑) + if vehicle: + transform = carla.Transform( + vehicle.get_transform().transform(carla.Location(x=-4, z=50)), + carla.Rotation(yaw=-180, pitch=-90) + ) + spectator.set_transform(transform) + + # 获取相机图像(保留原始逻辑) + image = image_queue.get() + + # 更新相机矩阵(保留原始逻辑) + world_2_camera = np.array(camera.get_transform().get_inverse_matrix()) + + # 初始化当前帧变量 + boxes = [] + ids = [] + track_data = {} + current_vehicles = 0 + max_distance = 0.0 + + # 检测车辆并计算2D边界框(保留原始核心逻辑) + for npc in world.get_actors().filter('*vehicle*'): + if vehicle and npc.id != vehicle.id: + # 计算车辆距离和最大距离 + dist = npc.get_transform().location.distance(vehicle.get_transform().location) + max_distance = max(max_distance, dist) + + # 过滤50米内、车辆前方的目标 + if dist < 50: + forward_vec = vehicle.get_transform().get_forward_vector() + ray = npc.get_transform().location - vehicle.get_transform().location + if forward_vec.dot(ray) > 0: + # 计算3D顶点的2D投影 + verts = [v for v in npc.bounding_box.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, CAMERA_HEIGHT, CAMERA_WIDTH + ) + + # 过滤小框和超出画布的框 + if (y_max - y_min) * (x_max - x_min) > 100 and (x_max - x_min) > 20: + if point_in_canvas((x_min, y_min), CAMERA_HEIGHT, CAMERA_WIDTH) and \ + point_in_canvas((x_max, y_max), CAMERA_HEIGHT, CAMERA_WIDTH): + ids.append(npc.id) + boxes.append(np.array([x_min, y_min, x_max, y_max])) + # 更新跟踪数据 + if npc.id not in tracked_vehicles: + tracked_vehicles[npc.id] = {'color': get_vehicle_color(npc.id)} + tracked_vehicles[npc.id]['distance'] = dist + tracked_vehicles[npc.id]['frame'] = frame_counter + track_data[npc.id] = tracked_vehicles[npc.id] + current_vehicles += 1 + + # 更新历史数据缓存(仅保留最近50帧,优化逻辑) + history_frames.append(frame_counter) + history_vehicles.append(current_vehicles) + history_max_dist.append(max_distance) + # 截断数据,保持固定长度 + if len(history_frames) > MAX_HISTORY_FRAMES: + history_frames.pop(0) + history_vehicles.pop(0) + history_max_dist.pop(0) + + # 绘制边界框(保留原始逻辑,使用自定义函数) boxes = np.array(boxes) - labels = np.array([2] * len(boxes)) + labels = np.array([2] * len(boxes)) # 2对应COCO的car类别 probs = np.array([1.0] * len(boxes)) - output_image = image - if len(boxes) > 0: - output_image = draw_bounding_boxes(image, boxes, labels, COCO_CLASS_NAMES, ids) - - # 计算统计数据(新的维度) - type_count, distance_ranges, speed_ranges = calculate_vehicle_stats(vehicle_data) - # 基础统计(补充平均速度) - base_stats = { - "total_vehicles": len(vehicle_distances), - "valid_boxes": len(boxes), - "avg_distance": np.mean(vehicle_distances) if vehicle_distances else 0.0, - "max_distance": np.max(vehicle_distances) if vehicle_distances else 0.0, - "avg_speed": np.mean([d[3] for d in vehicle_data]) if vehicle_data else 0.0 - } - - # 生成小框图并显示 - if output_image.shape[-1] == 4: - main_img = output_image[:, :, :3].astype(np.uint8) - else: - main_img = output_image.astype(np.uint8) - canvas = create_small_view_layout(main_img, base_stats, vehicle_data, type_count, distance_ranges, speed_ranges, image_w, image_h) - cv2.imshow('2D Ground Truth (Vehicle Stats)', canvas) - - # 退出条件:按q键 - if cv2.waitKey(1) == ord('q'): - break + output = custom_draw_bounding_boxes( + image, boxes, labels, COCO_CLASS_NAMES, ids, track_data + ) if len(boxes) > 0 else image + + # ========== 新增:绘制3D物体(车辆3D框、红绿灯)========== + output_3d, _, _ = draw_3d_objects(output, world, camera, vehicle, K, K_b) + + # 转换图像格式并拼接图表(优化逻辑,使用带3D的图像) + output_rgb = convert_image_format(output_3d) + chart_image = draw_dynamic_chart(history_frames, history_vehicles, history_max_dist) + combined_image = np.hstack((output_rgb, chart_image)) + + # ========== 绘制天气信息到图像(新增)========== + cv2.putText( + combined_image, f"Weather: {current_weather}", + (10, 30), cv2.FONT_HERSHEY_SIMPLEX, FONT_SCALE_MEDIUM, (255, 255, 255), 2 + ) + # 绘制天气和3D控制提示 + cv2.putText( + combined_image, "Press 1-6:Weather | V:3D Vehicles | T:Traffic Lights | S:Light State | Q:Quit", + (10, CAMERA_HEIGHT - 10), cv2.FONT_HERSHEY_SIMPLEX, FONT_SCALE_SMALL, (255, 255, 255), 1 + ) + + # 绘制跟踪监测窗口(保留原始功能,优化代码) + track_window = np.zeros((TRACK_WINDOW_HEIGHT, TRACK_WINDOW_WIDTH, 3), dtype=np.uint8) + cv2.putText( + track_window, "Vehicle Tracking Monitor", (10, 30), + cv2.FONT_HERSHEY_SIMPLEX, FONT_SCALE_MEDIUM, (255, 255, 255), LINE_THICKNESS + ) + # 新增:在跟踪窗口显示当前天气和3D显示状态 + cv2.putText( + track_window, f"Current Weather: {current_weather}", + (10, 60), cv2.FONT_HERSHEY_SIMPLEX, FONT_SCALE_SMALL, (255, 255, 255), 1 + ) + cv2.putText( + track_window, f"3D Vehicles: {'ON' if SHOW_VEHICLES_3D else 'OFF'}", + (10, 85), cv2.FONT_HERSHEY_SIMPLEX, FONT_SCALE_SMALL, (0, 255, 0), 1 + ) + cv2.putText( + track_window, f"Traffic Lights: {'ON' if SHOW_TRAFFIC_LIGHTS else 'OFF'}", + (10, 110), cv2.FONT_HERSHEY_SIMPLEX, FONT_SCALE_SMALL, (0, 255, 255), 1 + ) + y_offset = 135 + # 显示前10辆跟踪的车辆 + for vid, data in list(tracked_vehicles.items())[:10]: + if y_offset > TRACK_WINDOW_HEIGHT - 20: + break + color = data['color'] + dist = data.get('distance', 0.0) + cv2.putText( + track_window, f"ID: {vid} | Dist: {dist:.1f}m", (10, y_offset), + cv2.FONT_HERSHEY_SIMPLEX, FONT_SCALE_SMALL, color, 1 + ) + y_offset += 30 + + # 显示窗口(保留原始逻辑) + cv2.imshow('2D Ground Truth + 3D Visualization', combined_image) + cv2.imshow('Vehicle Tracking Monitor', track_window) except KeyboardInterrupt: - print("\n🛑 程序被用户中断") + print("程序被用户中断!") + except Exception as e: + print(f"程序运行出错:{e}") finally: - # 清理资源(恢复异步模式) - settings.synchronous_mode = False - world.apply_settings(settings) - clear(world, camera) - ego_vehicle.destroy() + # 清理资源(修复传感器注销警告,优化异常处理) + print("开始清理资源...") + # 1. 停止相机监听并销毁(避免注销警告) + if camera: + if camera.is_listening: + camera.stop() + camera.destroy() + print("相机已销毁") + # 2. 销毁主车辆 + if vehicle: + vehicle.destroy() + print("主车辆已销毁") + # 3. 恢复Carla世界设置(避免同步模式残留) + if world: + settings = world.get_settings() + settings.synchronous_mode = False + world.apply_settings(settings) + print("Carla同步模式已关闭") + # 4. 关闭OpenCV窗口 cv2.destroyAllWindows() + print("资源清理完成") if __name__ == '__main__': main() \ No newline at end of file diff --git a/src/car_tracking/requirements.txt b/src/car_tracking/requirements.txt index d4a0e68678..7d979b2d39 100644 --- a/src/car_tracking/requirements.txt +++ b/src/car_tracking/requirements.txt @@ -1,11 +1,6 @@ +# 适配Python3.7.9 + Carla 0.9.14的核心依赖库 carla==0.9.14 -tensorflow==2.4.0 -numpy==1.19.5 -opencv-python -scipy -psutil -lapx -pillow -tqdm -xmltodict -ultralytics \ No newline at end of file +opencv-python==4.5.5.62 +numpy==1.21.6 + +# 备注:queue、random、time为Python3.7.9内置库,无需额外安装 \ No newline at end of file From 8de15e0cef3aba8963b4c8ff93925d3625006af0 Mon Sep 17 00:00:00 2001 From: Yuansugu123 <2451009878@qq.com> Date: Tue, 23 Dec 2025 15:05:10 +0800 Subject: [PATCH 2/2] =?UTF-8?q?=E5=B7=B2=E5=B0=86=E4=BB=A3=E7=A0=81?= =?UTF-8?q?=E5=8A=9F=E8=83=BD=E5=90=88=E5=B9=B6=E8=BF=9Bmain.py=EF=BC=8C?= =?UTF-8?q?=E5=88=A0=E9=99=A4=E4=BB=A3=E7=A0=81=E6=96=87=E4=BB=B6?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- src/car_tracking/car1.py | 632 ----------------------- src/car_tracking/car_2d_ground_truth.py | 362 -------------- src/car_tracking/caryolo.py | 636 ------------------------ 3 files changed, 1630 deletions(-) delete mode 100644 src/car_tracking/car1.py delete mode 100644 src/car_tracking/car_2d_ground_truth.py delete mode 100644 src/car_tracking/caryolo.py diff --git a/src/car_tracking/car1.py b/src/car_tracking/car1.py deleted file mode 100644 index 448f8e4022..0000000000 --- a/src/car_tracking/car1.py +++ /dev/null @@ -1,632 +0,0 @@ -# 导入必要的库 -import carla # Carla仿真器的Python API -import queue # 用于相机图像的队列存储 -import random # 用于随机生成NPC车辆和颜色 -import cv2 # OpenCV库,用于图像处理和显示 -import numpy as np # 数值计算库,用于矩阵和数组操作 -import time # 用于计时和FPS计算 -from collections import deque # 用于平滑FPS计算的双端队列 - -# ---------------------- 替代utils的基础工具函数 ---------------------- -def draw_bounding_boxes(image, boxes, labels, class_names, ids=None): - """ - 在图像上绘制2D边界框(包含类别标签和ID) - 参数: - image: 输入图像(numpy数组) - boxes: 边界框坐标列表,格式为[[x1, y1, x2, y2], ...] - labels: 边界框对应的类别标签索引列表 - class_names: 类别名称列表(如COCO类别) - ids: 可选,边界框对应的对象ID列表(如车辆ID) - 返回: - 绘制了边界框的图像 - """ - img = image.copy() # 复制图像,避免修改原图像 - for i, box in enumerate(boxes): - x1, y1, x2, y2 = map(int, box) # 转换为整数坐标 - # 获取类别名称 - label = class_names[labels[i]] if labels[i] < len(class_names) else 'unknown' - id_text = f'ID: {ids[i]}' if ids is not None else '' # 拼接ID文本 - # 绘制矩形边界框(绿色,线宽2) - cv2.rectangle(img, (x1, y1), (x2, y2), (0, 255, 0), 2) - # 绘制类别和ID文本(绿色,字体大小0.5,线宽2) - cv2.putText(img, f'{label} {id_text}', (x1, y1 - 10), - cv2.FONT_HERSHEY_SIMPLEX, 0.5, (0, 255, 0), 2) - return img - -def build_projection_matrix(w, h, fov, is_behind_camera=False): - """ - 构建相机的投影矩阵,用于将3D世界坐标转换为2D图像坐标 - 参数: - w: 图像宽度 - h: 图像高度 - fov: 相机视场角(度数) - is_behind_camera: 是否为相机后方的点(需要翻转y轴) - 返回: - 3x3的投影矩阵K - """ - # 计算焦距(基于视场角和图像宽度) - focal = w / (2.0 * np.tan(fov * np.pi / 360.0)) - # 初始化投影矩阵(单位矩阵) - K = np.identity(3) - K[0, 0] = K[1, 1] = focal # x和y方向的焦距 - K[0, 2] = w / 2.0 # 图像中心x坐标 - K[1, 2] = h / 2.0 # 图像中心y坐标 - if is_behind_camera: - K[1, 1] = -K[1, 1] # 翻转y轴,处理相机后方的点 - return K - -def get_image_point(world_point, K, world_to_camera): - """ - 将3D世界坐标转换为2D图像坐标 - 参数: - world_point: Carla的Location对象(3D世界坐标) - K: 相机投影矩阵 - world_to_camera: 世界坐标系到相机坐标系的转换矩阵 - 返回: - 2D图像坐标(x, y) - """ - # 将3D世界坐标转换为齐次坐标(4维) - point = np.array([world_point.x, world_point.y, world_point.z, 1]) - # 转换到相机坐标系(4维) - camera_point = np.dot(world_to_camera, point) - # 转换到图像坐标系(3维,归一化) - image_point = np.dot(K, camera_point[:3]) - # 透视除法,得到2D图像坐标 - image_point = image_point / image_point[2] - return np.array([image_point[0], image_point[1]]) - -def point_in_canvas(point, h, w): - """ - 判断2D点是否在图像画布范围内 - 参数: - point: 2D点坐标(x, y) - h: 图像高度 - w: 图像宽度 - 返回: - 布尔值,True表示在画布内,False表示超出范围 - """ - x, y = point - return 0 <= x <= w and 0 <= y <= h - -def clear_npc(world): - """ - 清除仿真世界中所有的NPC车辆 - 参数: - world: Carla的World对象 - """ - for actor in world.get_actors().filter('*vehicle*'): - try: - actor.destroy() # 销毁车辆Actor - except Exception: - pass # 忽略销毁失败的异常 - -def clear_static_vehicle(world): - """ - 清除静态车辆(预留函数,与clear_npc功能重复,暂为空实现) - 参数: - world: Carla的World对象 - """ - pass - -def clear(world, *actors): - """ - 清理指定的Actor并关闭同步模式,最后清除剩余的NPC车辆 - 注意:区分传感器和车辆Actor,传感器需要先stop()再destroy() - 参数: - world: Carla的World对象 - *actors: 要清理的Actor列表(如相机、车辆) - """ - try: - # 关闭同步模式,恢复仿真器默认设置 - settings = world.get_settings() - settings.synchronous_mode = False - world.apply_settings(settings) - except Exception as e: - print(f"警告:关闭同步模式失败 - {e}") - # 遍历并销毁指定的Actor - for actor in actors: - try: - if actor and actor.is_alive: - # 传感器Actor(如相机)需要先停止回调,再销毁 - if 'sensor' in actor.type_id: - actor.stop() - actor.destroy() - except Exception as e: - print(f"警告:销毁Actor失败 - {e}") - # 清除剩余的NPC车辆 - clear_npc(world) - -def get_vehicle_speed(vehicle): - """ - 计算车辆的速度(单位:km/h) - 参数: - vehicle: Carla的Vehicle对象 - 返回: - 车辆的实时速度(km/h) - """ - velocity = vehicle.get_velocity() # 获取车辆的速度矢量(m/s) - # 计算速度的模长(m/s),转换为km/h(乘以3.6) - speed = 3.6 * np.sqrt(velocity.x**2 + velocity.y**2 + velocity.z**2) - return speed - -# ---------------------- 配置参数(集中管理) ---------------------- -class Config: - """ - 配置参数类,集中管理所有硬编码参数,便于修改和维护 - 所有参数均为静态属性,可直接通过Config.XXX访问 - """ - # Carla客户端配置 - CARLA_HOST = 'localhost' # Carla服务器地址 - CARLA_PORT = 2000 # Carla服务器端口 - CARLA_TIMEOUT = 10.0 # 连接超时时间(秒) - # 仿真模式配置 - SYNC_MODE = True # 是否启用同步模式 - FIXED_DELTA = 0.05 # 同步模式的固定时间步长(秒) - # NPC车辆配置 - NPC_COUNT = 50 # 要生成的NPC车辆数量 - MAX_DETECTION_DIST = 50.0 # 车辆检测的最大距离(米) - # 相机配置 - CAMERA_WIDTH = 640 # 相机图像宽度 - CAMERA_HEIGHT = 640 # 相机图像高度 - CAMERA_FOV = 90.0 # 相机视场角(度数) - CAMERA_POS = carla.Transform(carla.Location(x=1, z=2)) # 相机相对于车辆的位置(前1米,高2米) - # FPS统计配置 - FPS_WINDOW_SIZE = 10 # 计算FPS的窗口大小(最近10帧) - # 面板配置 - PANEL_ALPHA = 0.6 # 统计面板的透明度(0-1) - # 天气配置 - WEATHER_SWITCH_INTERVAL = 5.0 # 天气自动切换的时间间隔(秒) - ENABLE_WEATHER_SWITCH = True # 是否启用天气自动切换 - WEATHER_DISPLAY_MAX_LEN = 20 # 天气名称的最大显示长度 - -# ---------------------- 核心功能封装 ---------------------- -class CameraSensor: - """ - 相机传感器类,封装相机的创建、图像采集和销毁逻辑 - 负责与Carla的相机传感器交互,提供图像获取接口 - """ - def __init__(self, world, vehicle, config): - """ - 初始化相机传感器 - 参数: - world: Carla的World对象 - vehicle: 要挂载相机的车辆(Ego车辆) - config: 配置参数对象(Config类实例) - """ - self.world = world # 仿真世界 - self.vehicle = vehicle # 挂载相机的车辆 - self.config = config # 配置参数 - self.bp = self._create_bp() # 相机蓝图 - self.actor = None # 相机Actor对象 - self.queue = queue.Queue(maxsize=1) # 图像队列(最大长度1,避免缓存过多) - self._spawn() # 生成相机Actor - - def _create_bp(self): - """ - 私有方法:创建相机的蓝图(Blueprint) - 返回: - 配置好的相机蓝图对象 - """ - bp_lib = self.world.get_blueprint_library() # 获取蓝图库 - camera_bp = bp_lib.find('sensor.camera.rgb') # 查找RGB相机蓝图 - # 设置相机参数 - camera_bp.set_attribute('image_size_x', str(self.config.CAMERA_WIDTH)) - camera_bp.set_attribute('image_size_y', str(self.config.CAMERA_HEIGHT)) - camera_bp.set_attribute('fov', str(self.config.CAMERA_FOV)) - return camera_bp - - def _spawn(self): - """ - 私有方法:生成相机Actor并挂载到车辆上,设置图像回调 - 抛出: - RuntimeError: 相机生成失败时抛出异常 - """ - try: - # 生成相机Actor,挂载到车辆的指定位置 - self.actor = self.world.spawn_actor(self.bp, self.config.CAMERA_POS, attach_to=self.vehicle) - # 设置图像回调函数,将图像存入队列 - self.actor.listen(lambda img: self._callback(img)) - except carla.exceptions.CarlaError as e: - raise RuntimeError(f"相机生成失败:{e}") - - def _callback(self, image): - """ - 私有方法:相机的图像回调函数,处理原始图像并存入队列 - 参数: - image: Carla的Image对象(原始图像数据) - """ - if not self.queue.full(): - # 将原始图像数据转换为numpy数组(高度×宽度×4(RGBA)) - img_data = np.reshape(np.copy(image.raw_data), (image.height, image.width, 4)) - self.queue.put(img_data) # 存入队列 - - def get_image(self): - """ - 获取相机的最新图像 - 若队列空,返回空的numpy数组(避免程序中断) - 返回: - 图像numpy数组(高度×宽度×4(RGBA)) - """ - try: - return self.queue.get(timeout=0.1) # 超时时间0.1秒 - except queue.Empty: - # 返回空图像(黑色背景) - return np.zeros((self.config.CAMERA_HEIGHT, self.config.CAMERA_WIDTH, 4), dtype=np.uint8) - - def destroy(self): - """ - 销毁相机Actor,释放资源 - """ - if self.actor: - self.actor.stop() # 停止图像回调 - self.actor.destroy() # 销毁Actor - -class Vehicle3dBoxRenderer: - """ - 车辆3D边界框渲染类,封装3D边界框的计算和绘制逻辑 - 负责将仿真世界中的车辆3D边界框转换为2D图像上的线框并绘制 - """ - def __init__(self, world, ego_vehicle, camera, config): - """ - 初始化3D边界框渲染器 - 参数: - world: Carla的World对象 - ego_vehicle: Ego车辆对象 - camera: 相机传感器对象(CameraSensor类实例) - config: 配置参数对象(Config类实例) - """ - self.world = world # 仿真世界 - self.ego_vehicle = ego_vehicle # Ego车辆 - self.camera = camera # 相机传感器 - self.config = config # 配置参数 - # 3D边界框的边缘对(定义立方体的12条边) - self.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]] - # 构建相机投影矩阵(前向和后向) - self.K = build_projection_matrix( - config.CAMERA_WIDTH, config.CAMERA_HEIGHT, config.CAMERA_FOV - ) - self.K_b = build_projection_matrix( - config.CAMERA_WIDTH, config.CAMERA_HEIGHT, config.CAMERA_FOV, is_behind_camera=True - ) - - def render(self, image): - """ - 在图像上绘制所有符合条件的车辆3D边界框线框 - 参数: - image: 输入图像(numpy数组,BGR格式) - 返回: - 绘制了3D边界框的图像、总车辆数、已渲染的车辆数 - """ - img_h, img_w = self.config.CAMERA_HEIGHT, self.config.CAMERA_WIDTH - # 获取世界坐标系到相机坐标系的转换矩阵(逆矩阵) - world_2_camera = np.array(self.camera.actor.get_transform().get_inverse_matrix()) - ego_transform = self.ego_vehicle.get_transform() # Ego车辆的变换 - ego_forward = ego_transform.get_forward_vector() # Ego车辆的前向矢量 - camera_transform = self.camera.actor.get_transform() # 相机的变换 - cam_forward = camera_transform.get_forward_vector() # 相机的前向矢量 - - total_vehicles = 0 # 总车辆数 - rendered_vehicles = 0 # 已渲染的车辆数 - - # 遍历仿真世界中的所有车辆 - for npc in self.world.get_actors().filter('*vehicle*'): - total_vehicles += 1 - # 过滤掉Ego车辆自身 - if npc.id == self.ego_vehicle.id: - continue - - # 过滤超出检测距离的车辆 - dist = npc.get_transform().location.distance(ego_transform.location) - if dist > self.config.MAX_DETECTION_DIST: - continue - - # 过滤Ego车辆后方的车辆(只处理前方的车辆) - ray = npc.get_transform().location - ego_transform.location - if ego_forward.dot(ray) <= 0: - continue - - rendered_vehicles += 1 - bb = npc.bounding_box # 车辆的3D边界框 - # 获取边界框的8个顶点在世界坐标系中的坐标 - verts = [v for v in bb.get_world_vertices(npc.get_transform())] - points_2d = [] # 存储顶点的2D图像坐标 - - # 遍历每个顶点,转换为2D图像坐标 - for vert in verts: - ray0 = vert - camera_transform.location - # 判断顶点是否在相机前方 - if cam_forward.dot(ray0) > 0: - p = get_image_point(vert, self.K, world_2_camera) - else: - p = get_image_point(vert, self.K_b, world_2_camera) - points_2d.append(p) - - # 绘制3D边界框的12条边 - for edge in self.edges: - p1 = points_2d[edge[0]] - p2 = points_2d[edge[1]] - # 判断边的两个顶点是否至少有一个在画布内 - p1_in = point_in_canvas(p1, img_h, img_w) - p2_in = point_in_canvas(p2, img_h, img_w) - if p1_in or p2_in: - # 绘制线框(蓝色,线宽1) - cv2.line( - image, - (int(p1[0]), int(p1[1])), - (int(p2[0]), int(p2[1])), - (255, 0, 0, 255), - 1 - ) - - return image, total_vehicles, rendered_vehicles - -# ---------------------- 天气管理类 ---------------------- -class WeatherManager: - """ - 天气管理类,封装天气的初始化、自动切换和手动切换逻辑 - 支持Carla预设天气和自定义雾天天气 - """ - def __init__(self, world, config): - """ - 初始化天气管理器 - 参数: - world: Carla的World对象 - config: 配置参数对象(Config类实例) - """ - self.world = world # 仿真世界 - self.config = config # 配置参数 - self.current_weather_name = "" # 当前天气名称 - # 天气列表:元组(天气参数对象,天气名称) - self.weather_list = [ - (carla.WeatherParameters.ClearNoon, "ClearNoon"), # 晴朗中午 - (carla.WeatherParameters.CloudyNoon, "CloudyNoon"), # 多云中午 - (carla.WeatherParameters.WetNoon, "WetNoon"), # 潮湿中午 - (carla.WeatherParameters.MidRainyNoon, "MidRainyNoon"), # 中雨中午 - (carla.WeatherParameters.HardRainNoon, "HardRainNoon"), # 大雨中午 - (carla.WeatherParameters.ClearSunset, "ClearSunset"), # 晴朗日落 - (carla.WeatherParameters.CloudySunset, "CloudySunset"), # 多云日落 - (carla.WeatherParameters.WetSunset, "WetSunset"), # 潮湿日落 - (self._create_foggy_weather(), "FoggyNoon") # 自定义雾天 - ] - self.current_index = 0 # 当前天气的索引 - self.last_switch_time = time.perf_counter() # 上一次天气切换的时间 - self._apply_weather(self.current_index) # 应用初始天气 - - def _create_foggy_weather(self): - """ - 私有方法:创建自定义雾天天气参数 - 返回: - 配置好的雾天天气参数对象 - """ - weather = carla.WeatherParameters() # 初始化天气参数 - weather.fog_density = 0.7 # 雾的浓度(0-1) - weather.fog_distance = 10.0 # 雾的起始距离(米) - weather.cloudiness = 0.8 # 云量(0-1) - weather.precipitation = 0.0 # 降水量(0-1,关闭降水) - return weather - - def _apply_weather(self, index): - """ - 私有方法:应用指定索引的天气 - 参数: - index: 天气列表的索引 - """ - self.current_index = index % len(self.weather_list) # 循环索引 - weather_params, weather_name = self.weather_list[self.current_index] - self.world.set_weather(weather_params) # 设置天气 - self.world.tick() # 触发仿真步,使天气生效 - self.current_weather_name = weather_name # 更新当前天气名称 - print(f"\n=== 天气已切换为:{weather_name} ===") # 打印天气切换信息 - - def update(self): - """ - 更新天气状态(自动切换) - 返回: - 当前天气名称 - """ - if not self.config.ENABLE_WEATHER_SWITCH: - return self.current_weather_name - current_time = time.perf_counter() - # 判断是否达到切换时间间隔 - if current_time - self.last_switch_time >= self.config.WEATHER_SWITCH_INTERVAL: - self.current_index += 1 # 切换到下一个天气 - self._apply_weather(self.current_index) - self.last_switch_time = current_time # 更新切换时间 - return self.current_weather_name - - def switch_manually(self): - """ - 手动切换天气(用于键盘触发) - 返回: - 切换后的天气名称 - """ - self.current_index += 1 # 切换到下一个天气 - self._apply_weather(self.current_index) - self.last_switch_time = time.perf_counter() # 更新切换时间 - return self.current_weather_name - -# ---------------------- 主函数 ---------------------- -def main(): - """ - 程序主函数,负责整体流程的调度: - 1. 初始化Carla客户端和仿真世界 - 2. 生成Ego车辆和NPC车辆 - 3. 初始化相机、3D渲染器、天气管理器 - 4. 主循环:处理图像、渲染3D框、更新天气、显示统计信息 - 5. 资源清理 - """ - config = Config() # 初始化配置参数 - - # 1. 初始化Carla客户端 - try: - client = carla.Client(config.CARLA_HOST, config.CARLA_PORT) - client.set_timeout(config.CARLA_TIMEOUT) - world = client.get_world() - except carla.exceptions.CarlaError as e: - print(f"Carla连接失败:{e}") - return - - # 2. 设置同步模式 - try: - settings = world.get_settings() - settings.synchronous_mode = config.SYNC_MODE - settings.fixed_delta_seconds = config.FIXED_DELTA - world.apply_settings(settings) - except carla.exceptions.CarlaError as e: - print(f"设置同步模式失败:{e}") - return - - # 3. 获取基础组件 - spectator = world.get_spectator() # 旁观者相机(用于视角控制) - spawn_points = world.get_map().get_spawn_points() # 车辆生成点列表 - if not spawn_points: - print("没有找到生成点") - return - - # 4. 生成Ego车辆(遍历生成点,确保生成成功) - bp_lib = world.get_blueprint_library() - vehicle_bp = bp_lib.find('vehicle.lincoln.mkz_2020') # 选择林肯MKZ车辆蓝图 - vehicle_bp.set_attribute('role_name', 'ego') # 标记为Ego车辆 - vehicle = None - for spawn_point in spawn_points: - vehicle = world.try_spawn_actor(vehicle_bp, spawn_point) - if vehicle: - break - if not vehicle: - print("Ego车辆生成失败") - return - - # 5. 清除现有NPC车辆 - clear_npc(world) - clear_static_vehicle(world) - - # 6. 生成NPC车辆(按顺序选择生成点,避免重叠;随机设置颜色) - car_bp = [bp for bp in bp_lib.filter('vehicle') if int(bp.get_attribute('number_of_wheels')) == 4] - if not car_bp: - print("没有找到四轮车辆蓝图") - return - spawn_point_idx = 0 # 生成点索引 - npc_count = 0 # 已生成的NPC数量 - # 循环生成,直到达到指定数量或遍历完生成点两次 - while npc_count < config.NPC_COUNT and spawn_point_idx < len(spawn_points) * 2: - spawn_point = spawn_points[spawn_point_idx % len(spawn_points)] # 循环使用生成点 - npc_bp = random.choice(car_bp) # 随机选择车辆蓝图 - # 随机设置车辆颜色(如果蓝图支持颜色属性) - if npc_bp.has_attribute('color'): - color = random.choice(npc_bp.get_attribute('color').recommended_values) - npc_bp.set_attribute('color', color) - # 尝试生成NPC车辆 - npc = world.try_spawn_actor(npc_bp, spawn_point) - if npc: - npc.set_autopilot(True) # 启用自动驾驶 - npc_count += 1 - spawn_point_idx += 1 - print(f"成功生成 {npc_count} 个NPC车辆") - time.sleep(0.5) # 短暂延迟,确保车辆稳定 - vehicle.set_autopilot(True) # Ego车辆启用自动驾驶 - - # 7. 初始化核心组件 - try: - camera = CameraSensor(world, vehicle, config) # 相机传感器 - renderer = Vehicle3dBoxRenderer(world, vehicle, camera, config) # 3D边界框渲染器 - weather_manager = WeatherManager(world, config) # 天气管理器 - except RuntimeError as e: - print(e) - clear(world, vehicle) - return - - # 8. 初始化FPS统计(双端队列存储最近的帧时间) - frame_times = deque(maxlen=config.FPS_WINDOW_SIZE) - last_frame_time = time.perf_counter() - - # 创建显示窗口(提前创建,避免窗口闪烁) - cv2.namedWindow('3D Ground Truth + Weather + Stats', cv2.WINDOW_NORMAL) - - # 主循环 - try: - while True: - if config.SYNC_MODE: - world.tick() # 触发仿真步(同步模式必须调用) - current_time = time.perf_counter() - - # 移动旁观者相机到Ego车辆上方(鸟瞰视角) - transform = carla.Transform( - vehicle.get_transform().transform(carla.Location(x=-4, z=50)), - carla.Rotation(yaw=-180, pitch=-90) - ) - spectator.set_transform(transform) - - # 获取相机图像(RGBA格式) - image = camera.get_image() - - # 图像处理:将RGBA转换为BGR格式(OpenCV默认) - image_bgr = cv2.cvtColor(image[:, :, :3], cv2.COLOR_RGBA2BGR) - - # 绘制3D边界框,获取统计数据 - image_bgr, total_veh, rendered_veh = renderer.render(image_bgr) - - # 更新天气状态(自动切换) - current_weather = weather_manager.update() - - # 计算FPS和车辆速度 - frame_time = current_time - last_frame_time - frame_times.append(frame_time) - fps = 1.0 / np.mean(frame_times) if frame_times else 0.0 # 平均FPS - last_frame_time = current_time - ego_speed = get_vehicle_speed(vehicle) # Ego车辆速度 - - # ---------------------- 绘制实时统计面板 ---------------------- - # 1. 绘制半透明黑色背景面板 - panel_x, panel_y = 10, 10 # 面板左上角坐标 - panel_w, panel_h = 420, 220 # 面板宽度和高度 - overlay = image_bgr.copy() # 复制图像用于绘制背景 - cv2.rectangle(overlay, (panel_x, panel_y), (panel_x + panel_w, panel_y + panel_h), (0, 0, 0), -1) - # 混合背景和原图像,实现半透明效果 - cv2.addWeighted(overlay, config.PANEL_ALPHA, image_bgr, 1 - config.PANEL_ALPHA, 0, image_bgr) - - # 2. 绘制统计文本(白色字体,清晰可见) - text_y_step = 30 # 文本行间距 - text_pos_y = panel_y + text_y_step # 初始文本y坐标 - text_style = (cv2.FONT_HERSHEY_SIMPLEX, 0.6, (255, 255, 255), 2) # 字体样式 - # 优先显示天气信息(避免被遮挡) - cv2.putText(image_bgr, f"Current Weather: {current_weather}", (panel_x + 15, text_pos_y), *text_style) - text_pos_y += text_y_step - # 依次显示其他统计信息 - cv2.putText(image_bgr, f"FPS: {fps:.1f}", (panel_x + 15, text_pos_y), *text_style) - text_pos_y += text_y_step - cv2.putText(image_bgr, f"Ego Speed: {ego_speed:.1f} km/h", (panel_x + 15, text_pos_y), *text_style) - text_pos_y += text_y_step - cv2.putText(image_bgr, f"Total Vehicles: {total_veh}", (panel_x + 15, text_pos_y), *text_style) - text_pos_y += text_y_step - cv2.putText(image_bgr, f"Rendered 3D Boxes: {rendered_veh}", (panel_x + 15, text_pos_y), *text_style) - text_pos_y += text_y_step - cv2.putText(image_bgr, f"Press 'q' to quit | 'w' to switch weather", (panel_x + 15, text_pos_y), *text_style) - - # 显示图像(强制刷新窗口) - cv2.imshow('3D Ground Truth + Weather + Stats', image_bgr) - - # 键盘事件处理 - key = cv2.waitKey(1) & 0xFF - if key == ord('q'): - break # 按q退出循环 - elif key == ord('w'): - # 按w手动切换天气,并刷新显示 - current_weather = weather_manager.switch_manually() - cv2.imshow('3D Ground Truth + Weather + Stats', image_bgr) - - except KeyboardInterrupt: - print("\n用户中断程序") # 捕获键盘中断(Ctrl+C) - except Exception as e: - print(f"程序异常:{e}") # 捕获其他异常 - finally: - # 资源清理:销毁相机、车辆,关闭窗口 - camera.destroy() - clear(world, vehicle) - cv2.destroyAllWindows() - print("资源已清理,程序结束") - -# 程序入口 -if __name__ == '__main__': - main() - 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 9231deab73..0000000000 --- a/src/car_tracking/car_2d_ground_truth.py +++ /dev/null @@ -1,362 +0,0 @@ -import carla -import queue -import random -import cv2 -import numpy as np - -# ===================== 依赖导入(保留原始依赖)===================== -from what.models.detection.datasets.coco import COCO_CLASS_NAMES -from utils.box_utils import draw_bounding_boxes -from utils.projection import * -from utils.world import * - -# ===================== 配置常量(集中管理,便于修改)===================== -# 相机配置 -CAMERA_WIDTH = 640 -CAMERA_HEIGHT = 640 -# 图表配置 -CHART_WIDTH = 400 -CHART_HEIGHT = CAMERA_HEIGHT # 与相机高度一致 -MAX_HISTORY_FRAMES = 50 # 最近50帧数据 -# 跟踪窗口配置 -TRACK_WINDOW_WIDTH = 300 -TRACK_WINDOW_HEIGHT = 400 -# 绘图配置 -FONT_SCALE_SMALL = 0.4 -FONT_SCALE_MEDIUM = 0.6 -LINE_THICKNESS = 2 -POINT_RADIUS = 2 - - -# ===================== 工具函数(独立封装,提升复用性)===================== -def get_vehicle_color(vehicle_id): - """为车辆生成固定唯一的RGB颜色(基于ID种子,保证跟踪时颜色不变)""" - np.random.seed(vehicle_id) - return tuple(np.random.randint(0, 255, 3).tolist()) - - -def custom_draw_bounding_boxes(image, boxes, labels, class_names, ids=None, track_data=None): - """保留原始边界框绘制逻辑,叠加跟踪数据(距离+颜色外框)""" - # 调用原始画框函数,保证核心功能不变 - img = draw_bounding_boxes(image, boxes, labels, class_names, ids) - - # 叠加跟踪数据标注(不破坏原始框) - if ids is not None and track_data is not None and len(boxes) > 0: - for i, box in enumerate(boxes): - vid = ids[i] - if vid in track_data: - x1, y1, x2, y2 = map(int, box) - color = track_data[vid]['color'] - dist = track_data[vid]['distance'] - # 绘制距离文本(在原始标注下方) - cv2.putText( - img, f"Dist: {dist:.1f}m", (x1, y1 + 10), - cv2.FONT_HERSHEY_SIMPLEX, FONT_SCALE_SMALL, color, 1 - ) - # 绘制跟踪颜色外框(不覆盖原始绿色框) - cv2.rectangle(img, (x1 - 1, y1 - 1), (x2 + 1, y2 + 1), color, 1) - return img - - -def init_chart_background(width, height): - """初始化图表背景(绘制固定元素:标题、网格、图例,避免每帧重复绘制)""" - chart = np.zeros((height, width, 3), dtype=np.uint8) - # 绘制标题 - cv2.putText( - chart, "Real-Time Statistics (Last 50 Frames)", (10, 30), - cv2.FONT_HERSHEY_SIMPLEX, FONT_SCALE_MEDIUM, (255, 255, 255), LINE_THICKNESS - ) - # 绘制网格(浅灰色,提升视觉效果) - grid_color = (50, 50, 50) - # 水平网格线 - for y in range(50, height - 30, 50): - cv2.line(chart, (50, y), (width - 50, y), grid_color, 1) - # 垂直网格线 - for x in range(50, width - 50, 50): - cv2.line(chart, (x, 30), (x, height - 30), grid_color, 1) - # 绘制图例(固定位置,提升可读性) - cv2.putText( - chart, "Current Vehicles (green)", (10, height - 10), - cv2.FONT_HERSHEY_SIMPLEX, FONT_SCALE_SMALL, (0, 255, 0), 1 - ) - cv2.putText( - chart, "Max Distance (red)", (200, height - 10), - cv2.FONT_HERSHEY_SIMPLEX, FONT_SCALE_SMALL, (0, 0, 255), 1 - ) - return chart - - -def draw_dynamic_chart(history_frames, history_vehicles, history_max_dist): - """ - 绘制实时动态折线图(仅绘制变化的折线和数据点,复用固定背景) - 参数: - history_frames: 最近50帧的帧数列表 - history_vehicles: 最近50帧的车辆数量列表 - history_max_dist: 最近50帧的最大距离列表 - 返回: - 绘制完成的图表图像 - """ - # 初始化图表背景(固定元素) - chart = init_chart_background(CHART_WIDTH, CHART_HEIGHT) - - # 数据为空时直接返回背景 - if len(history_frames) == 0: - return chart - - # ========== 数据归一化(优化计算逻辑,避免除零错误)========== - # 车辆数量归一化(映射到图表y轴范围:30 ~ CHART_HEIGHT-30) - max_veh = max(history_vehicles) if history_vehicles else 1 - max_veh = max_veh if max_veh != 0 else 1 # 处理除零 - norm_veh = [(v / max_veh) * (CHART_HEIGHT - 60) for v in history_vehicles] - y_veh = np.array([CHART_HEIGHT - 30 - v for v in norm_veh], dtype=int) - - # 最大距离归一化 - max_d = max(history_max_dist) if history_max_dist else 1 - max_d = max_d if max_d != 0 else 1 # 处理除零 - norm_dist = [(d / max_d) * (CHART_HEIGHT - 60) for d in history_max_dist] - y_dist = np.array([CHART_HEIGHT - 30 - d for d in norm_dist], dtype=int) - - # x轴归一化(动态滚动,仅显示最近50帧的x坐标) - x_coords = np.array([ - 50 + (i * (CHART_WIDTH - 100) / (len(history_frames) - 1 if len(history_frames) > 1 else 1)) - for i in range(len(history_frames)) - ], dtype=int) - - # ========== 绘制折线和数据点(优化绘制逻辑,提升流畅度)========== - # 绘制车辆数量折线(绿色) - if len(x_coords) > 1: - cv2.polylines(chart, [np.column_stack((x_coords, y_veh))], isClosed=False, color=(0, 255, 0), - thickness=LINE_THICKNESS) - # 绘制车辆数量数据点 - for x, y in zip(x_coords, y_veh): - cv2.circle(chart, (x, y), POINT_RADIUS, (0, 255, 0), -1) - - # 绘制最大距离折线(红色) - if len(x_coords) > 1: - cv2.polylines(chart, [np.column_stack((x_coords, y_dist))], isClosed=False, color=(0, 0, 255), - thickness=LINE_THICKNESS) - # 绘制最大距离数据点 - for x, y in zip(x_coords, y_dist): - cv2.circle(chart, (x, y), POINT_RADIUS, (0, 0, 255), -1) - - # ========== 绘制当前数值标注(实时更新)========== - if len(history_vehicles) > 0 and len(history_max_dist) > 0: - current_veh = history_vehicles[-1] - current_dist = history_max_dist[-1] - cv2.putText( - chart, f"Now: {current_veh} cars | {current_dist:.1f}m", - (CHART_WIDTH - 200, 30), cv2.FONT_HERSHEY_SIMPLEX, FONT_SCALE_SMALL, - (255, 255, 255), 1 - ) - - return chart - - -def camera_callback(image, rgb_image_queue): - """相机回调函数(保留原始逻辑,简化代码)""" - rgb_image_queue.put(np.reshape(np.copy(image.raw_data), (image.height, image.width, 4))) - - -def convert_image_format(image): - """将4通道BGRA图像转换为3通道RGB图像(提取前3通道,简化逻辑)""" - return image[..., :3] if image.shape[-1] == 4 else image.copy() - - -# ===================== 主程序逻辑(优化结构,提升可读性)===================== -def main(): - # 初始化Carla客户端和世界 - client = carla.Client('localhost', 2000) - world = client.get_world() - - # 设置同步模式(保留原始逻辑) - settings = world.get_settings() - settings.synchronous_mode = True - settings.fixed_delta_seconds = 0.05 - world.apply_settings(settings) - - # 获取观众和生成点(保留原始逻辑) - spectator = world.get_spectator() - spawn_points = world.get_map().get_spawn_points() - - # 生成主车辆(保留原始逻辑,增加异常处理) - bp_lib = world.get_blueprint_library() - vehicle_bp = bp_lib.find('vehicle.lincoln.mkz_2020') - vehicle = world.try_spawn_actor(vehicle_bp, random.choice(spawn_points)) - if not vehicle: - print("警告:主车辆生成失败,程序退出!") - return - - # 生成相机(使用配置常量,简化代码) - 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_init_trans = carla.Transform(carla.Location(x=1, z=2)) - camera = world.spawn_actor(camera_bp, camera_init_trans, attach_to=vehicle) - - # 初始化图像队列(保留原始逻辑) - image_queue = queue.Queue() - camera.listen(lambda image: camera_callback(image, image_queue)) - - # 清理现有NPC(保留原始逻辑) - clear_npc(world) - clear_static_vehicle(world) - - # 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]] - fov = camera_bp.get_attribute("fov").as_float() - K = build_projection_matrix(CAMERA_WIDTH, CAMERA_HEIGHT, fov) - K_b = build_projection_matrix(CAMERA_WIDTH, CAMERA_HEIGHT, fov, is_behind_camera=True) - - # 生成NPC车辆(保留原始逻辑,使用配置常量) - for _ in range(50): - vehicle_bp_list = bp_lib.filter('vehicle') - car_bp = [bp for bp in vehicle_bp_list if int(bp.get_attribute('number_of_wheels')) == 4] - if car_bp: - npc = world.try_spawn_actor(random.choice(car_bp), random.choice(spawn_points)) - if npc: - npc.set_autopilot(True) - vehicle.set_autopilot(True) - - # 初始化跟踪和数据缓存变量(改为局部变量,减少全局变量) - tracked_vehicles = {} # key:车辆ID, value:{'color':颜色, 'distance':距离, 'frame':帧数} - frame_counter = 0 - history_frames = [] # 最近50帧的帧数 - history_vehicles = [] # 最近50帧的车辆数量 - history_max_dist = [] # 最近50帧的最大距离 - - # 主循环(优化逻辑,提升可读性) - try: - while True: - world.tick() - frame_counter += 1 - - # 移动观众视角到车辆顶部(保留原始逻辑) - transform = carla.Transform( - vehicle.get_transform().transform(carla.Location(x=-4, z=50)), - carla.Rotation(yaw=-180, pitch=-90) - ) - spectator.set_transform(transform) - - # 获取相机图像(保留原始逻辑) - image = image_queue.get() - - # 更新相机矩阵(保留原始逻辑) - world_2_camera = np.array(camera.get_transform().get_inverse_matrix()) - - # 初始化当前帧变量 - boxes = [] - ids = [] - track_data = {} - current_vehicles = 0 - max_distance = 0.0 - - # 检测车辆并计算2D边界框(保留原始核心逻辑) - for npc in world.get_actors().filter('*vehicle*'): - if npc.id != vehicle.id: - # 计算车辆距离和最大距离 - dist = npc.get_transform().location.distance(vehicle.get_transform().location) - max_distance = max(max_distance, dist) - - # 过滤50米内、车辆前方的目标 - if dist < 50: - forward_vec = vehicle.get_transform().get_forward_vector() - ray = npc.get_transform().location - vehicle.get_transform().location - if forward_vec.dot(ray) > 0: - # 计算3D顶点的2D投影 - verts = [v for v in npc.bounding_box.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, CAMERA_HEIGHT, CAMERA_WIDTH - ) - - # 过滤小框和超出画布的框 - if (y_max - y_min) * (x_max - x_min) > 100 and (x_max - x_min) > 20: - if point_in_canvas((x_min, y_min), CAMERA_HEIGHT, CAMERA_WIDTH) and \ - point_in_canvas((x_max, y_max), CAMERA_HEIGHT, CAMERA_WIDTH): - ids.append(npc.id) - boxes.append(np.array([x_min, y_min, x_max, y_max])) - # 更新跟踪数据 - if npc.id not in tracked_vehicles: - tracked_vehicles[npc.id] = {'color': get_vehicle_color(npc.id)} - tracked_vehicles[npc.id]['distance'] = dist - tracked_vehicles[npc.id]['frame'] = frame_counter - track_data[npc.id] = tracked_vehicles[npc.id] - current_vehicles += 1 - - # 更新历史数据缓存(仅保留最近50帧,优化逻辑) - history_frames.append(frame_counter) - history_vehicles.append(current_vehicles) - history_max_dist.append(max_distance) - # 截断数据,保持固定长度 - if len(history_frames) > MAX_HISTORY_FRAMES: - history_frames.pop(0) - history_vehicles.pop(0) - history_max_dist.pop(0) - - # 绘制边界框(保留原始逻辑,使用自定义函数) - boxes = np.array(boxes) - labels = np.array([2] * len(boxes)) - probs = np.array([1.0] * len(boxes)) - output = custom_draw_bounding_boxes( - image, boxes, labels, COCO_CLASS_NAMES, ids, track_data - ) if len(boxes) > 0 else image - - # 转换图像格式并拼接图表(优化逻辑) - output_rgb = convert_image_format(output) - chart_image = draw_dynamic_chart(history_frames, history_vehicles, history_max_dist) - combined_image = np.hstack((output_rgb, chart_image)) - - # 绘制跟踪监测窗口(保留原始功能,优化代码) - track_window = np.zeros((TRACK_WINDOW_HEIGHT, TRACK_WINDOW_WIDTH, 3), dtype=np.uint8) - cv2.putText( - track_window, "Vehicle Tracking Monitor", (10, 30), - cv2.FONT_HERSHEY_SIMPLEX, FONT_SCALE_MEDIUM, (255, 255, 255), LINE_THICKNESS - ) - y_offset = 60 - # 显示前10辆跟踪的车辆 - for vid, data in list(tracked_vehicles.items())[:10]: - if y_offset > TRACK_WINDOW_HEIGHT - 20: - break - color = data['color'] - dist = data.get('distance', 0.0) - cv2.putText( - track_window, f"ID: {vid} | Dist: {dist:.1f}m", (10, y_offset), - cv2.FONT_HERSHEY_SIMPLEX, FONT_SCALE_SMALL, color, 1 - ) - y_offset += 30 - - # 显示窗口(保留原始逻辑) - cv2.imshow('2D Ground Truth', combined_image) - cv2.imshow('Vehicle Tracking Monitor', track_window) - - # 按q退出(保留原始逻辑) - if cv2.waitKey(1) == ord('q'): - break - except KeyboardInterrupt: - print("程序被用户中断!") - finally: - # 清理资源(优化异常处理,确保资源释放) - try: - camera.destroy() - clear(world, camera) - except Exception as e: - print(f"清理资源时警告:{e}") - cv2.destroyAllWindows() - # 恢复Carla世界设置(新增,避免同步模式残留) - settings.synchronous_mode = False - world.apply_settings(settings) - - -if __name__ == '__main__': - main() \ No newline at end of file diff --git a/src/car_tracking/caryolo.py b/src/car_tracking/caryolo.py deleted file mode 100644 index b5f1e17e57..0000000000 --- a/src/car_tracking/caryolo.py +++ /dev/null @@ -1,636 +0,0 @@ -import os -import sys -import traceback -import queue -import random -import time -import numpy as np -import cv2 -import carla -from collections import deque - -# ====================== 关键:添加src/2d-carla-tracking-master到Python搜索路径 ====================== -CARLA_TRACKING_ROOT = r"D:\nn\src\2d-carla-tracking-master" -sys.path.append(CARLA_TRACKING_ROOT) - -# ====================== 导入所需模块 ====================== -try: - from utils.projection import build_projection_matrix, get_image_point, point_in_canvas - from utils.world import clear_npc, clear_static_vehicle, clear -except ImportError as e: - print(f"导入模块失败:{e}") - sys.exit(1) - -# ====================== 全局配置变量 ====================== -# Carla连接配置 -CARLA_HOST = 'localhost' -CARLA_PORT = 2000 -CARLA_TIMEOUT = 10.0 -SYNC_DELTA_SECONDS = 0.05 # 20FPS - -# 摄像头配置 - 减小视图尺寸 -CAMERA_WIDTH = 960 # 减小分辨率 -CAMERA_HEIGHT = 540 -CAMERA_FOV = 90 -CAMERA_POSITION = carla.Transform(carla.Location(x=1, z=2)) - -# NPC车辆配置 -NPC_VEHICLE_NUM = 25 - -# 3D边界框配置 -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]] -DISTANCE_THRESHOLD = 80 # 减小显示距离 - -# 红绿灯配置 -SHOW_TRAFFIC_LIGHTS = True # 是否显示红绿灯 -TRAFFIC_LIGHT_DISTANCE = 60 # 减小红绿灯显示距离 - -# 显示控制 -SHOW_INFO_PANEL = True # 是否显示信息面板 -SHOW_VEHICLES = True # 是否显示车辆 -SHOW_TRAFFIC_LIGHTS_STATE = True # 是否显示红绿灯状态文字 - -# ====================== 颜色定义 ====================== -# 车辆边界框颜色 -VEHICLE_COLOR = (0, 255, 0) # 绿色(BGR格式) - -# 红绿灯状态颜色(BGR格式) -TRAFFIC_LIGHT_COLORS = { - 0: (0, 255, 0), # 绿色 - 1: (0, 255, 255), # 黄色 - 2: (0, 0, 255), # 红色 - 3: (255, 255, 255) # 白色 -} - -TRAFFIC_LIGHT_STATE_NAMES = { - 0: "GREEN", - 1: "YELLOW", - 2: "RED", - 3: "UNKNOWN" -} - -# 信息面板颜色 -PANEL_BG_COLOR = (40, 40, 40) # 深灰色背景 -PANEL_BORDER_COLOR = (0, 200, 0) # 浅绿色边框 -TEXT_COLOR = (240, 240, 240) # 浅灰色文字 -HIGHLIGHT_COLOR = (0, 255, 255) # 黄色高亮文字 - - -# ====================== 性能监控器 ====================== -class PerformanceMonitor: - def __init__(self, window_size=30): - self.window_size = window_size - self.timestamps = deque(maxlen=window_size) - self.frame_times = deque(maxlen=window_size) - - def start_frame(self): - self.frame_start = time.time() - - def end_frame(self): - current_time = time.time() - frame_time = (current_time - self.frame_start) * 1000 - - self.timestamps.append(current_time) - self.frame_times.append(frame_time) - - if len(self.timestamps) > 1: - fps = len(self.timestamps) / (self.timestamps[-1] - self.timestamps[0]) - avg_frame_time = np.mean(self.frame_times) if self.frame_times else 0 - return fps, avg_frame_time, frame_time - return 0, 0, 0 - - -# ====================== Carla环境初始化 ====================== -def init_carla_environment(): - """初始化Carla环境""" - try: - client = carla.Client(CARLA_HOST, CARLA_PORT) - client.set_timeout(CARLA_TIMEOUT) - - # 连接 - world = client.get_world() - print("✅ Carla模拟器连接成功!") - - # 设置同步模式 - settings = world.get_settings() - settings.synchronous_mode = True - settings.fixed_delta_seconds = SYNC_DELTA_SECONDS - world.apply_settings(settings) - print(f"✅ 同步模式已启用,帧率: {1 / SYNC_DELTA_SECONDS:.1f} FPS") - - # 获取蓝图和生成点 - bp_lib = world.get_blueprint_library() - spawn_points = world.get_map().get_spawn_points() - - if not spawn_points: - print("⚠️ 警告:未找到生成点") - spawn_points = [carla.Transform()] - - # 生成主车辆 - print("🚗 生成主车辆...") - vehicle_bp = bp_lib.find('vehicle.lincoln.mkz_2020') - vehicle = None - for _ in range(10): - vehicle = world.try_spawn_actor(vehicle_bp, random.choice(spawn_points)) - if vehicle: - break - - if not vehicle: - print("⚠️ 警告:使用默认车辆") - vehicle_bp = random.choice(list(bp_lib.filter('vehicle.*'))) - vehicle = world.spawn_actor(vehicle_bp, random.choice(spawn_points)) - - print("✅ 主车辆生成成功") - - # 生成摄像头 - print("📷 生成摄像头...") - 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(f"✅ 摄像头生成成功: {CAMERA_WIDTH}x{CAMERA_HEIGHT}") - - # 图像队列 - image_queue = queue.Queue(maxsize=1) - - def camera_callback(image): - try: - img = np.reshape(np.copy(image.raw_data), - (image.height, image.width, 4)) - img = img[:, :, :3].astype(np.uint8) - - # 如果尺寸不匹配,调整尺寸 - if CAMERA_WIDTH != image.width or CAMERA_HEIGHT != image.height: - img = cv2.resize(img, (CAMERA_WIDTH, CAMERA_HEIGHT)) - - if not image_queue.full(): - image_queue.put_nowait(img) - except Exception as e: - pass - - camera.listen(camera_callback) - - # 清理现有NPC - print("🧹 清理现有NPC...") - try: - clear_npc(world) - clear_static_vehicle(world) - print("✅ 现有NPC已清理") - except: - print("⚠️ 清理NPC失败(可能已清理)") - - # 生成NPC车辆 - print(f"🚗 生成 {NPC_VEHICLE_NUM} 辆NPC车辆...") - car_bps = [] - - # 获取四轮车辆蓝图 - for bp in bp_lib.filter('vehicle.*'): - try: - wheels = bp.get_attribute('number_of_wheels') - if wheels and int(wheels.as_int()) == 4: - car_bps.append(bp) - except: - car_bps.append(bp) - - if not car_bps: - car_bps = list(bp_lib.filter('vehicle.*')) - - spawned = 0 - max_attempts = min(NPC_VEHICLE_NUM * 3, len(spawn_points) * 2) - - for attempt in range(max_attempts): - if spawned >= NPC_VEHICLE_NUM: - break - - bp = random.choice(car_bps) - spawn_point = random.choice(spawn_points) - npc = world.try_spawn_actor(bp, spawn_point) - - if npc: - try: - npc.set_autopilot(True) - spawned += 1 - if spawned % 10 == 0: - print(f" 已生成 {spawned} 辆NPC车辆") - except: - npc.destroy() - - print(f"✅ 成功生成 {spawned} 辆NPC车辆") - vehicle.set_autopilot(True) - print("✅ 主车辆自动驾驶已开启") - - return world, client, vehicle, camera, image_queue - - except Exception as e: - print(f"❌ Carla初始化失败: {e}") - traceback.print_exc() - raise - - -# ====================== 3D边界框和红绿灯绘制函数 ====================== -def draw_3d_bounding_boxes(image, world, camera, vehicle): - """在图像上绘制3D真值边界框和红绿灯""" - try: - img = image.copy() - height, width = img.shape[:2] - - # 构建投影矩阵 - world_2_camera = np.array(camera.get_transform().get_inverse_matrix()) - K = build_projection_matrix(width, height, CAMERA_FOV) - K_b = build_projection_matrix(width, height, CAMERA_FOV, is_behind_camera=True) - - vehicle_count = 0 - traffic_light_count = 0 - - # 绘制车辆 - if SHOW_VEHICLES: - vehicles = list(world.get_actors().filter('*vehicle*')) - - for npc in vehicles: - if npc.id == vehicle.id: - continue - - # 计算距离 - dist = npc.get_transform().location.distance(vehicle.get_transform().location) - if dist >= DISTANCE_THRESHOLD: - continue - - # 检查是否在相机前方 - forward_vec = vehicle.get_transform().get_forward_vector() - ray = npc.get_transform().location - vehicle.get_transform().location - - if forward_vec.dot(ray) <= 0: - continue - - # 获取边界框顶点 - bb = npc.bounding_box - verts = bb.get_world_vertices(npc.get_transform()) - - # 投影到2D - 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) - - # 绘制3D边界框 - for edge in EDGES: - p1 = points_2d[edge[0]] - p2 = points_2d[edge[1]] - - if point_in_canvas(p1, height, width) or point_in_canvas(p2, height, width): - thickness = max(1, int(2 - dist / 50)) - color_intensity = max(50, int(255 - dist)) - color = (0, color_intensity, 0) - - cv2.line(img, (int(p1[0]), int(p1[1])), - (int(p2[0]), int(p2[1])), color, thickness) - - vehicle_count += 1 - - # 绘制红绿灯 - if SHOW_TRAFFIC_LIGHTS: - traffic_lights = list(world.get_actors().filter('*traffic_light*')) - - for light in traffic_lights: - # 计算距离 - dist = light.get_transform().location.distance(vehicle.get_transform().location) - if dist >= TRAFFIC_LIGHT_DISTANCE: - continue - - # 检查是否在相机前方 - forward_vec = vehicle.get_transform().get_forward_vector() - ray = light.get_transform().location - vehicle.get_transform().location - - if forward_vec.dot(ray) <= 0: - continue - - # 获取红绿灯位置 - location = light.get_transform().location - - # 投影到2D - ray0 = location - camera.get_transform().location - cam_forward_vec = camera.get_transform().get_forward_vector() - - if cam_forward_vec.dot(ray0) > 0: - point_2d = get_image_point(location, K, world_2_camera) - else: - point_2d = get_image_point(location, K_b, world_2_camera) - - # 检查点是否在画布内 - if not point_in_canvas(point_2d, height, width): - continue - - x, y = int(point_2d[0]), int(point_2d[1]) - - # 获取红绿灯状态 - light_state = light.get_state() - state_mapping = { - carla.TrafficLightState.Green: 0, # 绿色 - carla.TrafficLightState.Yellow: 1, # 黄色 - carla.TrafficLightState.Red: 2, # 红色 - } - state_idx = state_mapping.get(light_state, 3) # 默认白色 - light_color = TRAFFIC_LIGHT_COLORS[state_idx] - - # 绘制红绿灯 - radius = max(6, int(15 - dist / 20)) - cv2.circle(img, (x, y), radius, light_color, -1) - cv2.circle(img, (x, y), radius, (255, 255, 255), 1) - - # 添加文字标签 - if SHOW_TRAFFIC_LIGHTS_STATE and radius > 8: - state_name = TRAFFIC_LIGHT_STATE_NAMES[state_idx] - text_size = cv2.getTextSize(state_name, cv2.FONT_HERSHEY_SIMPLEX, 0.5, 1)[0] - text_x = x - text_size[0] // 2 - text_y = y - radius - 5 - - # 文字背景 - cv2.rectangle(img, (text_x - 3, text_y - text_size[1] - 3), - (text_x + text_size[0] + 3, text_y + 3), - (40, 40, 40), -1) - - # 文字 - cv2.putText(img, state_name, (text_x, text_y), - cv2.FONT_HERSHEY_SIMPLEX, 0.5, (255, 255, 255), 1) - - traffic_light_count += 1 - - return img, vehicle_count, traffic_light_count - - except Exception as e: - print(f"❌ 3D边界框绘制错误: {e}") - return image, 0, 0 - - -# ====================== 绘制清晰的信息面板(修复乱码) ====================== -def draw_info_panel(image, fps, avg_frame_time, frame_count, vehicle_count, traffic_light_count): - """在图像上绘制清晰的信息面板""" - try: - img = image.copy() - - # 面板尺寸和位置(根据图像大小调整) - panel_width = 320 - panel_height = 180 - panel_x = 10 - panel_y = 10 - - # 确保面板不会超出图像边界 - if panel_x + panel_width > img.shape[1]: - panel_x = img.shape[1] - panel_width - 10 - if panel_y + panel_height > img.shape[0]: - panel_y = img.shape[0] - panel_height - 10 - - # 创建半透明背景 - overlay = img.copy() - cv2.rectangle(overlay, (panel_x, panel_y), - (panel_x + panel_width, panel_y + panel_height), - PANEL_BG_COLOR, -1) - alpha = 0.8 - cv2.addWeighted(overlay, alpha, img, 1 - alpha, 0, img) - - # 绘制边框 - cv2.rectangle(img, (panel_x, panel_y), - (panel_x + panel_width, panel_y + panel_height), - PANEL_BORDER_COLOR, 2) - - # 标题 - 使用英文避免乱码 - title = "CARLA 3D VISUALIZATION" - cv2.putText(img, title, (panel_x + 10, panel_y + 25), - cv2.FONT_HERSHEY_SIMPLEX, 0.7, HIGHLIGHT_COLOR, 2) - - # 分隔线 - line_y = panel_y + 45 - cv2.line(img, (panel_x + 10, line_y), (panel_x + panel_width - 10, line_y), - (100, 200, 100), 1) - - # 信息区域 - info_start_y = line_y + 10 - line_spacing = 25 - - # 第1行:FPS和帧数 - row1_y = info_start_y - fps_text = f"FPS: {fps:.1f}" - frame_text = f"Frame: {frame_count}" - - cv2.putText(img, fps_text, (panel_x + 15, row1_y), - cv2.FONT_HERSHEY_SIMPLEX, 0.6, TEXT_COLOR, 1) - cv2.putText(img, frame_text, (panel_x + 150, row1_y), - cv2.FONT_HERSHEY_SIMPLEX, 0.6, TEXT_COLOR, 1) - - # 第2行:帧时间 - row2_y = row1_y + line_spacing - frame_time_text = f"Frame Time: {avg_frame_time:.1f}ms" - cv2.putText(img, frame_time_text, (panel_x + 15, row2_y), - cv2.FONT_HERSHEY_SIMPLEX, 0.6, TEXT_COLOR, 1) - - # 第3行:车辆和红绿灯数量 - row3_y = row2_y + line_spacing - vehicles_text = f"Vehicles: {vehicle_count}" - lights_text = f"Lights: {traffic_light_count}" - - cv2.putText(img, vehicles_text, (panel_x + 15, row3_y), - cv2.FONT_HERSHEY_SIMPLEX, 0.6, TEXT_COLOR, 1) - cv2.putText(img, lights_text, (panel_x + 150, row3_y), - cv2.FONT_HERSHEY_SIMPLEX, 0.6, TEXT_COLOR, 1) - - # 第4行:显示状态 - row4_y = row3_y + line_spacing - status_text = f"Display: V{'ON' if SHOW_VEHICLES else 'OFF'} T{'ON' if SHOW_TRAFFIC_LIGHTS else 'OFF'}" - cv2.putText(img, status_text, (panel_x + 15, row4_y), - cv2.FONT_HERSHEY_SIMPLEX, 0.55, TEXT_COLOR, 1) - - # 操作提示(最后一行) - hint_y = panel_y + panel_height - 10 - hint_text = "V:Vehicles T:Traffic I:Info Q:Quit" - cv2.putText(img, hint_text, (panel_x + 10, hint_y), - cv2.FONT_HERSHEY_SIMPLEX, 0.5, (150, 250, 150), 1) - - return img - - except Exception as e: - print(f"❌ 信息面板绘制错误: {e}") - return image - - -# ====================== 主函数 ====================== -def main(): - """主函数""" - # 声明全局变量 - global SHOW_INFO_PANEL, SHOW_VEHICLES, SHOW_TRAFFIC_LIGHTS, SHOW_TRAFFIC_LIGHTS_STATE - - world = None - camera = None - perf_monitor = None - - try: - print("=" * 60) - print("CARLA 3D Visualization System") - print("=" * 60) - print(f"Resolution: {CAMERA_WIDTH}x{CAMERA_HEIGHT}") - print(f"View Distance: {DISTANCE_THRESHOLD}m") - print(f"NPC Vehicles: {NPC_VEHICLE_NUM}") - print("=" * 60) - - # 1. 初始化Carla - print("Initializing Carla environment...") - world, client, vehicle, camera, image_queue = init_carla_environment() - - # 2. 性能监控 - perf_monitor = PerformanceMonitor() - - # 3. 获取spectator - spectator = world.get_spectator() - - # 4. 主循环 - print("\nStarting main loop...") - print("Controls:") - print(" V: Toggle vehicles display") - print(" T: Toggle traffic lights display") - print(" I: Toggle info panel") - print(" S: Save screenshot") - print(" R: Reset statistics") - print(" Q/ESC: Quit program") - - frame_count = 0 - - # 创建可调整大小的窗口 - window_title = "CARLA 3D Visualization" - cv2.namedWindow(window_title, cv2.WINDOW_NORMAL) - cv2.resizeWindow(window_title, CAMERA_WIDTH, CAMERA_HEIGHT) - - # 设置窗口位置(可选) - cv2.moveWindow(window_title, 100, 100) - - while True: - # 性能监控 - perf_monitor.start_frame() - - # 同步Carla世界 - world.tick() - - # 更新观察者视角 - try: - vehicle_transform = vehicle.get_transform() - spectator_transform = carla.Transform( - vehicle_transform.transform(carla.Location(x=-6, z=50)), # 更近的视角 - carla.Rotation(yaw=-180, pitch=-75) - ) - spectator.set_transform(spectator_transform) - except: - pass - - # 获取图像 - if image_queue.empty(): - time.sleep(0.001) - continue - - origin_image = image_queue.get() - frame_count += 1 - - # 绘制3D边界框和红绿灯 - result_image, vehicle_count, traffic_light_count = draw_3d_bounding_boxes( - origin_image, world, camera, vehicle - ) - - # 获取性能数据 - fps, avg_frame_time, current_frame_time = perf_monitor.end_frame() - - # 绘制信息面板 - if SHOW_INFO_PANEL: - result_image = draw_info_panel( - result_image, fps, avg_frame_time, frame_count, - vehicle_count, traffic_light_count - ) - - # 显示图像 - cv2.imshow(window_title, result_image) - - # 检查按键 - key = cv2.waitKey(1) & 0xFF - if key == ord('q') or key == 27: - print("Quitting...") - break - elif key == ord('v'): - SHOW_VEHICLES = not SHOW_VEHICLES - status = "ON" if SHOW_VEHICLES else "OFF" - print(f"Vehicles display: {status}") - elif key == ord('t'): - SHOW_TRAFFIC_LIGHTS = not SHOW_TRAFFIC_LIGHTS - status = "ON" if SHOW_TRAFFIC_LIGHTS else "OFF" - print(f"Traffic lights display: {status}") - elif key == ord('i'): - SHOW_INFO_PANEL = not SHOW_INFO_PANEL - status = "ON" if SHOW_INFO_PANEL else "OFF" - print(f"Info panel: {status}") - elif key == ord('s'): - timestamp = time.strftime("%Y%m%d_%H%M%S") - filename = f"carla_{timestamp}.png" - cv2.imwrite(filename, result_image) - print(f"Screenshot saved: {filename}") - elif key == ord('r'): - frame_count = 0 - perf_monitor = PerformanceMonitor() - print("Statistics reset") - - except KeyboardInterrupt: - print("\nInterrupted") - except Exception as e: - print(f"\nError: {e}") - traceback.print_exc() - finally: - print("\nCleaning up resources...") - - if camera is not None: - try: - camera.stop() - camera.destroy() - print("Camera destroyed") - except: - pass - - if world is not None: - try: - settings = world.get_settings() - settings.synchronous_mode = False - settings.fixed_delta_seconds = None - world.apply_settings(settings) - print("Carla sync mode disabled") - except: - pass - - cv2.destroyAllWindows() - - # 清理车辆 - try: - if vehicle is not None: - vehicle.destroy() - print("Ego vehicle destroyed") - - # 清理NPC - npc_count = 0 - for actor in world.get_actors().filter('vehicle.*'): - if actor.is_alive: - try: - actor.destroy() - npc_count += 1 - except: - pass - print(f"NPC vehicles destroyed: {npc_count}") - except: - pass - - print("Program exited") - - -if __name__ == '__main__': - main() \ No newline at end of file