diff --git a/src/carla_tracking/main.py b/src/carla_tracking/main.py index 21a707e690..c81816ae6d 100644 --- a/src/carla_tracking/main.py +++ b/src/carla_tracking/main.py @@ -239,6 +239,9 @@ def _should_keep_track(self, track): return track.age <= self.max_age def _iou_batch(self, b1, b2): + if b1.shape[0] == 0 or b2.shape[0] == 0: + return np.zeros((b1.shape[0], b2.shape[0]), dtype=np.float32) + b1_x1, b1_y1, b1_x2, b1_y2 = b1[:, 0], b1[:, 1], b1[:, 2], b1[:, 3] b2_x1, b2_y1, b2_x2, b2_y2 = b2[:, 0], b2[:, 1], b2[:, 2], b2[:, 3] @@ -348,6 +351,9 @@ 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): x1, y1, x2, y2 = map(int, box) @@ -359,38 +365,37 @@ def draw_bounding_boxes(image, boxes, labels, class_names, **kwargs): if x1 >= x2 or y1 >= y2: continue + # 颜色计算 color = (0, 255, 0) if distances and i < len(distances) and distances[i]: dist = distances[i] - if dist < 15: - r, g = 255, int(255 * (dist / 15)) - color = (0, g, r) - elif dist < 30: - r, g = int(255 * (1 - (dist - 15) / 15)), 255 - color = (0, g, r) - elif dist < 50: - b, g = int(255 * ((dist - 30) / 20)), 255 - color = (b, g, 0) + if dist not in color_cache: + if dist < 15: + r, g = 255, int(255 * (dist / 15)) + color_cache[dist] = (0, g, r) + elif dist < 30: + r, g = int(255 * (1 - (dist - 15) / 15)), 255 + color_cache[dist] = (0, g, r) + elif dist < 50: + b, g = int(255 * ((dist - 30) / 20)), 255 + color_cache[dist] = (b, g, 0) + else: + color_cache[dist] = (0, 255, 0) + color = color_cache[dist] cv2.rectangle(result, (x1, y1), (x2, y2), color, 2) - text_parts = [] - if i < len(labels): - text_parts.append(class_names.get(labels[i], f"cls{labels[i]}")) - - if probs and i < len(probs): - text_parts.append(f"{probs[i]:.2f}") - - if track_ids and i < len(track_ids): - text_parts.append(f"ID:{track_ids[i]}") - - if distances and i < len(distances) and distances[i]: - text_parts.append(f"D:{distances[i]:.1f}m") - - if velocities and i < len(velocities) and velocities[i]: - text_parts.append(f"S:{velocities[i]:.1f}m/s") + # 构建文本信息 + text_parts = [ + class_names.get(labels[i], f"cls{labels[i]}") if i < len(labels) else "", + f"{probs[i]:.2f}" if probs and i < len(probs) else "", + f"ID:{track_ids[i]}" if track_ids and i < len(track_ids) else "", + f"D:{distances[i]:.1f}m" if distances and i < len(distances) and distances[i] else "", + f"S:{velocities[i]:.1f}m/s" if velocities and i < len(velocities) and velocities[i] else "" + ] - label_text = " ".join(text_parts) + # 过滤空字符串并拼接 + label_text = " ".join(filter(None, text_parts)) text_size = cv2.getTextSize(label_text, cv2.FONT_HERSHEY_SIMPLEX, 0.5, 2)[0] text_bg_y1 = max(0, y1 - text_size[1] - 5) @@ -561,144 +566,83 @@ def spawn_npcs(world, count=30, ego_vehicle=None): print(f"正在生成 {count} 辆NPC车辆...") + # 先检查当前已有的车辆 + existing_vehicles = world.get_actors().filter('vehicle.*') + print(f"当前已有车辆数量: {len(existing_vehicles)}") + bp_lib = world.get_blueprint_library() - # 可用的车辆类型 - vehicle_names = [ - 'vehicle.audi.a2', - 'vehicle.audi.tt', - 'vehicle.toyota.prius', - 'vehicle.volkswagen.t2', - 'vehicle.nissan.patrol', - 'vehicle.mercedes.coupe', - 'vehicle.dodge.charger_police', - 'vehicle.ford.mustang', - 'vehicle.mini.cooperst', - 'vehicle.nissan.micra', - 'vehicle.seat.leon' - ] - - vehicle_bps = [] - for name in vehicle_names: - try: - bp = bp_lib.find(name) - if bp: - vehicle_bps.append(bp) - except: - continue + # 创建一个包含所有车辆蓝图的列表 + vehicle_blueprints = list(bp_lib.filter('vehicle.*')) + if not vehicle_blueprints: + print("错误:没有找到任何车辆蓝图!") + return [] - if not vehicle_bps: - vehicle_bps = [bp for bp in bp_lib.filter('vehicle.*')] + 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 = [] - # 获取主车辆前方的道路信息 - if ego_transform: - ego_yaw = ego_transform.rotation.yaw - print(f"主车辆朝向: {ego_yaw:.1f}度") - - # 按距离排序生成点(如果主车辆存在) - if ego_location: - spawn_points_with_dist = [] - for i, spawn_point in enumerate(spawn_points): - dist = math.hypot( - spawn_point.location.x - ego_location.x, - spawn_point.location.y - ego_location.y - ) - - # 计算与主车辆方向的角度差 - if ego_transform: - spawn_yaw = spawn_point.rotation.yaw - angle_diff = abs((spawn_yaw - ego_yaw + 180) % 360 - 180) - - # 优先选择主车辆前方的点 - if dist < 100: # 100米范围内 - if angle_diff < 90: # 方向大致相同 - priority = dist * 0.5 # 前方点权重更高 - else: - priority = dist * 2.0 # 后方点权重较低 - else: - priority = dist * 1.5 - else: - priority = dist - - spawn_points_with_dist.append((spawn_point, dist, priority)) - - # 按优先级排序(优先选择近的、主车辆前方的点) - spawn_points_with_dist.sort(key=lambda x: x[2]) - sorted_spawn_points = [sp[0] for sp in spawn_points_with_dist] - - print(f"找到了 {len(spawn_points)} 个生成点,优先选择 {min(50, len(sorted_spawn_points))} 个最近的") - else: - random.shuffle(spawn_points) - sorted_spawn_points = spawn_points + # 只尝试每个生成点一次,避免无限循环 + random.shuffle(spawn_points) - # 第一阶段:在主车辆20-80米内生成 - print("第一阶段:在主车辆20-80米内生成...") - for spawn_point in sorted_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 ) - # 只在20-80米范围内生成 - if dist_to_ego < 20.0 or dist_to_ego > 80.0: + # 只生成在合理距离范围内的车辆(30-80米) + if dist_to_ego < 30.0 or dist_to_ego > 80.0: continue - # 检查是否太靠近其他车辆(包括主车辆) + # 检查是否太靠近已生成的NPC too_close = False - - # 检查与主车辆的距离 - if ego_location: - dist_to_ego = math.hypot( - spawn_point.location.x - ego_location.x, - spawn_point.location.y - ego_location.y - ) - if dist_to_ego < 10.0: # 不能太靠近主车辆 - too_close = True - - # 检查与已生成NPC的距离 - if not too_close: - for npc in npc_vehicles: + for npc in npc_vehicles: + try: npc_loc = npc.get_location() dist = math.hypot( npc_loc.x - spawn_point.location.x, npc_loc.y - spawn_point.location.y ) - if dist < 8.0: # NPC之间保持8米距离 + if dist < 5.0: # 5米内不生成 too_close = True break + except: + continue if too_close: continue try: - vehicle_bp = random.choice(vehicle_bps) + # 随机选择一个车辆蓝图 + 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)) - # 设置NPC速度限制 - if vehicle_bp.has_attribute('speed'): - max_speed = random.uniform(30.0, 70.0) # 30-70 km/h - vehicle_bp.set_attribute('speed', str(max_speed)) - + # 尝试生成车辆 npc = world.try_spawn_actor(vehicle_bp, spawn_point) if npc: @@ -711,92 +655,48 @@ def spawn_npcs(world, count=30, ego_vehicle=None): spawn_point.location.x - ego_location.x, spawn_point.location.y - ego_location.y ) - direction = "前方" if abs(spawn_point.rotation.yaw - ego_yaw) < 90 else "后方" - print(f"生成NPC {spawned_count}/{count} - 距离主车辆: {dist_to_ego:.1f}米 ({direction})") + print(f"✓ 生成NPC {spawned_count}/{count} - 距离: {dist_to_ego:.1f}米") else: - print(f"生成NPC {spawned_count}/{count}") - + print(f"✓ 生成NPC {spawned_count}/{count}") except Exception as e: print(f"生成NPC失败: {e}") continue - # 第二阶段:如果数量不够,放宽距离限制 - if spawned_count < count: - print(f"第一阶段生成 {spawned_count} 辆,开始第二阶段生成 (放宽距离限制)...") + print(f"最终生成 {spawned_count} 辆NPC车辆") - for spawn_point in sorted_spawn_points: - if spawned_count >= count: - break + # 验证生成的车辆 + valid_npcs = [] + for npc in npc_vehicles: + try: + loc = npc.get_location() + if loc: + valid_npcs.append(npc) + except: + print(f"警告:无法获取NPC位置,可能已销毁") - # 跳过已经检查过的生成点 - skip = False - for npc in npc_vehicles: + print(f"有效NPC车辆: {len(valid_npcs)} 辆") + + # 打印距离统计 + if ego_location and valid_npcs: + distances = [] + for npc in valid_npcs: + try: npc_loc = npc.get_location() dist = math.hypot( - npc_loc.x - spawn_point.location.x, - npc_loc.y - spawn_point.location.y - ) - if dist < 8.0: - skip = True - break - - if skip: - continue - - if ego_location: - dist_to_ego = math.hypot( - spawn_point.location.x - ego_location.x, - spawn_point.location.y - ego_location.y + npc_loc.x - ego_location.x, + npc_loc.y - ego_location.y ) - # 第二阶段放宽到10-100米 - if dist_to_ego < 10.0 or dist_to_ego > 100.0: - continue - - try: - vehicle_bp = random.choice(vehicle_bps) - - 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: - npc.set_autopilot(True) - npc_vehicles.append(npc) - spawned_count += 1 - - 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}") - except Exception as e: - print(f"生成NPC失败: {e}") + distances.append(dist) + except: continue - print(f"成功生成 {spawned_count} 辆NPC车辆") - - # 打印生成统计 - if ego_location and npc_vehicles: - distances = [] - for npc in npc_vehicles: - npc_loc = npc.get_location() - dist = math.hypot( - npc_loc.x - ego_location.x, - npc_loc.y - ego_location.y - ) - distances.append(dist) - if distances: print( f"NPC距离统计: 最近{min(distances):.1f}米, 最远{max(distances):.1f}米, 平均{np.mean(distances):.1f}米") + else: + print("警告:无法计算NPC距离") - return npc_vehicles + return valid_npcs # 优化内存占用:使用更高效的回调函数 @@ -805,7 +705,18 @@ def camera_callback(image, rgb_image_queue): # 使用array接口避免额外复制,只取RGB通道 array = np.frombuffer(image.raw_data, dtype=np.uint8) array = array.reshape((image.height, image.width, 4)) - rgb_image_queue.put(array[..., :3]) # 只存储RGB通道,丢弃Alpha通道 + 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) def depth_camera_callback(image, depth_queue): @@ -826,7 +737,17 @@ def depth_camera_callback(image, depth_queue): # 预处理深度图像 depth_in_meters = preprocess_depth_image(depth_in_meters) - depth_queue.put(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) # -------------------------- 主函数 -------------------------- @@ -967,16 +888,29 @@ def main(): print("=" * 50) frame_count = 0 - fps_history = deque(maxlen=30) detection_stats = {'total_detections': 0, 'total_frames': 0, 'max_vehicles_per_frame': 0} + # 性能监控变量 + timings = { + 'carla_tick': [], + 'image_get': [], + 'depth_get': [], + 'detection': [], + 'tracking': [], + 'drawing': [], + 'display': [], + 'total': [] + } + while True: - start_time = time.time() + 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() @@ -987,6 +921,7 @@ def main(): spectator.set_transform(spectator_transform) # 获取图像 + image_start = time.time() if image_queue.empty(): time.sleep(0.001) continue @@ -994,8 +929,10 @@ def main(): 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() @@ -1011,8 +948,10 @@ def main(): 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 = [], [], [], [] try: @@ -1056,6 +995,8 @@ def main(): 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( @@ -1063,6 +1004,7 @@ def main(): ) # 目标跟踪 + 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) @@ -1094,6 +1036,7 @@ def main(): track_velocities.append(None) # 绘制跟踪结果 + drawing_start = time.time() if track_boxes: image = draw_bounding_boxes( image, track_boxes, @@ -1107,15 +1050,21 @@ def main(): 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 - fps = 1.0 / (time.time() - start_time) - fps_history.append(fps) - avg_fps = np.mean(fps_history) if fps_history else fps + total_time = time.time() - frame_start + fps = 1.0 / total_time if total_time > 0 else 0 + timings['total'].append(total_time) # 显示信息 info = [ - f"FPS: {avg_fps:.1f}", + f"FPS: {fps:.1f}", f"Frame: {frame_count}", f"Tracks: {len(tracker.tracks)}", f"Detections: {len(boxes)}", @@ -1130,13 +1079,45 @@ def main(): 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)) - - # 每30帧打印一次统计信息 - if frame_count % 30 == 0: - print( - f"[Frame {frame_count}] FPS: {avg_fps:.1f}, Detections: {len(boxes)}, Tracks: {len(tracker.tracks)}") + 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 @@ -1171,7 +1152,7 @@ def main(): print(f"交通管理器重新配置失败: {e}") # FPS控制 - elapsed = time.time() - start_time + elapsed = time.time() - frame_start if elapsed < 0.05: time.sleep(0.05 - elapsed) @@ -1185,18 +1166,29 @@ def main(): # 清理资源 print("\n正在清理资源...") + # 先停止传感器监听 if camera: try: + camera.stop() camera.destroy() except: pass if depth_camera: try: + depth_camera.stop() depth_camera.destroy() except: pass + # 清理队列中的残留数据 + if image_queue: + while not image_queue.empty(): + try: + image_queue.get_nowait() + except: + break + if world: settings = world.get_settings() settings.synchronous_mode = False @@ -1204,11 +1196,27 @@ def main(): cv2.destroyAllWindows() + # 清理PyTorch缓存 + if torch.cuda.is_available(): + torch.cuda.empty_cache() + # 打印最终统计 print("\n" + "=" * 50) print("程序运行统计:") print(f"总帧数: {frame_count}") - print(f"平均FPS: {np.mean(fps_history) if fps_history else 0:.1f}") + + # 性能统计总结 + 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']}")