diff --git a/src/carla_tracking/README.md b/src/carla_tracking/README.md index badad65bcf..cff3cef147 100644 --- a/src/carla_tracking/README.md +++ b/src/carla_tracking/README.md @@ -1,99 +1,99 @@ -# CARLA 目标检测与跟踪系统 +# CARLA目标检测与跟踪系统 + +这是一个基于CARLA仿真环境的目标检测与跟踪系统,使用YOLOv5作为检测模型,结合改进的SORT算法实现多目标跟踪,并利用深度信息提升跟踪性能。 ## 功能特点 -- 基于CARLA仿真环境的实时目标检测与跟踪 -- 使用YOLOv5系列模型(yolov5s、yolov5su、yolov5m、yolov5mu、yolov5x)进行目标检测 -- 改进版SORT跟踪算法,结合深度信息优化跟踪效果 -- 动态调整的卡尔曼滤波器,根据目标距离自适应调整参数 -- 基于深度图像的目标距离估算(支持中位数和加权平均两种计算方式) -- 实时计算目标速度信息 -- 可视化标注,显示目标边界框、ID、距离和速度信息 -- 支持NPC车辆自动生成与交通管理 - -## 依赖环境 - -- Python 3.8+ -- CARLA Simulator 0.9.10+ -- 必要的Python库: - - carla - - opencv-python - - numpy - - torch - - ultralytics - - scipy - - argparse - - queue - -## 主要组件说明 - -### 1. 目标检测模块 -- `load_detection_model`:加载YOLOv5检测模型,自动选择设备(GPU/CPU),支持半精度计算加速 -- 支持模型自动 fallback 机制,当指定模型不存在时自动选择可用模型 - -### 2. 跟踪模块 -- `Sort`类:改进版SORT跟踪器,结合深度信息优化跟踪效果 - - 动态IOU阈值:根据目标距离调整匹配阈值 - - 距离加权匹配:近距离目标权重更高 - - 基于距离的跟踪生命周期管理 -- `KalmanFilter`类:卡尔曼滤波器,用于目标运动预测 - - 根据目标距离动态调整过程噪声协方差Q - - 支持位置和速度状态估计 - -### 3. 深度信息处理 -- `get_target_distance`:从深度图像中计算目标距离 - - 支持中位数和加权平均两种距离计算方式 - - 基于目标区域的有效深度提取 - -### 4. CARLA相关函数 -- `setup_carla_client`:设置CARLA客户端和世界环境,配置同步模式 -- `spawn_ego_vehicle`:生成主车辆,优先选择林肯MKZ,失败时自动 fallback 到其他车型 -- `spawn_npcs`:生成NPC车辆,确保在主车辆周围合理范围内生成 -- 交通管理器配置:设置NPC车辆的自动驾驶行为参数 - -## 运行说明 - -### 基本命令 +- 连接CARLA仿真环境,生成主车辆和NPC车辆 +- 使用YOLOv5模型进行实时目标检测 +- 基于改进的SORT算法实现多目标跟踪,结合卡尔曼滤波预测 +- 利用深度相机数据计算目标距离和速度 +- 实时性能监控与可视化 +- 支持多种YOLOv5模型选择,可根据硬件性能调整 + +## 安装依赖 + ```bash -python main.py --model yolov5mu --tracker sort --npc-count 30 +# 基础依赖 +pip install numpy opencv-python torch torchvision +pip install carla # 请根据CARLA版本安装对应版本 +pip install scipy argparse ``` -### 主要参数 -- `--model`:选择YOLOv5模型(yolov5s、yolov5su、yolov5m、yolov5mu、yolov5x) -- `--tracker`:选择跟踪器(当前仅支持sort) -- `--host`:CARLA服务器地址(默认localhost) -- `--port`:CARLA服务器端口(默认2000) -- `--conf-thres`:检测置信度阈值(默认0.15) -- `--iou-thres`:IOU阈值(默认0.4) -- `--use-depth`:使用深度信息(默认True) -- `--show-depth`:显示深度图像(默认False) -- `--npc-count`:NPC车辆数量(默认30) - -### 操作说明 -- 按 'q' 键:退出程序 -- 按 'r' 键:重新生成NPC车辆 - -## 自定义配置 - -- 在`load_detection_model`函数中修改模型路径或添加新模型 -- 在`Sort`类初始化时调整跟踪参数(max_age, min_hits, iou_threshold) -- 在`KalmanFilter`中调整滤波器参数(Q_base, R, dist_thresholds) -- 在`spawn_npcs`函数中调整NPC生成范围和密度 -- 在主循环中调整性能监控参数和显示信息 - -## 性能优化 - -- 对于GPU用户,自动使用半精度计算加速 -- 深度图像预处理优化,平衡精度和速度 -- 跟踪算法中使用距离信息动态调整参数,提高跟踪效率 -- 性能监控功能,自动识别瓶颈并提供优化建议 -- 通过调整模型类型和参数,可以在检测精度和运行速度之间取得平衡 +## 使用方法 + +```bash +python main.py [参数选项] +``` + +### 参数说明 + +| 参数 | 说明 | 可选值 | 默认值 | +|------|------|--------|--------| +| --model | 选择YOLOv5模型 | yolov5s, yolov5su, yolov5m, yolov5mu, yolov5x | yolov5mu | +| --tracker | 选择跟踪算法 | sort | sort | +| --host | CARLA服务器地址 | - | localhost | +| --port | CARLA服务器端口 | - | 2000 | +| --conf-thres | 检测置信度阈值 | 0-1 | 0.15 | +| --iou-thres | IOU阈值 | 0-1 | 0.4 | +| --use-depth | 是否使用深度相机 | - | True | +| --show-depth | 是否显示深度图像 | - | False | +| --npc-count | NPC车辆数量 | - | 30 | + +## 系统架构 + +1. **CARLA环境交互模块**:负责连接CARLA服务器,生成车辆和传感器 +2. **传感器数据获取模块**:获取RGB图像和深度图像数据 +3. **目标检测模块**:使用YOLOv5模型检测图像中的车辆目标 +4. **目标跟踪模块**:基于改进的SORT算法,结合卡尔曼滤波实现多目标跟踪 +5. **可视化模块**:实时显示检测和跟踪结果,以及性能监控信息 + +## 核心算法说明 + +### 卡尔曼滤波器 + +实现了一个针对目标跟踪优化的卡尔曼滤波器,根据目标距离动态调整过程噪声协方差矩阵Q: +- 远距离目标(>50m):增大Q值,允许更大的预测误差 +- 中距离目标(30-50m):中等Q值 +- 近距离目标(<30m):减小Q值,提高跟踪稳定性 + +### 改进的SORT跟踪算法 + +在标准SORT算法基础上进行了以下改进: +- 结合深度信息动态调整IOU阈值 +- 对不同距离的目标使用不同的尺寸过滤条件 +- 对远距离目标延长跟踪保留时间 +- 使用加权平滑处理目标中心位置,提高跟踪稳定性 +- 基于距离变化计算目标速度 + +## 操作说明 + +- 运行程序后,系统会自动连接CARLA并生成仿真环境 +- 按 'q' 键退出程序 +- 按 'r' 键重新生成NPC车辆 ## 性能监控 -程序每50帧会输出一次性能统计,包括: +系统提供实时性能监控面板,显示各阶段耗时: - CARLA同步时间 -- 图像和深度获取时间 -- 目标检测和跟踪时间 -- 结果绘制和显示时间 -- 自动识别性能瓶颈并提供优化建议 \ No newline at end of file +- 图像获取时间 +- 深度图像处理时间 +- 目标检测时间 +- 目标跟踪时间 +- 结果绘制时间 +- 显示输出时间 + +每50帧会打印一次性能统计信息,并自动识别性能瓶颈并给出优化建议。 + +## 优化建议 + +- 若检测耗时过长,可尝试使用更小的模型(如yolov5s)或降低检测分辨率 +- 若跟踪耗时过长,可减少NPC车辆数量 +- 若CARLA同步耗时过长,可降低仿真频率或减少场景复杂度 +- 对于远距离目标检测,可调整检测阈值以提高检测率 + +## 注意事项 + +- 确保CARLA服务器已启动并正常运行 +- 根据硬件性能选择合适的模型,高性能GPU推荐使用yolov5x,CPU或低性能GPU推荐使用yolov5s +- 深度相机数据处理可能会增加系统负载,若性能不足可关闭(--use-depth=False) \ No newline at end of file diff --git a/src/carla_tracking/main.py b/src/carla_tracking/main.py index 1a3821fe93..e838be2983 100644 --- a/src/carla_tracking/main.py +++ b/src/carla_tracking/main.py @@ -9,37 +9,28 @@ from collections import deque import math import time +import sys +from pathlib import Path # -------------------------- 优化版SORT跟踪器 -------------------------- class KalmanFilter: def __init__(self): self.dt = 1.0 - self.x = np.zeros((4, 1)) - self.F = np.array([[1, 0, self.dt, 0], - [0, 1, 0, self.dt], - [0, 0, 1, 0], - [0, 0, 0, 1]], dtype=np.float32) - self.H = np.array([[1, 0, 0, 0], - [0, 1, 0, 0]], dtype=np.float32) - self.P = np.eye(4, dtype=np.float32) * 1000 - self.Q_base = np.array([[1, 0, 0, 0], - [0, 1, 0, 0], - [0, 0, 5, 0], - [0, 0, 0, 5]], dtype=np.float32) + self.x = np.zeros((6, 1)) # 增加速度和加速度状态 + self.F = np.array([[1, 0, self.dt, 0, 0.5 * self.dt ** 2, 0], + [0, 1, 0, self.dt, 0, 0.5 * self.dt ** 2], + [0, 0, 1, 0, self.dt, 0], + [0, 0, 0, 1, 0, self.dt], + [0, 0, 0, 0, 1, 0], + [0, 0, 0, 0, 0, 1]], dtype=np.float32) + self.H = np.array([[1, 0, 0, 0, 0, 0], + [0, 1, 0, 0, 0, 0]], dtype=np.float32) + self.P = np.eye(6, dtype=np.float32) * 1000 + self.Q = np.eye(6, dtype=np.float32) * 0.1 self.R = np.eye(2, dtype=np.float32) * 5 - self.Q = self.Q_base.copy() - self.dist_thresholds = [30, 50] - - def predict(self, distance=None): - if distance is not None: - if distance > self.dist_thresholds[1]: - self.Q = self.Q_base * 2.0 - elif distance > self.dist_thresholds[0]: - self.Q = self.Q_base * 1.5 - else: - self.Q = self.Q_base * 0.8 + def predict(self): self.x = self.F @ self.x self.P = self.F @ self.P @ self.F.T + self.Q return self.x[:2] @@ -56,7 +47,7 @@ def update(self, z): class Track: __slots__ = ('id', 'kf', 'x1', 'y1', 'x2', 'y2', 'center', 'width', 'height', 'hits', 'age', 'history', 'distance', 'distance_history', - 'velocity', 'last_distance') + 'velocity', 'last_distance', 'world_location', 'world_velocity') def __init__(self, box, track_id): self.id = track_id @@ -74,15 +65,17 @@ def __init__(self, box, track_id): self.distance_history = deque(maxlen=5) self.velocity = 0.0 self.last_distance = None + self.world_location = None # 在CARLA世界中的位置 + self.world_velocity = None # 在CARLA世界中的速度 def predict(self): self.age += 1 - center = self.kf.predict(self.distance) + center = self.kf.predict() if self.history: - smooth_weight = 0.7 if (self.distance and self.distance > 30) else 0.4 self.history.append(center) + # 平滑处理 n = len(self.history) if n > 1: weights = np.linspace(0.1, 1.0, n) @@ -95,14 +88,12 @@ def predict(self): else: smoothed_center = center + # 更新边界框 self.x1 = smoothed_center[0, 0] - self.width / 2 self.y1 = smoothed_center[1, 0] - self.height / 2 self.x2 = self.x1 + self.width self.y2 = self.y1 + self.height - if self.last_distance is not None and self.distance is not None: - self.velocity = abs(self.distance - self.last_distance) / self.kf.dt - return [self.x1, self.y1, self.x2, self.y2] def update(self, box, distance=None): @@ -151,6 +142,7 @@ def set_depths(self, depths): def update(self, detections): if len(detections) == 0: for track in self.tracks: + track.predict() track.age += 1 self.tracks = [t for t in self.tracks if t.age <= self.max_age] if self.tracks and self.min_hits > 0: @@ -158,6 +150,10 @@ def update(self, detections): for t in self.tracks if t.hits >= self.min_hits]) return np.array([]) + # 先预测所有track + for track in self.tracks: + track.predict() + if len(self.tracks) == 0: tracks_to_return = [] for i, det in enumerate(detections): @@ -181,9 +177,8 @@ def update(self, detections): det_distance = self.depths[det_idx] if det_idx < len(self.depths) else None self.tracks[track_idx].update(detections[det_idx][:4], det_distance) - for track_idx in range(len(self.tracks)): - if track_idx in unmatched_tracks: - self.tracks[track_idx].age += 1 + for track_idx in unmatched_tracks: + self.tracks[track_idx].age += 1 for det_idx in unmatched_dets: box = detections[det_idx][:4] @@ -254,22 +249,7 @@ def _iou_batch(self, b1, b2): b1_area = (b1_x2 - b1_x1) * (b1_y2 - b1_y1) b2_area = (b2_x2 - b2_x1) * (b2_y2 - b2_y1) - base_iou = inter_area / (b1_area[:, None] + b2_area[None, :] - inter_area + 1e-6) - - if len(self.depths) == b2.shape[0]: - distances = self.depths - distance_weights = np.ones_like(distances) - near_mask = distances < self.depth_thresholds['near'] - medium_mask = (distances >= self.depth_thresholds['near']) & (distances < self.depth_thresholds['medium']) - far_mask = (distances >= self.depth_thresholds['medium']) & (distances < self.depth_thresholds['far']) - - distance_weights[near_mask] = 1.5 - distance_weights[medium_mask] = 1.2 - distance_weights[far_mask] = 0.9 - distance_weights[distances >= self.depth_thresholds['far']] = 0.7 - - return base_iou * distance_weights[None, :] - return base_iou + return inter_area / (b1_area[:, None] + b2_area[None, :] - inter_area + 1e-6) def _hungarian_algorithm(self, cost_matrix): if cost_matrix.size == 0: @@ -340,6 +320,192 @@ def load_detection_model(model_type): return model, model.names +# -------------------------- 运动控制器 -------------------------- +class VehicleController: + """车辆运动控制器""" + + def __init__(self, vehicle): + self.vehicle = vehicle + self.max_speed = 50.0 # 最大速度 km/h + self.target_speed = 30.0 # 目标速度 km/h + self.obstacle_distance = 100.0 # 安全距离 + self.last_update = time.time() + self.control_state = { + 'throttle': 0.0, + 'steer': 0.0, + 'brake': 0.0, + 'reverse': False + } + + def update_control(self, detected_obstacles): + """根据检测到的障碍物更新控制""" + control = carla.VehicleControl() + + # 获取当前速度 + velocity = self.vehicle.get_velocity() + speed = math.sqrt(velocity.x ** 2 + velocity.y ** 2) * 3.6 # m/s to km/h + + # 基本PID控制器 + speed_error = self.target_speed - speed + throttle = np.clip(speed_error * 0.01, 0.0, 0.8) + + # 检查障碍物 + brake = 0.0 + if detected_obstacles: + closest_distance = min([d for d in detected_obstacles if d < 50]) + if closest_distance < 15: # 紧急制动 + throttle = 0.0 + brake = 1.0 + elif closest_distance < 30: # 减速 + throttle = np.clip(throttle * 0.3, 0.0, 0.3) + brake = 0.3 + + # 随机转向模拟真实驾驶 + if random.random() < 0.05: # 5%概率微调方向 + steer = random.uniform(-0.1, 0.1) + else: + steer = 0.0 + + control.throttle = throttle + control.steer = steer + control.brake = brake + control.hand_brake = False + control.reverse = False + + self.control_state = { + 'throttle': throttle, + 'steer': steer, + 'brake': brake, + 'speed': speed + } + + return control + + def set_target_speed(self, speed): + self.target_speed = np.clip(speed, 0.0, self.max_speed) + + def emergency_stop(self): + control = carla.VehicleControl() + control.throttle = 0.0 + control.brake = 1.0 + return control + + +# -------------------------- NPC车辆运动控制 -------------------------- +class NPCManager: + """NPC车辆管理器""" + + def __init__(self, client): + self.client = client + self.traffic_manager = None + self.npc_vehicles = [] + + try: + self.traffic_manager = self.client.get_trafficmanager() + self.traffic_manager.set_synchronous_mode(True) + print("交通管理器初始化成功") + except Exception as e: + print(f"交通管理器初始化失败: {e}") + + def spawn_npcs(self, world, count=30, ego_vehicle=None): + """生成NPC车辆并设置自动驾驶""" + bp_lib = world.get_blueprint_library() + spawn_points = world.get_map().get_spawn_points() + + if not spawn_points: + print("没有可用的生成点") + return [] + + # 清理现有NPC + self.destroy_all_npcs() + + random.shuffle(spawn_points) + spawned_count = 0 + + for spawn_point in spawn_points: + if spawned_count >= count: + break + + # 检查是否太靠近主车辆 + if ego_vehicle: + try: + ego_loc = ego_vehicle.get_location() + dist = math.sqrt( + (spawn_point.location.x - ego_loc.x) ** 2 + + (spawn_point.location.y - ego_loc.y) ** 2 + ) + if dist < 20.0: # 离主车辆太近 + continue + except: + pass + + # 随机选择车辆蓝图 + try: + vehicle_bp = random.choice(list(bp_lib.filter('vehicle.*'))) + except: + continue + + # 设置随机颜色 + if vehicle_bp.has_attribute('color'): + colors = vehicle_bp.get_attribute('color').recommended_values + if colors: + vehicle_bp.set_attribute('color', random.choice(colors)) + + # 尝试生成车辆 + npc = world.try_spawn_actor(vehicle_bp, spawn_point) + + if npc: + # 设置自动驾驶 + if self.traffic_manager: + try: + npc.set_autopilot(True, self.traffic_manager.get_port()) + + # 设置个性化参数 + self.traffic_manager.distance_to_leading_vehicle( + npc, random.uniform(2.0, 5.0) + ) + self.traffic_manager.vehicle_percentage_speed_difference( + npc, random.uniform(-30.0, 30.0) + ) + # 不同版本的CARLA API可能没有set_lane_change_permission + try: + self.traffic_manager.set_lane_change_permission(npc, False) + except AttributeError: + # 旧版本API,忽略这个设置 + pass + except Exception as e: + print(f"设置NPC自动驾驶失败: {e}") + + self.npc_vehicles.append(npc) + spawned_count += 1 + print(f"生成NPC {spawned_count}/{count}") + + print(f"总共生成 {len(self.npc_vehicles)} 辆NPC车辆") + return self.npc_vehicles + + def update_npc_behavior(self): + """更新NPC行为""" + for npc in self.npc_vehicles: + try: + # 随机更新NPC的速度差异,增加变化性 + if random.random() < 0.01 and self.traffic_manager: # 1%概率 + self.traffic_manager.vehicle_percentage_speed_difference( + npc, random.uniform(-40.0, 40.0) + ) + except: + pass + + def destroy_all_npcs(self): + """销毁所有NPC车辆""" + for npc in self.npc_vehicles: + try: + if npc.is_alive: + npc.destroy() + except: + pass + self.npc_vehicles.clear() + + # -------------------------- 性能监控面板函数 -------------------------- def draw_performance_panel(image, timings, fps, frame_count): """ @@ -347,10 +513,9 @@ def draw_performance_panel(image, timings, fps, frame_count): """ h, w = image.shape[:2] - # 计算各阶段耗时(最近10帧平均) def get_avg_time(key, default=0.0): if key in timings and timings[key]: - recent = timings[key][-10:] # 取最近10帧 + recent = timings[key][-10:] return np.mean(recent) if recent else default return default @@ -365,28 +530,26 @@ def get_avg_time(key, default=0.0): total_time = get_avg_time('total') * 1000 # 面板位置和尺寸 - panel_x = 10 # 左上角x坐标 - panel_y = 10 # 左上角y坐标 - panel_width = 280 # 面板宽度 - panel_height = 200 # 面板高度 + panel_x = 10 + panel_y = 10 + panel_width = 280 + panel_height = 200 # 创建半透明面板背景 panel_bg = np.zeros((panel_height, panel_width, 3), dtype=np.uint8) - panel_bg[:] = (20, 20, 20) # 深灰色背景 + panel_bg[:] = (20, 20, 20) # 叠加面板到图像上 x1, y1 = panel_x, panel_y x2, y2 = panel_x + panel_width, panel_y + panel_height - # 确保面板在图像范围内 if x2 <= w and y2 <= h: - # 半透明叠加 alpha = 0.7 image[y1:y2, x1:x2] = cv2.addWeighted( image[y1:y2, x1:x2], 1 - alpha, panel_bg, alpha, 0 ) - # 绘制标题和分隔线 + # 绘制标题 title = "性能监控面板" cv2.putText(image, title, (panel_x + 10, panel_y + 20), cv2.FONT_HERSHEY_SIMPLEX, 0.6, (0, 255, 255), 1) @@ -398,7 +561,6 @@ def get_avg_time(key, default=0.0): y_offset = 45 line_height = 20 - # FPS和帧数 cv2.putText(image, f"FPS: {fps:.1f}", (panel_x + 10, panel_y + y_offset), cv2.FONT_HERSHEY_SIMPLEX, 0.5, (0, 255, 0), 1) cv2.putText(image, f"Frame: {frame_count}", (panel_x + 120, panel_y + y_offset), @@ -406,19 +568,16 @@ def get_avg_time(key, default=0.0): y_offset += line_height - # CARLA同步 color = (0, 255, 0) if carla_time < 5.0 else (0, 165, 255) if carla_time < 10.0 else (0, 0, 255) cv2.putText(image, f"CARLA: {carla_time:.1f}ms", (panel_x + 10, panel_y + y_offset), cv2.FONT_HERSHEY_SIMPLEX, 0.5, color, 1) - # 图像获取 color = (0, 255, 0) if image_time < 1.0 else (0, 165, 255) if image_time < 3.0 else (0, 0, 255) cv2.putText(image, f"Image: {image_time:.1f}ms", (panel_x + 120, panel_y + y_offset), cv2.FONT_HERSHEY_SIMPLEX, 0.5, color, 1) y_offset += line_height - # 深度获取 if depth_time > 0: color = (0, 255, 0) if depth_time < 2.0 else (0, 165, 255) if depth_time < 5.0 else (0, 0, 255) cv2.putText(image, f"Depth: {depth_time:.1f}ms", (panel_x + 10, panel_y + y_offset), @@ -426,80 +585,32 @@ def get_avg_time(key, default=0.0): y_offset += line_height - # 目标检测 color = (0, 255, 0) if detection_time < 10.0 else (0, 165, 255) if detection_time < 20.0 else (0, 0, 255) cv2.putText(image, f"Detection: {detection_time:.1f}ms", (panel_x + 10, panel_y + y_offset), cv2.FONT_HERSHEY_SIMPLEX, 0.5, color, 1) y_offset += line_height - # 目标跟踪 color = (0, 255, 0) if tracking_time < 5.0 else (0, 165, 255) if tracking_time < 10.0 else (0, 0, 255) cv2.putText(image, f"Tracking: {tracking_time:.1f}ms", (panel_x + 10, panel_y + y_offset), cv2.FONT_HERSHEY_SIMPLEX, 0.5, color, 1) - # 结果绘制 color = (0, 255, 0) if drawing_time < 2.0 else (0, 165, 255) if drawing_time < 5.0 else (0, 0, 255) cv2.putText(image, f"Drawing: {drawing_time:.1f}ms", (panel_x + 120, panel_y + y_offset), cv2.FONT_HERSHEY_SIMPLEX, 0.5, color, 1) y_offset += line_height - # 显示输出 color = (0, 255, 0) if display_time < 2.0 else (0, 165, 255) if display_time < 5.0 else (0, 0, 255) cv2.putText(image, f"Display: {display_time:.1f}ms", (panel_x + 10, panel_y + y_offset), cv2.FONT_HERSHEY_SIMPLEX, 0.5, color, 1) y_offset += line_height - # 总帧时间 color = (0, 255, 0) if total_time < 50.0 else (0, 165, 255) if total_time < 100.0 else (0, 0, 255) cv2.putText(image, f"Total: {total_time:.1f}ms", (panel_x + 10, panel_y + y_offset), cv2.FONT_HERSHEY_SIMPLEX, 0.5, color, 1) - y_offset += line_height - - # 绘制简易性能柱状图 - bar_x = panel_x + 10 - bar_y = panel_y + y_offset + 5 - bar_width = 250 - bar_height = 15 - - # 绘制背景 - cv2.rectangle(image, (bar_x, bar_y), (bar_x + bar_width, bar_y + bar_height), (50, 50, 50), -1) - - # 计算各阶段占比 - stages = [carla_time, image_time, depth_time, detection_time, tracking_time, drawing_time, display_time] - stage_names = ['C', 'I', 'D', 'Det', 'T', 'Draw', 'Disp'] - stage_colors = [ - (0, 200, 200), # CARLA - 青色 - (200, 200, 0), # Image - 黄色 - (200, 0, 200), # Depth - 紫色 - (255, 100, 0), # Detection - 橙色 - (0, 150, 255), # Tracking - 浅蓝色 - (100, 255, 100), # Drawing - 浅绿色 - (255, 100, 100) # Display - 浅红色 - ] - - # 绘制各阶段柱状图 - if total_time > 0: - accumulated = 0 - for i, (stage_time, color, name) in enumerate(zip(stages, stage_colors, stage_names)): - if stage_time > 0: - stage_width = int((stage_time / total_time) * bar_width) - if stage_width > 0: - # 绘制阶段条 - stage_x1 = bar_x + accumulated - stage_x2 = stage_x1 + stage_width - cv2.rectangle(image, (stage_x1, bar_y), (stage_x2, bar_y + bar_height), color, -1) - - # 绘制阶段名称 - if stage_width > 15: - cv2.putText(image, name, (stage_x1 + 2, bar_y + 12), - cv2.FONT_HERSHEY_SIMPLEX, 0.4, (255, 255, 255), 1) - - accumulated += stage_width - return image @@ -513,7 +624,6 @@ def draw_bounding_boxes(image, boxes, labels, class_names, **kwargs): result = image.copy() h, w = image.shape[:2] - # 颜色缓存 color_cache = {} for i, box in enumerate(boxes): @@ -556,7 +666,6 @@ def draw_bounding_boxes(image, boxes, labels, class_names, **kwargs): f"S:{velocities[i]:.1f}m/s" if velocities and i < len(velocities) and velocities[i] else "" ] - # 过滤空字符串并拼接 label_text = " ".join(filter(None, text_parts)) text_size = cv2.getTextSize(label_text, cv2.FONT_HERSHEY_SIMPLEX, 0.5, 2)[0] @@ -575,14 +684,12 @@ def preprocess_depth_image(depth_image): if depth_image is None: return None - # 如果深度图像是float16类型,转换为float32进行处理 if depth_image.dtype == np.float16: depth_image = depth_image.astype(np.float32) depth_image = np.clip(depth_image, 0.1, 200.0) if depth_image.shape[0] > 3 and depth_image.shape[1] > 3: - # 使用较小的高斯模糊,减少计算量 depth_image = cv2.GaussianBlur(depth_image, (3, 3), 0.5) max_val = np.max(depth_image) @@ -596,7 +703,6 @@ def get_target_distance(depth_image, box, use_median=True): if depth_image is None: return 50.0 - # 如果深度图像是float16类型,转换为float32进行计算 if depth_image.dtype == np.float16: depth_image = depth_image.astype(np.float32) @@ -636,24 +742,33 @@ def get_target_distance(depth_image, box, use_median=True): return float(np.mean(valid_depths)) -# -------------------------- CARLA相关函数(修正版) -------------------------- +# -------------------------- CARLA相关函数 -------------------------- def setup_carla_client(host='localhost', port=2000): - client = carla.Client(host, port) - 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) - - return world, client + """连接CARLA服务器""" + try: + client = carla.Client(host, port) + 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) + + print(f"连接到CARLA服务器 {host}:{port}") + print(f"地图: {world.get_map().name}") + print(f"同步模式: {settings.synchronous_mode}") + print(f"时间步长: {settings.fixed_delta_seconds}") + + return world, client + except Exception as e: + print(f"连接CARLA失败: {e}") + raise -def spawn_ego_vehicle(world): - if not world: - return None - +def spawn_ego_vehicle(world, enable_physics=True): + """生成主车辆""" bp_lib = world.get_blueprint_library() # 优先使用林肯MKZ @@ -681,257 +796,134 @@ def spawn_ego_vehicle(world): continue if not vehicle_bp: - vehicle_bp = random.choice([bp for bp in bp_lib.filter('vehicle.*')]) + try: + vehicle_bp = random.choice(list(bp_lib.filter('vehicle.*'))) + except: + print("错误:没有找到可用的车辆蓝图") + return None spawn_points = world.get_map().get_spawn_points() if not spawn_points: + print("错误:没有可用的生成点") return None - # 随机选择生成点 + # 选择远离其他车辆的生成点 random.shuffle(spawn_points) for spawn_point in spawn_points[:10]: # 检查是否有其他车辆 too_close = False for actor in world.get_actors().filter('vehicle.*'): - actor_loc = actor.get_location() - dist = math.hypot( - actor_loc.x - spawn_point.location.x, - actor_loc.y - spawn_point.location.y - ) - if dist < 5.0: - too_close = True - break - - if not too_close: - vehicle = world.try_spawn_actor(vehicle_bp, spawn_point) - if vehicle: - vehicle.set_autopilot(True) - vehicle.set_simulate_physics(False) - return vehicle - - # 强制生成 - if spawn_points: - vehicle = world.try_spawn_actor(vehicle_bp, spawn_points[0]) - if vehicle: - vehicle.set_autopilot(True) - vehicle.set_simulate_physics(False) - return vehicle - - return None - - -def spawn_npcs(world, count=30, ego_vehicle=None): - """修正的NPC生成函数 - 确保在主车辆周围生成足够车辆""" - if not world: - return [] - - print(f"正在生成 {count} 辆NPC车辆...") - - # 先检查当前已有的车辆 - existing_vehicles = world.get_actors().filter('vehicle.*') - print(f"当前已有车辆数量: {len(existing_vehicles)}") - - bp_lib = world.get_blueprint_library() - - # 创建一个包含所有车辆蓝图的列表 - vehicle_blueprints = list(bp_lib.filter('vehicle.*')) - if not vehicle_blueprints: - print("错误:没有找到任何车辆蓝图!") - return [] - - print(f"可用的车辆蓝图数量: {len(vehicle_blueprints)}") - - spawn_points = world.get_map().get_spawn_points() - if not spawn_points: - print("警告:没有可用的生成点!") - return [] - - print(f"可用的生成点数量: {len(spawn_points)}") - - ego_location = ego_vehicle.get_location() if ego_vehicle else None - ego_transform = ego_vehicle.get_transform() if ego_vehicle else None - - spawned_count = 0 - npc_vehicles = [] - - # 只尝试每个生成点一次,避免无限循环 - random.shuffle(spawn_points) - - for i, spawn_point in enumerate(spawn_points): - if spawned_count >= count: - break - - if i % 10 == 0: - print(f"尝试生成点 {i + 1}/{len(spawn_points)}...") - - # 检查与主车辆的距离 - if ego_location: - dist_to_ego = math.hypot( - spawn_point.location.x - ego_location.x, - spawn_point.location.y - ego_location.y - ) - - # 只生成在合理距离范围内的车辆(30-80米) - if dist_to_ego < 30.0 or dist_to_ego > 80.0: - continue - - # 检查是否太靠近已生成的NPC - too_close = False - for npc in npc_vehicles: try: - npc_loc = npc.get_location() + actor_loc = actor.get_location() dist = math.hypot( - npc_loc.x - spawn_point.location.x, - npc_loc.y - spawn_point.location.y + actor_loc.x - spawn_point.location.x, + actor_loc.y - spawn_point.location.y ) - if dist < 5.0: # 5米内不生成 + if dist < 10.0: too_close = True break except: continue - if too_close: - continue - - try: - # 随机选择一个车辆蓝图 - vehicle_bp = random.choice(vehicle_blueprints) - - # 设置随机颜色 - if vehicle_bp.has_attribute('color'): - colors = vehicle_bp.get_attribute('color').recommended_values - if colors: - vehicle_bp.set_attribute('color', random.choice(colors)) + if not too_close: + try: + vehicle = world.spawn_actor(vehicle_bp, spawn_point) + if vehicle: + print(f"主车辆生成成功: {vehicle_bp.id}") + print(f"位置: X={spawn_point.location.x:.1f}, Y={spawn_point.location.y:.1f}") - # 尝试生成车辆 - npc = world.try_spawn_actor(vehicle_bp, spawn_point) + # 启用物理模拟 + vehicle.set_simulate_physics(enable_physics) - if npc: - npc.set_autopilot(True) - npc_vehicles.append(npc) - spawned_count += 1 + return vehicle + except Exception as e: + print(f"生成车辆失败: {e}") + continue - if ego_location: - dist_to_ego = math.hypot( - spawn_point.location.x - ego_location.x, - spawn_point.location.y - ego_location.y - ) - print(f"✓ 生成NPC {spawned_count}/{count} - 距离: {dist_to_ego:.1f}米") - else: - print(f"✓ 生成NPC {spawned_count}/{count}") + # 如果找不到合适的位置,强制生成 + if spawn_points: + try: + vehicle = world.spawn_actor(vehicle_bp, spawn_points[0]) + if vehicle: + vehicle.set_simulate_physics(enable_physics) + print(f"主车辆强制生成: {vehicle_bp.id}") + return vehicle except Exception as e: - print(f"生成NPC失败: {e}") - continue + print(f"强制生成车辆失败: {e}") - print(f"最终生成 {spawned_count} 辆NPC车辆") + return None - # 验证生成的车辆 - valid_npcs = [] - for npc in npc_vehicles: - try: - loc = npc.get_location() - if loc: - valid_npcs.append(npc) - except: - print(f"警告:无法获取NPC位置,可能已销毁") - print(f"有效NPC车辆: {len(valid_npcs)} 辆") +# 优化内存占用的回调函数 +def camera_callback(image, rgb_image_queue): + """RGB图像回调函数""" + try: + array = np.frombuffer(image.raw_data, dtype=np.uint8) + array = array.reshape((image.height, image.width, 4)) + rgb = array[..., :3] # 只存储RGB通道 - # 打印距离统计 - if ego_location and valid_npcs: - distances = [] - for npc in valid_npcs: + if rgb_image_queue.full(): try: - npc_loc = npc.get_location() - dist = math.hypot( - npc_loc.x - ego_location.x, - npc_loc.y - ego_location.y - ) - distances.append(dist) + rgb_image_queue.get_nowait() except: - continue - - if distances: - print( - f"NPC距离统计: 最近{min(distances):.1f}米, 最远{max(distances):.1f}米, 平均{np.mean(distances):.1f}米") - else: - print("警告:无法计算NPC距离") - - return valid_npcs - - -# 优化内存占用:使用更高效的回调函数 -def camera_callback(image, rgb_image_queue): - """优化内存占用的RGB图像回调函数""" - # 使用array接口避免额外复制,只取RGB通道 - array = np.frombuffer(image.raw_data, dtype=np.uint8) - array = array.reshape((image.height, image.width, 4)) - rgb = array[..., :3] # 只存储RGB通道,丢弃Alpha通道 - - # 使用put_nowait避免阻塞 - try: - rgb_image_queue.put_nowait(rgb) - except queue.Full: - # 丢弃最旧的一帧,放入新帧 - try: - rgb_image_queue.get_nowait() - except: - pass - rgb_image_queue.put_nowait(rgb) + pass + rgb_image_queue.put(rgb) + except Exception as e: + print(f"相机回调错误: {e}") def depth_camera_callback(image, depth_queue): - """优化内存占用的深度图像回调函数""" - # 使用更小的数据类型存储深度图像 - depth_data = np.frombuffer(image.raw_data, dtype=np.uint8) - depth_data = depth_data.reshape((image.height, image.width, 4)) - - # 使用uint16存储深度数据,减少内存占用 - depth_channel = ( - depth_data[..., 2].astype(np.uint16) + - depth_data[..., 1].astype(np.uint16) * 256 + - depth_data[..., 0].astype(np.uint16) * 256 ** 2 - ) + """深度图像回调函数""" + try: + depth_data = np.frombuffer(image.raw_data, dtype=np.uint8) + depth_data = depth_data.reshape((image.height, image.width, 4)) - # 转换为米为单位,并使用float16存储(相比float32减少50%内存) - depth_in_meters = depth_channel.astype(np.float16) / (256 ** 3 - 1) * 1000.0 + depth_channel = ( + depth_data[..., 2].astype(np.uint16) + + depth_data[..., 1].astype(np.uint16) * 256 + + depth_data[..., 0].astype(np.uint16) * 256 ** 2 + ) - # 预处理深度图像 - depth_in_meters = preprocess_depth_image(depth_in_meters) + depth_in_meters = depth_channel.astype(np.float16) / (256 ** 3 - 1) * 1000.0 + depth_in_meters = preprocess_depth_image(depth_in_meters) - # 使用put_nowait避免阻塞 - try: - depth_queue.put_nowait(depth_in_meters) - except queue.Full: - # 丢弃最旧的一帧,放入新帧 - try: - depth_queue.get_nowait() - except: - pass - depth_queue.put_nowait(depth_in_meters) + if depth_queue.full(): + try: + depth_queue.get_nowait() + except: + pass + depth_queue.put(depth_in_meters) + except Exception as e: + print(f"深度相机回调错误: {e}") # -------------------------- 主函数 -------------------------- def main(): - parser = argparse.ArgumentParser(description='CARLA目标检测与跟踪') - parser.add_argument('--model', type=str, default='yolov5mu', + # 初始化变量 + world = vehicle = camera = depth_camera = None + image_queue = depth_queue = None + client = controller = npc_manager = None + frame_count = 0 + + parser = argparse.ArgumentParser(description='CARLA目标检测与跟踪 - 动态车辆版本') + parser.add_argument('--model', type=str, default='yolov5m', choices=['yolov5s', 'yolov5su', 'yolov5m', 'yolov5mu', 'yolov5x']) - parser.add_argument('--tracker', type=str, default='sort', choices=['sort']) parser.add_argument('--host', type=str, default='localhost') parser.add_argument('--port', type=int, default=2000) parser.add_argument('--conf-thres', type=float, default=0.15) parser.add_argument('--iou-thres', type=float, default=0.4) parser.add_argument('--use-depth', action='store_true', default=True) parser.add_argument('--show-depth', action='store_true') - parser.add_argument('--npc-count', type=int, default=30) + parser.add_argument('--npc-count', type=int, default=20) + parser.add_argument('--enable-physics', action='store_true', default=True, + help='启用物理模拟(使车辆可以碰撞)') + parser.add_argument('--target-speed', type=float, default=30.0, + help='目标速度 (km/h)') + parser.add_argument('--manual-control', action='store_true', + help='手动控制模式(使用键盘WASD控制)') args = parser.parse_args() - # 初始化变量 - world = vehicle = camera = depth_camera = None - image_queue = depth_queue = None - client = None - try: # 1. 初始化设备 device = 'cuda' if torch.cuda.is_available() else 'cpu' @@ -940,26 +932,57 @@ def main(): # 2. 连接CARLA print("连接CARLA服务器...") world, client = setup_carla_client(args.host, args.port) + + # 获取交通管理器 + traffic_manager = None + try: + traffic_manager = client.get_trafficmanager() + traffic_manager.set_synchronous_mode(True) + print("交通管理器初始化成功") + except Exception as e: + print(f"交通管理器初始化失败,NPC可能无法自动驾驶: {e}") + spectator = world.get_spectator() - print(f"连接成功,地图: {world.get_map().name}") # 3. 清理环境 print("清理环境...") - for actor in world.get_actors().filter('vehicle.*'): - try: - actor.destroy() - except: - pass + try: + for actor in world.get_actors().filter('vehicle.*'): + try: + if actor.is_alive: + actor.destroy() + except: + pass + except: + pass + time.sleep(1) # 4. 生成主车辆 print("生成主车辆...") - vehicle = spawn_ego_vehicle(world) + vehicle = spawn_ego_vehicle(world, enable_physics=args.enable_physics) if not vehicle: print("主车辆生成失败,程序退出!") return - ego_location = vehicle.get_location() - print(f"主车辆位置: X={ego_location.x:.1f}, Y={ego_location.y:.1f}") + # 初始化车辆控制器 + controller = VehicleController(vehicle) + controller.set_target_speed(args.target_speed) + + if not args.manual_control and traffic_manager: + try: + # 设置自动驾驶(用于基础运动) + vehicle.set_autopilot(True, traffic_manager.get_port()) + traffic_manager.distance_to_leading_vehicle(vehicle, 2.5) + traffic_manager.vehicle_percentage_speed_difference(vehicle, 0.0) + print("主车辆自动驾驶已启用") + except Exception as e: + print(f"主车辆自动驾驶设置失败: {e}") + + try: + ego_location = vehicle.get_location() + print(f"主车辆位置: X={ego_location.x:.1f}, Y={ego_location.y:.1f}, Z={ego_location.z:.1f}") + except: + print("无法获取主车辆位置") # 5. 生成传感器 print("生成相机传感器...") @@ -969,71 +992,45 @@ def main(): camera_bp = bp_lib.find('sensor.camera.rgb') camera_bp.set_attribute('image_size_x', '800') camera_bp.set_attribute('image_size_y', '600') - camera_bp.set_attribute('fov', '100') + camera_bp.set_attribute('fov', '90') camera_bp.set_attribute('sensor_tick', '0.05') camera_transform = carla.Transform(carla.Location(x=1.5, z=1.8), carla.Rotation(pitch=-5, yaw=0)) - camera = world.spawn_actor(camera_bp, camera_transform, attach_to=vehicle) - - image_queue = queue.Queue(maxsize=3) - camera.listen(lambda image: camera_callback(image, image_queue)) + try: + camera = world.spawn_actor(camera_bp, camera_transform, attach_to=vehicle) + image_queue = queue.Queue(maxsize=3) + camera.listen(lambda image: camera_callback(image, image_queue)) + print("RGB相机传感器生成成功!") + except Exception as e: + print(f"RGB相机生成失败: {e}") + return # 深度相机 depth_camera = None depth_queue = None if args.use_depth: - depth_bp = bp_lib.find('sensor.camera.depth') - depth_bp.set_attribute('image_size_x', '800') - depth_bp.set_attribute('image_size_y', '600') - depth_bp.set_attribute('fov', '100') - depth_bp.set_attribute('sensor_tick', '0.05') - - depth_camera = world.spawn_actor(depth_bp, camera_transform, attach_to=vehicle) - depth_queue = queue.Queue(maxsize=3) - depth_camera.listen(lambda image: depth_camera_callback(image, depth_queue)) - print("深度相机传感器生成成功!") - - print("RGB相机传感器生成成功!") + try: + depth_bp = bp_lib.find('sensor.camera.depth') + depth_bp.set_attribute('image_size_x', '800') + depth_bp.set_attribute('image_size_y', '600') + depth_bp.set_attribute('fov', '90') + depth_bp.set_attribute('sensor_tick', '0.05') + + depth_camera = world.spawn_actor(depth_bp, camera_transform, attach_to=vehicle) + depth_queue = queue.Queue(maxsize=3) + depth_camera.listen(lambda image: depth_camera_callback(image, depth_queue)) + print("深度相机传感器生成成功!") + except Exception as e: + print(f"深度相机生成失败: {e}") # 6. 生成NPC车辆 print(f"生成 {args.npc_count} 辆NPC车辆...") - npc_vehicles = spawn_npcs(world, count=args.npc_count, ego_vehicle=vehicle) + npc_manager = NPCManager(client) + npc_vehicles = npc_manager.spawn_npcs(world, count=args.npc_count, ego_vehicle=vehicle) if len(npc_vehicles) < args.npc_count // 2: - print( - f"警告:只成功生成了 {len(npc_vehicles)} 辆NPC车辆(目标: {args.npc_count} 辆),可能无法获得足够的检测目标") - else: - print(f"成功生成 {len(npc_vehicles)} 辆NPC车辆") - - # 配置交通管理器 - try: - # 获取交通管理器 - traffic_manager = client.get_trafficmanager() - - # 设置交通管理器端口(通常使用默认端口8000) - tm_port = traffic_manager.get_port() - - # 设置同步模式 - traffic_manager.set_synchronous_mode(True) - - # 设置全局参数 - traffic_manager.set_global_distance_to_leading_vehicle(2.5) - traffic_manager.global_percentage_speed_difference(0.0) - - # 设置所有NPC车辆的交通管理器参数 - for npc in npc_vehicles: - # 重新设置自动驾驶,使用交通管理器 - npc.set_autopilot(True, tm_port) - - # 设置与前方车辆的距离 - traffic_manager.distance_to_leading_vehicle(npc, random.uniform(2.0, 5.0)) - # 设置速度差 - traffic_manager.vehicle_percentage_speed_difference(npc, random.uniform(-10.0, 10.0)) - - print("交通管理器配置完成") - except Exception as e: - print(f"交通管理器配置失败: {e}") + print(f"警告:只成功生成了 {len(npc_vehicles)} 辆NPC车辆") # 等待NPC车辆稳定 print("等待NPC车辆初始化...") @@ -1042,14 +1039,25 @@ def main(): # 7. 加载检测模型和跟踪器 print("加载检测模型和跟踪器...") - model, class_names = load_detection_model(args.model) + try: + model, class_names = load_detection_model(args.model) + except Exception as e: + print(f"模型加载失败: {e}") + return + tracker = Sort() # 8. 主循环 - print("\n开始目标检测与跟踪(按 'q' 键退出程序,按 'r' 重新生成NPC)") + print("\n开始目标检测与跟踪") print("=" * 50) + if args.manual_control: + print("手动控制模式:使用WASD控制车辆") + print("W: 加速, S: 刹车/倒车, A: 左转, D: 右转") + print("Q: 退出程序, R: 重新生成NPC") + else: + print("自动控制模式:车辆将自动行驶") + print("按 'q' 键退出程序,按 'r' 重新生成NPC") - frame_count = 0 detection_stats = {'total_detections': 0, 'total_frames': 0, 'max_vehicles_per_frame': 0} # 性能监控变量 @@ -1064,263 +1072,302 @@ def main(): 'total': [] } + # 手动控制变量 + manual_controls = { + 'throttle': 0.0, + 'brake': 0.0, + 'steer': 0.0, + 'reverse': False + } + while True: - frame_start = time.time() - frame_count += 1 - detection_stats['total_frames'] += 1 + try: + frame_start = time.time() + frame_count += 1 + detection_stats['total_frames'] += 1 - # 同步CARLA世界 - tick_start = time.time() - world.tick() - timings['carla_tick'].append(time.time() - tick_start) - - # 移动视角跟随主车辆 - ego_transform = vehicle.get_transform() - spectator_transform = carla.Transform( - ego_transform.transform(carla.Location(x=-10, z=12)), - carla.Rotation(yaw=ego_transform.rotation.yaw - 180, pitch=-30) - ) - spectator.set_transform(spectator_transform) - - # 获取图像 - image_start = time.time() - if image_queue.empty(): - time.sleep(0.001) - continue + # 同步CARLA世界 + tick_start = time.time() + world.tick() + timings['carla_tick'].append(time.time() - tick_start) + + # 更新NPC行为 + if npc_manager: + npc_manager.update_npc_behavior() - origin_image = image_queue.get() - image = cv2.cvtColor(origin_image, cv2.COLOR_BGR2RGB) # 注意:已经去掉了Alpha通道 - height, width, _ = image.shape - timings['image_get'].append(time.time() - image_start) - - # 获取深度图像 - depth_start = time.time() - depth_image = None - if args.use_depth and depth_queue and not depth_queue.empty(): - depth_image = depth_queue.get() - - if args.show_depth: - # 转换为8位用于显示 - if depth_image.dtype == np.float16: - depth_vis = depth_image.astype(np.float32) + # 移动视角跟随主车辆 + try: + ego_transform = vehicle.get_transform() + spectator_transform = carla.Transform( + ego_transform.transform(carla.Location(x=-10, z=12)), + carla.Rotation(yaw=ego_transform.rotation.yaw - 180, pitch=-30) + ) + spectator.set_transform(spectator_transform) + except: + pass + + # 手动控制 + if args.manual_control: + # 读取键盘输入 + key = cv2.waitKey(1) & 0xFF + + # 控制逻辑 + if key == ord('w'): # 加速 + manual_controls['throttle'] = min(manual_controls['throttle'] + 0.1, 1.0) + manual_controls['brake'] = 0.0 + elif key == ord('s'): # 刹车/倒车 + manual_controls['brake'] = min(manual_controls['brake'] + 0.1, 1.0) + manual_controls['throttle'] = 0.0 + elif key == ord('a'): # 左转 + manual_controls['steer'] = max(manual_controls['steer'] - 0.1, -1.0) + elif key == ord('d'): # 右转 + manual_controls['steer'] = min(manual_controls['steer'] + 0.1, 1.0) else: - depth_vis = depth_image.copy() + # 逐渐回正转向 + if manual_controls['steer'] > 0: + manual_controls['steer'] = max(manual_controls['steer'] - 0.05, 0) + elif manual_controls['steer'] < 0: + manual_controls['steer'] = min(manual_controls['steer'] + 0.05, 0) + + # 逐渐减速 + manual_controls['throttle'] = max(manual_controls['throttle'] - 0.05, 0) + manual_controls['brake'] = max(manual_controls['brake'] - 0.05, 0) + + # 应用手动控制 + control = carla.VehicleControl() + control.throttle = manual_controls['throttle'] + control.brake = manual_controls['brake'] + control.steer = manual_controls['steer'] + control.hand_brake = False + control.reverse = manual_controls['reverse'] + try: + vehicle.apply_control(control) + except: + pass + + # 获取图像 + image_start = time.time() + if image_queue.empty(): + time.sleep(0.001) + continue + + origin_image = image_queue.get() + image = cv2.cvtColor(origin_image, cv2.COLOR_BGR2RGB) + height, width, _ = image.shape + timings['image_get'].append(time.time() - image_start) + + # 获取深度图像 + depth_start = time.time() + depth_image = None + if args.use_depth and depth_queue and not depth_queue.empty(): + depth_image = depth_queue.get() + + if args.show_depth: + if depth_image.dtype == np.float16: + depth_vis = depth_image.astype(np.float32) + else: + depth_vis = depth_image.copy() - depth_vis = cv2.normalize(depth_vis, None, 0, 255, cv2.NORM_MINMAX) - depth_vis = depth_vis.astype(np.uint8) - depth_vis = cv2.applyColorMap(depth_vis, cv2.COLORMAP_JET) - cv2.imshow('Depth Image', depth_vis) - timings['depth_get'].append(time.time() - depth_start) + depth_vis = cv2.normalize(depth_vis, None, 0, 255, cv2.NORM_MINMAX) + depth_vis = depth_vis.astype(np.uint8) + depth_vis = cv2.applyColorMap(depth_vis, cv2.COLORMAP_JET) + cv2.imshow('Depth Image', depth_vis) + timings['depth_get'].append(time.time() - depth_start) - # 目标检测 - detection_start = time.time() - boxes, labels, probs, depths = [], [], [], [] + # 目标检测 + detection_start = time.time() + boxes, labels, probs, depths = [], [], [], [] - try: - results = model(image, conf=args.conf_thres, iou=args.iou_thres, - device=device, imgsz=640, verbose=False) - - for r in results: - for box in r.boxes: - x1, y1, x2, y2 = box.xyxy[0].cpu().numpy() - conf = box.conf[0].cpu().numpy() - cls = int(box.cls[0].cpu().numpy()) - - # 只保留车辆相关类别 - if cls in [2, 3, 5, 7]: - box_width = x2 - x1 - box_height = y2 - y1 - - # 尺寸过滤 - min_size = 6 - if depth_image is not None: - rough_distance = get_target_distance(depth_image, [x1, y1, x2, y2]) - if rough_distance > 30: - min_size = 4 - elif rough_distance > 50: - min_size = 2 - else: - min_size = 8 - - aspect_ratio = box_width / max(box_height, 1) - - if (box_width > min_size and box_height > min_size and - 0.3 < aspect_ratio < 3.0): - boxes.append([x1, y1, x2, y2]) - labels.append(cls) - probs.append(conf) - - # 计算目标距离 + try: + results = model(image, conf=args.conf_thres, iou=args.iou_thres, + device=device, imgsz=640, verbose=False) + + for r in results: + for box in r.boxes: + x1, y1, x2, y2 = box.xyxy[0].cpu().numpy() + conf = box.conf[0].cpu().numpy() + cls = int(box.cls[0].cpu().numpy()) + + # 只保留车辆相关类别 + if cls in [2, 3, 5, 7]: # 车辆类别 + box_width = x2 - x1 + box_height = y2 - y1 + + # 尺寸过滤 + min_size = 6 if depth_image is not None: - dist = get_target_distance(depth_image, [x1, y1, x2, y2], use_median=True) - depths.append(dist) - except Exception as e: - print(f"检测模型推理出错: {e}") - - timings['detection'].append(time.time() - detection_start) - - # 更新检测统计 - detection_stats['total_detections'] += len(boxes) - detection_stats['max_vehicles_per_frame'] = max( - detection_stats['max_vehicles_per_frame'], len(boxes) - ) - - # 目标跟踪 - tracking_start = time.time() - if boxes: - boxes_np = np.array(boxes, dtype=np.float32) - probs_np = np.array(probs, dtype=np.float32).reshape(-1, 1) - dets = np.hstack([boxes_np, probs_np]) if probs_np.size > 0 else boxes_np - - if depths: - tracker.set_depths(depths) - - track_results = tracker.update(dets) - - if track_results.size > 0: - track_boxes = [] - track_ids = [] - track_distances = [] - track_velocities = [] - - for track in track_results: - x1, y1, x2, y2, track_id = track - track_boxes.append([x1, y1, x2, y2]) - track_ids.append(int(track_id)) - - # 获取距离和速度 - track_obj = next((t for t in tracker.tracks if t.id == track_id), None) - if track_obj: - track_distances.append(track_obj.get_distance()) - track_velocities.append(track_obj.velocity) - else: - track_distances.append(None) - track_velocities.append(None) - - # 绘制跟踪结果 - drawing_start = time.time() - if track_boxes: - image = draw_bounding_boxes( - image, track_boxes, - labels=[2] * len(track_boxes), - class_names=class_names, - track_ids=track_ids, - probs=[0.9] * len(track_boxes), - distances=track_distances, - velocities=track_velocities - ) - - cv2.putText(image, f'Vehicles: {len(track_boxes)}', (width - 200, 30), - cv2.FONT_HERSHEY_SIMPLEX, 0.7, (0, 255, 255), 2) - timings['drawing'].append(time.time() - drawing_start) - else: - timings['tracking'].append(0.0) - timings['drawing'].append(0.0) - - timings['tracking'].append(time.time() - tracking_start) - - # 计算FPS - total_time = time.time() - frame_start - fps = 1.0 / total_time if total_time > 0 else 0 - timings['total'].append(total_time) - - # 绘制性能监控面板 - image = draw_performance_panel(image, timings, fps, frame_count) - - # 显示其他信息 - info = [ - f"FPS: {fps:.1f}", - f"Frame: {frame_count}", - f"Tracks: {len(tracker.tracks)}", - f"Detections: {len(boxes)}", - f"Model: {args.model}", - "Press 'q' to quit" - ] - - # 调整信息位置,避免与性能面板重叠 - y_pos = 220 - for line in info: - cv2.putText(image, line, (10, y_pos), - cv2.FONT_HERSHEY_SIMPLEX, 0.6, (0, 255, 255), 2) - y_pos += 25 - - # 显示结果 - display_start = time.time() - window_name = f'CARLA {args.model} + {args.tracker}' - cv2.imshow(window_name, cv2.cvtColor(image, cv2.COLOR_RGB2BGR)) - timings['display'].append(time.time() - display_start) - - # 每50帧打印一次性能统计 - if frame_count % 50 == 0 and frame_count > 0: - print("\n" + "=" * 60) - print("性能监控统计 (最近50帧平均值):") - print("-" * 60) - print(f"CARLA同步: {np.mean(timings['carla_tick'][-50:]):.4f}s") - print(f"图像获取: {np.mean(timings['image_get'][-50:]):.4f}s") - print(f"深度获取: {np.mean(timings['depth_get'][-50:]):.4f}s") - print(f"目标检测: {np.mean(timings['detection'][-50:]):.4f}s (瓶颈)") - print(f"目标跟踪: {np.mean(timings['tracking'][-50:]):.4f}s") - print(f"结果绘制: {np.mean(timings['drawing'][-50:]):.4f}s") - print(f"显示输出: {np.mean(timings['display'][-50:]):.4f}s") - print(f"总帧时间: {np.mean(timings['total'][-50:]):.4f}s") - print(f"理论FPS: {1.0 / np.mean(timings['total'][-50:]):.1f}") - print("=" * 60) - - # 自动识别瓶颈并提供建议 - bottleneck_times = { - '检测': np.mean(timings['detection'][-50:]), - '跟踪': np.mean(timings['tracking'][-50:]), - '绘制': np.mean(timings['drawing'][-50:]), - 'CARLA': np.mean(timings['carla_tick'][-50:]) - } - bottleneck = max(bottleneck_times, key=bottleneck_times.get) - if bottleneck_times[bottleneck] > 0.05: # 超过50ms - print(f"⚠️ 性能瓶颈: {bottleneck} ({bottleneck_times[bottleneck]:.3f}s)") - if bottleneck == '检测': - print("建议: 降低模型分辨率或使用更轻量模型") - elif bottleneck == '跟踪': - print("建议: 减少跟踪目标数量或简化跟踪算法") - elif bottleneck == '绘制': - print("建议: 简化绘制内容或减少信息显示") - elif bottleneck == 'CARLA': - print("建议: 减少NPC数量或降低仿真频率") - - # 退出检查 - key = cv2.waitKey(1) & 0xFF - if key == ord('q'): - print("用户触发退出程序...") - break - elif key == ord('r'): - # 重新生成NPC - print("重新生成NPC车辆...") - for actor in world.get_actors().filter('vehicle.*'): - if actor != vehicle: - try: - actor.destroy() - except: - pass - npc_vehicles = spawn_npcs(world, count=args.npc_count, ego_vehicle=vehicle) - print("NPC重新生成完成") + rough_distance = get_target_distance(depth_image, [x1, y1, x2, y2]) + if rough_distance > 30: + min_size = 4 + elif rough_distance > 50: + min_size = 2 + else: + min_size = 8 + + aspect_ratio = box_width / max(box_height, 1) + + if (box_width > min_size and box_height > min_size and + 0.3 < aspect_ratio < 3.0): + boxes.append([x1, y1, x2, y2]) + labels.append(cls) + probs.append(conf) + + # 计算目标距离 + if depth_image is not None: + dist = get_target_distance(depth_image, [x1, y1, x2, y2], use_median=True) + depths.append(dist) + except Exception as e: + print(f"检测模型推理出错: {e}") - # 重新配置交通管理器 - try: - traffic_manager = client.get_trafficmanager() - tm_port = traffic_manager.get_port() - traffic_manager.set_synchronous_mode(True) + timings['detection'].append(time.time() - detection_start) - for npc in npc_vehicles: - npc.set_autopilot(True, tm_port) - traffic_manager.distance_to_leading_vehicle(npc, random.uniform(2.0, 5.0)) - traffic_manager.vehicle_percentage_speed_difference(npc, random.uniform(-10.0, 10.0)) + # 更新检测统计 + detection_stats['total_detections'] += len(boxes) + detection_stats['max_vehicles_per_frame'] = max( + detection_stats['max_vehicles_per_frame'], len(boxes) + ) - print("交通管理器重新配置完成") - except Exception as e: - print(f"交通管理器重新配置失败: {e}") + # 目标跟踪 + tracking_start = time.time() + if boxes: + boxes_np = np.array(boxes, dtype=np.float32) + probs_np = np.array(probs, dtype=np.float32).reshape(-1, 1) + dets = np.hstack([boxes_np, probs_np]) if probs_np.size > 0 else boxes_np + + if depths: + tracker.set_depths(depths) + + track_results = tracker.update(dets) + + if track_results.size > 0: + track_boxes = [] + track_ids = [] + track_distances = [] + track_velocities = [] + + for track in track_results: + x1, y1, x2, y2, track_id = track + track_boxes.append([x1, y1, x2, y2]) + track_ids.append(int(track_id)) + + # 获取距离和速度 + track_obj = next((t for t in tracker.tracks if t.id == track_id), None) + if track_obj: + track_distances.append(track_obj.get_distance()) + track_velocities.append(track_obj.velocity) + else: + track_distances.append(None) + track_velocities.append(None) + + # 绘制跟踪结果 + drawing_start = time.time() + if track_boxes: + image = draw_bounding_boxes( + image, track_boxes, + labels=[2] * len(track_boxes), + class_names=class_names, + track_ids=track_ids, + probs=[0.9] * len(track_boxes), + distances=track_distances, + velocities=track_velocities + ) + + cv2.putText(image, f'Vehicles: {len(track_boxes)}', (width - 200, 30), + cv2.FONT_HERSHEY_SIMPLEX, 0.7, (0, 255, 255), 2) + + # 自动控制模式下的障碍物检测 + if not args.manual_control and track_distances: + # 找到最近的车 + valid_distances = [d for d in track_distances if d is not None] + if valid_distances: + closest_distance = min(valid_distances) + if closest_distance < 20: # 20米内有障碍物 + # 可以在这里添加减速逻辑 + pass + + timings['drawing'].append(time.time() - drawing_start) + else: + timings['tracking'].append(0.0) + timings['drawing'].append(0.0) + + timings['tracking'].append(time.time() - tracking_start) + + # 计算FPS + total_time = time.time() - frame_start + fps = 1.0 / total_time if total_time > 0 else 0 + timings['total'].append(total_time) + + # 绘制性能监控面板 + image = draw_performance_panel(image, timings, fps, frame_count) + + # 显示其他信息 + info = [ + f"FPS: {fps:.1f}", + f"Frame: {frame_count}", + f"Tracks: {len(tracker.tracks)}", + f"Detections: {len(boxes)}", + f"Model: {args.model}", + "Press 'q' to quit, 'r' to reset NPCs" + ] + + # 调整信息位置 + y_pos = 220 + for line in info: + cv2.putText(image, line, (10, y_pos), + cv2.FONT_HERSHEY_SIMPLEX, 0.6, (0, 255, 255), 2) + y_pos += 25 + + # 显示手动控制状态 + if args.manual_control: + try: + velocity = vehicle.get_velocity() + speed = math.sqrt(velocity.x ** 2 + velocity.y ** 2) * 3.6 # m/s to km/h + control_info = [ + f"Speed: {speed:.1f} km/h", + f"Throttle: {manual_controls['throttle']:.1f}", + f"Brake: {manual_controls['brake']:.1f}", + f"Steer: {manual_controls['steer']:.1f}" + ] + for i, line in enumerate(control_info): + cv2.putText(image, line, (width - 200, 60 + i * 25), + cv2.FONT_HERSHEY_SIMPLEX, 0.6, (255, 100, 100), 2) + except: + pass + + # 显示结果 + display_start = time.time() + window_name = f'CARLA Detection & Tracking - {"Manual" if args.manual_control else "Auto"}' + cv2.imshow(window_name, cv2.cvtColor(image, cv2.COLOR_RGB2BGR)) + timings['display'].append(time.time() - display_start) + + # 每100帧打印一次性能统计 + if frame_count % 100 == 0 and frame_count > 0: + print( + f"\n[帧数 {frame_count}] FPS: {fps:.1f} | 检测到车辆: {len(boxes)} | 跟踪目标: {len(tracker.tracks)}") + + # 退出检查 + key = cv2.waitKey(1) & 0xFF + if key == ord('q'): + print("用户触发退出程序...") + break + elif key == ord('r'): + # 重新生成NPC + print("重新生成NPC车辆...") + if npc_manager: + npc_manager.destroy_all_npcs() + npc_vehicles = npc_manager.spawn_npcs(world, count=args.npc_count, ego_vehicle=vehicle) + print(f"重新生成 {len(npc_vehicles)} 辆NPC车辆完成") + + # FPS控制 + elapsed = time.time() - frame_start + if elapsed < 0.05: + time.sleep(max(0, 0.05 - elapsed)) - # FPS控制 - elapsed = time.time() - frame_start - if elapsed < 0.05: - time.sleep(0.05 - elapsed) + except Exception as e: + print(f"主循环出错: {e}") + import traceback + traceback.print_exc() + break except KeyboardInterrupt: print("\n用户中断程序...") @@ -1332,7 +1379,7 @@ def main(): # 清理资源 print("\n正在清理资源...") - # 先停止传感器监听 + # 停止传感器 if camera: try: camera.stop() @@ -1347,18 +1394,25 @@ def main(): except: pass - # 清理队列中的残留数据 - if image_queue: - while not image_queue.empty(): - try: - image_queue.get_nowait() - except: - break + # 销毁NPC + if npc_manager: + npc_manager.destroy_all_npcs() + + # 销毁主车辆 + if vehicle: + try: + vehicle.destroy() + except: + pass + # 恢复世界设置 if world: - settings = world.get_settings() - settings.synchronous_mode = False - world.apply_settings(settings) + try: + settings = world.get_settings() + settings.synchronous_mode = False + world.apply_settings(settings) + except: + pass cv2.destroyAllWindows() @@ -1371,21 +1425,17 @@ def main(): print("程序运行统计:") print(f"总帧数: {frame_count}") - # 性能统计总结 - if timings['total']: - print(f"平均FPS: {1.0 / np.mean(timings['total']):.1f}") - print("\n性能时间分布:") - print(f"CARLA同步: {np.mean(timings['carla_tick']):.4f}s") - print(f"图像获取: {np.mean(timings['image_get']):.4f}s") - print(f"深度获取: {np.mean(timings['depth_get']):.4f}s") - print(f"目标检测: {np.mean(timings['detection']):.4f}s") - print(f"目标跟踪: {np.mean(timings['tracking']):.4f}s") - print(f"结果绘制: {np.mean(timings['drawing']):.4f}s") - print(f"显示输出: {np.mean(timings['display']):.4f}s") - - print(f"总检测次数: {detection_stats['total_detections']}") - print(f"平均每帧检测: {detection_stats['total_detections'] / max(1, detection_stats['total_frames']):.1f}") - print(f"最大单帧车辆数: {detection_stats['max_vehicles_per_frame']}") + if timings.get('total'): + try: + avg_fps = 1.0 / np.mean(list(timings['total'])) if timings['total'] else 0 + print(f"平均FPS: {avg_fps:.1f}") + except: + pass + + if detection_stats: + print(f"总检测次数: {detection_stats['total_detections']}") + print(f"平均每帧检测: {detection_stats['total_detections'] / max(1, detection_stats['total_frames']):.1f}") + print(f"最大单帧车辆数: {detection_stats['max_vehicles_per_frame']}") print("=" * 50) print("资源清理完成,程序正常退出!")