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] =?UTF-8?q?=E5=B0=86=E5=85=B6=E4=BB=96=E4=BB=A3=E7=A0=81?= =?UTF-8?q?=E5=8A=9F=E8=83=BD=E5=90=88=E5=B9=B6=E8=BF=9Bmain,=E4=BC=98?= =?UTF-8?q?=E5=8C=96main=E4=BB=A3=E7=A0=81=EF=BC=9A=E7=BA=A2=E7=BB=BF?= =?UTF-8?q?=E7=81=AF=E7=9C=9F=E5=80=BC=E5=8F=AF=E8=A7=86=E5=8C=96=E4=B8=8E?= =?UTF-8?q?=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