diff --git a/src/2d-carla-tracking b/src/2d-carla-tracking new file mode 160000 index 0000000000..713a7a59df --- /dev/null +++ b/src/2d-carla-tracking @@ -0,0 +1 @@ +Subproject commit 713a7a59dff78b269ea8cd2c2e140f103a7f24c8 diff --git a/src/tracking_car/README.md b/src/tracking_car/README.md index 7b4ccc6155..e69de29bb2 100644 --- a/src/tracking_car/README.md +++ b/src/tracking_car/README.md @@ -1,182 +0,0 @@ -```markdown -# 基于CARLA的智能代理系统 - -## 项目概述 - -本项目旨在利用神经网络技术,实现CARLA模拟器中车辆和行人的全栈智能代理,涵盖感知、规划与控制三大核心模块。通过深度学习算法赋予虚拟智能体环境理解、决策规划和动态控制能力,同时包含具身人仿真、机械臂控制等扩展功能,构建多智能体协同的仿真系统。 - -## 环境配置 - -* **支持平台**:Windows 10/11,Ubuntu 20.04/22.04 -* **核心软件**: - * Python 3.7-3.12(需兼容3.7版本) - * PyTorch(优先采用,不依赖TensorFlow) - * CARLA 0.9.11+(推荐0.9.13/0.9.15版本) -* **依赖安装**: - ```bash - # 基础依赖 - pip install -r requirements.txt -i http://mirrors.aliyun.com/pypi/simple --trusted-host mirrors.aliyun.com - - # CARLA客户端安装(需替换为对应版本) - pip install carla==0.9.15 - - # 文档生成工具 - pip install mkdocs - ``` - -## 文档生成 - -1. 安装文档工具链: - ```bash - pip install mkdocs -i http://mirrors.aliyun.com/pypi/simple --trusted-host mirrors.aliyun.com - pip install -r requirements.txt - ``` - -2. 构建并预览文档: - ```bash - # 进入项目根目录 - cd nn - - # 构建静态文档 - mkdocs build - - # 启动本地文档服务 - mkdocs serve - ``` - -3. 浏览器访问 [http://127.0.0.1:8000](http://127.0.0.1:8000) 查看文档。 - -## 核心功能模块 - -1. **车辆智能代理** - * **感知系统**:基于CNN的目标检测(车辆、行人、交通信号灯)、车道线识别 - * **规划系统**:全局路径规划(A*、RRT*算法)、局部避障 - * **控制系统**:强化学习车辆控制(油门、刹车、转向)、PID参数自适应调优 - * **手动控制**:支持键盘操作的车辆交互模式(WASD控制方向,空格/左Shift控制升降) - -2. **具身人仿真** - * **感知模块**:william开发的具身人环境感知系统(`humanoid_perception`) - * **运动模拟**:基于Mujoco的物理引擎实现具身人运动控制(`Mujoco_manrun`) - -3. **神经网络模型** - * **CNN模型**:图像识别与目标检测(`chap05_CNN`),包含完整训练流程(Adam优化器、交叉熵损失) - * **RNN模型**:序列数据处理与生成(`chap06_RNN`),基于LSTM实现唐诗生成等文本任务 - * **FNN模型**:基础神经网络结构(`chap04_simple_neural_network`),包含测试评估函数 - -4. **辅助系统** - * **车道辅助**:Active-Lane-Keeping-Assistant实现车道偏离预警与辅助 - * **机械臂测试**:humantest模块提供机械臂力控仿真与交互功能 - -## 快速启动 - -1. 启动CARLA服务器: - ```bash - # Linux - ./CarlaUE4.sh - - # Windows - CarlaUE4.exe - ``` - -2. 运行示例场景: - ```bash - # 自动驾驶车辆(强化学习) - python src/driverless_car/main.py - - # 车辆手动控制 - python src/manual_control/main.py - - # CNN模型训练 - python src/chap05_CNN/CNN_pytorch.py - - # RNN文本生成 - python src/chap06_RNN/tangshi_for_pytorch/rnn.py - ``` - -## 关键代码说明 - -### 神经网络训练框架 -```python -# CNN训练示例(chap05_CNN/CNN_pytorch.py) -def train(cnn): - optimizer = torch.optim.Adam(cnn.parameters(), lr=learning_rate) - loss_func = nn.CrossEntropyLoss() - - for epoch in range(max_epoch): - for step, (x_, y_) in enumerate(train_loader): - x, y = Variable(x_), Variable(y_) - output = cnn(x) - loss = loss_func(output, y) - optimizer.zero_grad(set_to_none=True) - loss.backward() - optimizer.step() - - if step != 0 and step % 20 == 0: - print(f"测试准确率: {test(cnn)}") -``` - -### 强化学习智能体 -```python -# 车辆强化学习(driverless_car/main.py) -def main(): - env = DroneEnv() - agent = Agent() - episodes = 1000 - epsilon = 1.0 # 探索率 - - for episode in range(episodes): - state = env.reset() - total_reward = 0 - - while True: - action = agent.get_action(state, epsilon) # 根据状态和探索率获取动作 - next_state, reward, done = env.step(action) - agent.remember(state, action, reward, next_state, done) # 存储经验 - agent.train(batch_size=32) # 训练模型 - - if done: - epsilon = max(0.01, epsilon * 0.995) # 衰减探索率 - break -``` - -### RNN模型结构 -```python -# 唐诗生成RNN(chap06_RNN/tangshi_for_pytorch/rnn.py) -class RNN_model(nn.Module): - def __init__(self, batch_sz, vocab_len, word_embedding, embedding_dim, lstm_hidden_dim): - super(RNN_model, self).__init__() - self.word_embedding_lookup = word_embedding - self.rnn_lstm = nn.LSTM(input_size=embedding_dim, - hidden_size=lstm_hidden_dim, - num_layers=2, - batch_first=False) - self.fc = nn.Linear(lstm_hidden_dim, vocab_len) - self.softmax = nn.LogSoftmax(dim=1) - - def forward(self, sentence, is_test=False): - batch_input = self.word_embedding_lookup(sentence).view(1, -1, self.word_embedding_dim) - h0 = torch.zeros(2, batch_input.size(1), self.lstm_dim) - c0 = torch.zeros(2, batch_input.size(1), self.lstm_dim) - - output, (hn, cn) = self.rnn_lstm(batch_input, (h0, c0)) - out = self.fc(output.contiguous().view(-1, self.lstm_dim)) - return self.softmax(out) -``` - -## 贡献指南 - -请在提交代码前阅读 [贡献指南](https://github.com/OpenHUTB/.github/blob/master/CONTRIBUTING.md),代码优化方向包括: - -* 遵循 [PEP 8 代码风格](https://peps.pythonlang.cn/pep-0008/) 并完善注释 -* 实现神经网络在CARLA场景中的端到端应用 -* 撰写模块功能说明与API文档 -* 添加自动化测试(模型性能、场景稳定性、数据一致性) -* 优化感知-规划-控制链路的实时性 - -## 参考资源 - -* [CARLA官方文档](https://carla.readthedocs.io/) -* [PyTorch神经网络教程](https://pytorch.org/tutorials/) -* [项目文档中心](https://openhutb.github.io/nn/) -* [代理模拟器文档](https://openhutb.github.io/carla_doc/) -* [神经网络原理](https://github.com/OpenHUTB/neuro) \ No newline at end of file diff --git a/src/tracking_car/config.yaml b/src/tracking_car/config.yaml new file mode 100644 index 0000000000..642d8d90c8 --- /dev/null +++ b/src/tracking_car/config.yaml @@ -0,0 +1,55 @@ +# config.yaml - 完整配置示例 +host: "localhost" +port: 2000 +num_npcs: 20 +img_width: 640 +img_height: 480 +conf_thres: 0.5 +iou_thres: 0.3 +max_age: 5 +min_hits: 3 +yolo_model: "yolov8n.pt" +yolo_imgsz_max: 320 +yolo_iou: 0.45 +yolo_quantize: false +kf_dt: 0.05 +max_speed: 50.0 +window_width: 1280 +window_height: 720 +smooth_alpha: 0.2 +fps_window_size: 15 +display_fps: 30 +track_history_len: 20 +track_line_width: 2 +track_alpha: 0.6 +stop_speed_thresh: 1.0 +stop_frames_thresh: 5 +overtake_speed_ratio: 1.5 +overtake_dist_thresh: 50.0 +lane_change_thresh: 0.5 +brake_accel_thresh: 2.0 +turn_angle_thresh: 15.0 +danger_dist_thresh: 10.0 +predict_frames: 10 +default_weather: "clear" +auto_adjust_detection: true +use_lidar: true +lidar_channels: 32 +lidar_range: 100.0 +lidar_points_per_second: 500000 +fuse_lidar_vision: true +record_data: true +record_dir: "track_records" +record_format: "csv" +record_fps: 10 +save_screenshots: false +use_3d_visualization: true +pcd_view_size: 800 + +# 检测框样式配置(匹配截图) +det_box_color: [255, 0, 0] # 蓝色框 +det_box_thickness: 2 +det_label_bg_color: [0, 0, 0] # 黑色背景 +det_label_text_color: [255, 255, 255] # 白色文字 +det_label_font_scale: 0.5 +det_label_padding: 5 \ No newline at end of file diff --git a/src/tracking_car/main.py b/src/tracking_car/main.py index 9aa5f82b55..6569b4e63c 100644 --- a/src/tracking_car/main.py +++ b/src/tracking_car/main.py @@ -4,255 +4,848 @@ import random import cv2 import numpy as np +import time +import os +import sys +import yaml +import threading +import torch +import csv +import open3d as o3d +import psutil from ultralytics import YOLO - -# 内置简易SORT跟踪器(无第三方跟踪库依赖) -class KalmanFilter: - def __init__(self): - self.dt = 0.05 # 与CARLA同步步长一致 - self.x = np.zeros(8) # [x1, y1, x2, y2, vx, vy, vw, vh] - self.F = np.array([ - [1, 0, 0, 0, self.dt, 0, 0, 0], - [0, 1, 0, 0, 0, self.dt, 0, 0], - [0, 0, 1, 0, 0, 0, self.dt, 0], - [0, 0, 0, 1, 0, 0, 0, self.dt], - [0, 0, 0, 0, 1, 0, 0, 0], - [0, 0, 0, 0, 0, 1, 0, 0], - [0, 0, 0, 0, 0, 0, 1, 0], - [0, 0, 0, 0, 0, 0, 0, 1] - ]) - self.H = np.array([[1,0,0,0,0,0,0,0], [0,1,0,0,0,0,0,0], [0,0,1,0,0,0,0,0], [0,0,0,1,0,0,0,0]]) - self.Q = np.diag([1,1,1,1,10,10,10,10]) - self.R = np.diag([10,10,10,10]) - self.P = np.eye(8) * 100 - +from dataclasses import dataclass +from scipy.optimize import linear_sum_assignment +from datetime import datetime +from numba import njit +from loguru import logger +from sklearn.cluster import DBSCAN + +# ======================== 通用工具函数 ======================== +def valid_img(img): + return img is not None and len(img.shape)==3 and img.shape[2]==3 and img.size>0 + +def clip_box(bbox, img_shape): + h,w = img_shape + return np.array([ + max(0,min(bbox[0],w-1)), + max(0,min(bbox[1],h-1)), + max(bbox[0]+1,min(bbox[2],w-1)), + max(bbox[1]+1,min(bbox[3],h-1)) + ], dtype=np.float32) + +def make_div(x, d=32): + return (x + d -1) // d * d + +# ======================== 常量配置 ======================== +PLATFORM, IS_WIN, IS_LINUX = sys.platform, sys.platform.startswith('win'), sys.platform.startswith('linux') +WEATHER = { + 'clear':carla.WeatherParameters(0.0,0.0,0.0,0.0,180.0,75.0,0.0,0.0,1.0,0.0,0.0), + 'rain':carla.WeatherParameters(80.0,80.0,50.0,30.0,180.0,45.0,20.0,50.0,0.8,80.0,0.5), + 'fog':carla.WeatherParameters(90.0,0.0,0.0,10.0,180.0,30.0,70.0,20.0,0.5,10.0,0.8), + 'night':carla.WeatherParameters(20.0,0.0,0.0,0.0,0.0,-90.0,10.0,100.0,0.7,0.0,1.0), + 'cloudy':carla.WeatherParameters(90.0,0.0,0.0,20.0,180.0,60.0,10.0,100.0,0.9,0.0,0.3), + 'snow':carla.WeatherParameters(90.0,90.0,80.0,40.0,180.0,20.0,30.0,30.0,0.6,50.0,0.7) +} +VEHICLE_CLS = {2:"Car",5:"Bus",7:"Truck",-1:"Unknown"} + +# ======================== 配置类 ======================== +@dataclass +class Config: + host, port, num_npcs = "localhost", 2000, 20 + img_width, img_height = 640, 480 + conf_thres, iou_thres, max_age, min_hits = 0.5, 0.3, 5, 3 + yolo_model, yolo_imgsz_max, yolo_iou, yolo_quantize = "yolov8n.pt", 320, 0.45, False + kf_dt, max_speed = 0.05, 50.0 + window_width, window_height, smooth_alpha, fps_window_size, display_fps = 1280,720,0.2,15,30 + track_history_len, track_line_width, track_alpha = 20,2,0.6 + stop_speed_thresh, stop_frames_thresh = 1.0,5 + overtake_speed_ratio, overtake_dist_thresh = 1.5,50.0 + lane_change_thresh, brake_accel_thresh, turn_angle_thresh, danger_dist_thresh, predict_frames = 0.5,2.0,15.0,10.0,10 + default_weather, auto_adjust_detection = "clear", True + use_lidar, lidar_channels, lidar_range, lidar_points_per_second, fuse_lidar_vision = True,32,100.0,500000,True + record_data, record_dir, record_format, record_fps, save_screenshots = True,"track_records","csv",10,False + use_3d_visualization, pcd_view_size = True,800 + + @classmethod + def from_yaml(cls, p=None): + try: + p = p or os.path.join(os.path.dirname(os.path.abspath(__file__)), "config.yaml") + if not os.path.exists(p): return cls() + with open(p, "r", encoding="utf-8") as f: data = yaml.safe_load(f.read().strip().replace("\t"," ")) + if not isinstance(data, dict): return cls() + valid_keys = set(cls.__dataclass_fields__.keys()) + data = {k:v for k,v in data.items() if k in valid_keys} + for k,v in data.items(): + try: data[k] = cls.__dataclass_fields__[k].type(v) + except: del data[k] + return cls(**data) + except: return cls() + +# ======================== 天气图像增强 ======================== +class WeatherEnhancer: + def __init__(self, cfg): + self.cfg = cfg; self.weather = "clear" + self.params = { + 'clear':{'b':1.0,'c':1.0,'g':1.0}, + 'rain':{'b':1.1,'c':1.2,'g':0.9,'dh':True,'dr':True}, + 'fog':{'b':1.3,'c':1.4,'g':0.8,'dh':True}, + 'night':{'b':1.5,'c':1.3,'g':0.7,'dn':True}, + 'cloudy':{'b':1.2,'c':1.1,'g':1.0}, + 'snow':{'b':1.1,'c':1.3,'g':0.9,'dh':True,'ds':True} + } + def set_weather(self, w): + if w in WEATHER: self.weather = w + def enhance(self, img): + if not self.cfg.auto_adjust_detection or not valid_img(img): return img + p = self.params.get(self.weather, self.params['clear']) + enh = cv2.convertScaleAbs(img.copy(), alpha=p['c'], beta=int(p['b']*255-255)) + g = p['g']; inv_g = 1.0/g + g_table = np.array([((i/255.0)**inv_g)*255 for i in range(256)]).astype(np.uint8) + enh = cv2.LUT(enh, g_table) + if p.get('dh'): enh = self._dehaze(enh) + if p.get('dr'): enh = self._derain(enh) + if p.get('ds'): enh = self._desnow(enh) + if p.get('dn'): enh = cv2.fastNlMeansDenoisingColored(enh, None,10,10,7,21) + return enh + def _dehaze(self, img): + gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY) + dc = cv2.erode(gray, np.ones((7,7), np.uint8)) + nz = dc[dc<10]; al = 255.0 if len(nz)==0 else np.max(img[dc<10]) + t = np.clip(1-0.1*(gray/al),0.1,1.0) + deh = np.zeros_like(img, dtype=np.float32) + for c in range(3): deh[:,:,c] = (img[:,:,c].astype(np.float32)-al)/t + al + return np.clip(deh,0,255).astype(np.uint8) + def _derain(self, img): + gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY) + km = cv2.getStructuringElement(cv2.MORPH_RECT, (1,5)) + rm = cv2.morphologyEx(gray, cv2.MORPH_OPEN, km) + return cv2.inpaint(img, (255-rm).astype(np.uint8),3,cv2.INPAINT_TELEA) + def _desnow(self, img): + blur = cv2.GaussianBlur(img,(5,5),0) + gray = cv2.cvtColor(blur, cv2.COLOR_BGR2GRAY) + _, sm = cv2.threshold(gray,200,255,cv2.THRESH_BINARY) + return cv2.inpaint(img,sm,5,cv2.INPAINT_NS) + +# ======================== LiDAR处理 ======================== +class LiDARProc: + def __init__(self, cfg): + self.cfg = cfg; self.q = queue.Queue(maxsize=2); self.data, self.trans = None, None + def cb(self, pc): + try: + self.data = np.frombuffer(pc.raw_data, dtype=np.float32).reshape(-1,4)[:,:3] + self.trans = pc.transform + if self.q.full(): self.q.get_nowait() + self.q.put((self.data.copy(), self.trans)) + except: pass + def detect(self): + if self.data is None or len(self.data)<50: return [] + gm = self.data[:,2] < -1.0; ng = self.data[~gm] + if len(ng)<50: return [] + cls = DBSCAN(eps=0.8, min_samples=30).fit(ng[:,:2]) + boxes = [] + for l in set(cls.labels_): + if l == -1: continue + cp = ng[cls.labels_==l] + if len(cp)<30: continue + mn = cp.min(axis=0); mx = cp.max(axis=0) + boxes.append({'3d_bbox':[mn[0],mn[1],mn[2],mx[0],mx[1],mx[2]],'center':[(mn[0]+mx[0])/2,(mn[1]+mx[1])/2,(mn[2]+mx[2])/2],'size':[mx[0]-mn[0],mx[1]-mn[1],mx[2]-mn[2]],'num_points':len(cp)}) + return boxes + def get_3d(self): + if self.data is None: return None + pcd = o3d.geometry.PointCloud() + pcd.points = o3d.utility.Vector3dVector(self.data) + z_min,z_max = self.data[:,2].min(),self.data[:,2].max() + c = (self.data[:,2]-z_min)/(z_max-z_min+1e-6) + cm = np.zeros((len(c),3)); cm[:,0]=1-c; cm[:,2]=c + pcd.colors = o3d.utility.Vector3dVector(cm) + return pcd + +# ======================== 数据记录 ======================== +class Recorder: + def __init__(self, cfg): + self.cfg = cfg; self.dir = os.path.join(cfg.record_dir, datetime.now().strftime("%Y%m%d_%H%M%S")) + self.fr = 0; self.fs = {} + if cfg.record_data: + os.makedirs(self.dir, exist_ok=True) + os.makedirs(os.path.join(self.dir,"screenshots"), exist_ok=True) + self._init() + def _init(self): + tp = os.path.join(self.dir, "track_results.csv") + self.fs['tracks'] = open(tp, 'w', newline='', encoding='utf-8') + self.tw = csv.writer(self.fs['tracks']) + self.tw.writerow(['timestamp','frame_id','track_id','x1','y1','x2','y2','cls_id','cls_name','behavior','speed','confidence']) + pp = os.path.join(self.dir, "performance.csv") + self.fs['performance'] = open(pp, 'w', newline='', encoding='utf-8') + self.pw = csv.writer(self.fs['performance']) + self.pw.writerow(['timestamp','frame_id','fps','cpu_usage','memory_usage','gpu_usage','detection_count','track_count']) + cp = os.path.join(self.dir, "config.yaml") + with open(cp, 'w', encoding='utf-8') as f: yaml.dump(self.cfg.__dict__, f, indent=2) + def record(self, tracks, dets, fps): + if not self.cfg.record_data: return + if self.fr % (self.cfg.display_fps//self.cfg.record_fps) !=0: + self.fr +=1; return + ts = datetime.now().strftime("%Y-%m-%d %H:%M:%S.%f") + if tracks and len(tracks)>0: + for t in tracks: + try: + b = "stopped" if t.is_stopped else "overtaking" if t.is_overtaking else "lane_changing" if t.is_lane_changing else "braking" if t.is_braking else "dangerous" if t.is_dangerous else "normal" + s = t._calc_speed() if hasattr(t,'_calc_speed') else 0.0 + self.tw.writerow([ts,self.fr,t.track_id,t.bbox[0],t.bbox[1],t.bbox[2],t.bbox[3],t.cls_id,VEHICLE_CLS.get(t.cls_id,"Unknown"),b,s,t.conf if hasattr(t,'conf') else 0.0]) + except: pass + cpu = psutil.cpu_percent(); mem = psutil.virtual_memory().percent + gpu = torch.cuda.utilization() if (torch.cuda.is_available() and hasattr(torch.cuda,'utilization')) else 0.0 + self.pw.writerow([ts,self.fr,fps,cpu,mem,gpu,len(dets) if dets is not None else 0,len(tracks) if tracks is not None else 0]) + for f in self.fs.values(): f.flush() + self.fr +=1 + def save_ss(self, img, w): + if not self.cfg.save_screenshots or not valid_img(img): return + p = os.path.join(self.dir,"screenshots",f"screenshot_{w}_{self.fr:06d}.png") + cv2.imwrite(p, img) + def close(self): + if self.cfg.record_data: + for f in self.fs.values(): + try: f.close() + except: pass + +# ======================== 卡尔曼滤波 ======================== +class KF: + def __init__(self, dt=0.05, ms=50.0): + self.dt = dt; self.ms = ms; self.x = np.zeros(8, dtype=np.float32) + self.F = np.array([[1,0,0,0,dt,0,0,0],[0,1,0,0,0,dt,0,0],[0,0,1,0,0,0,dt,0],[0,0,0,1,0,0,0,dt],[0,0,0,0,1,0,0,0],[0,0,0,0,0,1,0,0],[0,0,0,0,0,0,1,0],[0,0,0,0,0,0,0,1]], dtype=np.float32) + self.H = np.eye(4,8,dtype=np.float32) + self.Q = np.diag([1,1,1,1,5,5,5,5]).astype(np.float32) + self.R = np.diag([5,5,5,5]).astype(np.float32) + self.P = np.eye(8,dtype=np.float32)*50 def predict(self): self.x = self.F @ self.x self.P = self.F @ self.P @ self.F.T + self.Q return self.x[:4] - def update(self, z): + z = z.astype(np.float32) y = z - self.H @ self.x S = self.H @ self.P @ self.H.T + self.R - K = self.P @ self.H.T @ np.linalg.inv(S) + S_inv = np.linalg.pinv(S) if np.linalg.det(S)<1e-6 else np.linalg.inv(S) + K = self.P @ self.H.T @ S_inv self.x = self.x + K @ y - self.P = (np.eye(8) - K @ self.H) @ self.P + self.P = (np.eye(8)-K@self.H) @ self.P return self.x[:4] - + def update_noise(self, s): + sf = min(1.0, s/self.ms) + self.Q = np.diag([1+sf*4]*4 + [5+sf*20]*4).astype(np.float32) + +# ======================== IOU计算 ======================== +@njit +def iou(box1, box2): + ix1 = max(box1[0], box2[0]); iy1 = max(box1[1], box2[1]) + ix2 = min(box1[2], box2[2]); iy2 = min(box1[3], box2[3]) + ia = max(0, ix2-ix1)*max(0, iy2-iy1) + a1 = (box1[2]-box1[0])*(box1[3]-box1[1]) + a2 = (box2[2]-box2[0])*(box2[3]-box2[1]) + ua = a1+a2-ia + return ia/ua if ua>0 else 0 + +# ======================== 跟踪目标 ======================== class Track: - def __init__(self, track_id, bbox): - self.track_id = track_id - self.kf = KalmanFilter() - self.bbox = bbox - self.hits = 1 - self.age = 0 - self.time_since_update = 0 - + def __init__(self, tid, bbox, img_shape, kf_cfg, cfg): + self.track_id = tid; self.kf = KF(dt=kf_cfg["dt"], ms=kf_cfg["max_speed"]) + self.img_shape = img_shape; self.cfg = cfg; self.bbox = clip_box(bbox.astype(np.float32), img_shape) + self.kf.x[:4] = self.bbox; self.track_hist = []; self.speed_hist = []; self.accel_hist = [] + self.heading_hist = []; self.lat_dis = []; self.pred_traj = [] + self.is_stopped = self.is_overtaking = self.is_lane_changing = self.is_braking = self.is_accelerating = self.is_turning = self.is_dangerous = False + self.stop_fr = self.overtake_fr = self.lane_change_fr = self.brake_fr = self.turn_fr = 0 + self.hits = 1; self.age = 0; self.tsu = 0; self.cls_id = None; self.conf = 0.0 + self._update_hist() + def _update_hist(self): + cx = (self.bbox[0]+self.bbox[2])/2; cy = (self.bbox[1]+self.bbox[3])/2 + self.track_hist.append((cx, cy)) + if len(self.track_hist)>1: self.lat_dis.append(abs(cx - self.track_hist[-2][0])) + if len(self.track_hist)>self.cfg.track_history_len: self.track_hist.pop(0) + if len(self.lat_dis)>10: self.lat_dis.pop(0) + def _calc_speed(self): + if len(self.track_hist)<2: return 0.0 + pc = self.track_hist[-2]; cc = self.track_hist[-1] + s = np.linalg.norm(np.array(cc)-np.array(pc))/self.kf.dt + self.speed_hist.append(s) + if len(self.speed_hist)>1: self.accel_hist.append((s-self.speed_hist[-2])/self.kf.dt) + if len(self.speed_hist)>5: self.speed_hist.pop(0) + if len(self.accel_hist)>5: self.accel_hist.pop(0) + return np.mean(self.speed_hist) if self.speed_hist else 0.0 + def _calc_heading(self): + if len(self.track_hist)<3: return 0.0 + dx = self.track_hist[-1][0]-self.track_hist[-3][0] + dy = self.track_hist[-1][1]-self.track_hist[-3][1] + h = np.degrees(np.arctan2(dy, dx)) + self.heading_hist.append(h) + if len(self.heading_hist)>5: self.heading_hist.pop(0) + return h + def _pred_traj(self): + self.pred_traj = [] + if len(self.track_hist)<5: return + tkf = KF(dt=self.kf.dt, ms=self.kf.ms) + tkf.x = self.kf.x.copy(); tkf.P = self.kf.P.copy() + for _ in range(self.cfg.predict_frames): + pb = tkf.predict() + self.pred_traj.append(((pb[0]+pb[2])/2, (pb[1]+pb[3])/2)) + def _analyze_behavior(self, ego_center): + s = self._calc_speed(); h = self._calc_heading() + if s < self.cfg.stop_speed_thresh: + self.stop_fr +=1; self.is_stopped = self.stop_fr >= self.cfg.stop_frames_thresh + else: self.stop_fr = 0; self.is_stopped = False + if ego_center and len(self.track_hist)>=2: + d = np.linalg.norm(np.array(self.track_hist[-1])-np.array(ego_center)) + if d < self.cfg.overtake_dist_thresh: + es = getattr(self, 'ego_speed', 0.0) + if s > es*self.cfg.overtake_speed_ratio: + self.overtake_fr +=1; self.is_overtaking = self.overtake_fr >=3 + else: self.overtake_fr =0; self.is_overtaking = False + else: self.overtake_fr =0; self.is_overtaking = False + if len(self.lat_dis)>=5: + al = np.mean(self.lat_dis[-5:]) + if al>self.cfg.lane_change_thresh: + self.lane_change_fr +=1; self.is_lane_changing = self.lane_change_fr >=3 + else: self.lane_change_fr =0; self.is_lane_changing = False + if len(self.accel_hist)>=3: + aa = np.mean(self.accel_hist[-3:]) + if aa < -self.cfg.brake_accel_thresh: + self.brake_fr +=1; self.is_braking = self.brake_fr >=2; self.is_accelerating = False + elif aa > self.cfg.brake_accel_thresh: + self.is_accelerating = True; self.is_braking = False; self.brake_fr =0 + else: self.is_braking = self.is_accelerating = False; self.brake_fr =0 + if len(self.heading_hist)>=3: + hd = np.abs(self.heading_hist[-1]-self.heading_hist[-3]) + if hd>self.cfg.turn_angle_thresh: + self.turn_fr +=1; self.is_turning = self.turn_fr >=2 + else: self.turn_fr =0; self.is_turning = False + if ego_center: + d = np.linalg.norm(np.array(self.track_hist[-1])-np.array(ego_center)) + self.is_dangerous = d < self.cfg.danger_dist_thresh + self._pred_traj() def predict(self): + if len(self.track_hist)>=2: + pc = np.array([(self.kf.x[0]+self.kf.x[2])/2, (self.kf.x[1]+self.kf.x[3])/2]) + cc = np.array([(self.bbox[0]+self.bbox[2])/2, (self.bbox[1]+self.bbox[3])/2]) + ps = np.linalg.norm(cc-pc)/self.kf.dt + mps = max(self.img_shape)/self.kf.dt + s = min(1.0, ps/mps)*self.kf.ms + else: s=0.0 self.bbox = self.kf.predict() - self.age += 1 - self.time_since_update += 1 + self.bbox = clip_box(self.bbox, self.img_shape) + self._update_hist(); self.age +=1; self.tsu +=1; self.kf.update_noise(s) return self.bbox - - def update(self, bbox): - self.bbox = self.kf.update(bbox) - self.hits += 1 - self.time_since_update = 0 - -class SimpleSORT: - def __init__(self, max_age=1, min_hits=3, iou_threshold=0.3): - self.max_age = max_age - self.min_hits = min_hits - self.iou_threshold = iou_threshold - self.tracks = [] - self.next_id = 1 - - def update(self, detections): - for track in self.tracks: - track.predict() - - track_boxes = np.array([t.bbox for t in self.tracks]) - det_boxes = np.array([d[:4] for d in detections]) - iou_matrix = self._iou_batch(det_boxes, track_boxes) - - matches = [] - used_dets = set() - used_tracks = set() - - for t_idx in range(len(self.tracks)): - if t_idx in used_tracks: + def update(self, bbox, cls_id, conf=0.0, ego_center=None): + self.bbox = self.kf.update(clip_box(bbox, self.img_shape)) + self._update_hist(); self.hits +=1; self.tsu =0; self.cls_id = cls_id; self.conf = conf + self._analyze_behavior(ego_center) + +# ======================== SORT跟踪器 ======================== +class SORT: + def __init__(self, cfg): + self.max_age = cfg.max_age; self.min_hits = cfg.min_hits; self.iou_th = cfg.iou_thres + self.img_shape = (cfg.img_height, cfg.img_width); self.kf_cfg = {"dt":cfg.kf_dt, "max_speed":cfg.max_speed} + self.cfg = cfg; self.tracks = []; self.next_id =1; self.ego_center = None; self.ego_speed =0.0 + def update(self, dets, ego_center=None, lidar_dets=None): + self.ego_center = ego_center + if dets is None or len(dets)==0: + if lidar_dets and len(lidar_dets)>0 and self.cfg.fuse_lidar_vision: + dets = self._lidar2d(lidar_dets) + self.tracks = [t for t in self.tracks if t.tsu <= self.max_age] + return np.array([]), np.array([]), np.array([]) + vd = [] + for d in dets: + if len(d)>=6: + x1,y1,x2,y2,conf,cls_id = d[:6] + if conf>0 and x2>x1 and y2>y1: vd.append([x1,y1,x2,y2,conf,int(cls_id)]) + vd = np.array(vd, dtype=np.float32) + if len(vd)==0: + self.tracks = [t for t in self.tracks if t.tsu <= self.max_age] + return np.array([]), np.array([]), np.array([]) + for t in self.tracks: t.predict() + if len(self.tracks)==0: + for d in vd: + self.tracks.append(Track(self.next_id, d[:4], self.img_shape, self.kf_cfg, self.cfg)) + self.next_id +=1 + return np.array([]), np.array([]), np.array([]) + try: + iou_mat = np.array([[iou(t.bbox, d[:4]) for t in self.tracks] for d in vd]) + cost_mat = 1 - iou_mat + t_idx, d_idx = linear_sum_assignment(cost_mat) + except: t_idx, d_idx = [], [] + matches, ud, ut = [], set(), set() + for ti, di in zip(t_idx, d_idx): + if tiself.iou_th: + matches.append((ti,di)); ud.add(di); ut.add(ti) + for ti, di in matches: + self.tracks[ti].update(vd[di][:4], int(vd[di][5]), vd[di][4], self.ego_center) + self.tracks[ti].ego_speed = self.ego_speed + for di in set(range(len(vd)))-ud: + self.tracks.append(Track(self.next_id, vd[di][:4], self.img_shape, self.kf_cfg, self.cfg)) + self.next_id +=1 + self.tracks = [t for t in self.tracks if t.tsu <= self.max_age] + vt = [t for t in self.tracks if t.hits >= self.min_hits] + if not vt: return np.array([]), np.array([]), np.array([]) + boxes = np.array([t.bbox.astype(int) for t in vt]) + ids = np.array([t.track_id for t in vt]) + cls = np.array([t.cls_id if t.cls_id is not None else -1 for t in vt]) + return boxes, ids, cls + def _lidar2d(self, lidar_dets): + dets = [] + for d in lidar_dets: + c = d['center']; s = d['size'] + x1 = c[0]*10 + self.img_shape[1]/2; y1 = c[1]*10 + self.img_shape[0]/2 + x2 = x1 + s[0]*5; y2 = y1 + s[1]*5 + dets.append([x1,y1,x2,y2,0.8,2]) + return np.array(dets) + +# ======================== 检测线程 ======================== +class DetThread(threading.Thread): + def __init__(self, det, cfg, enh, in_q, out_q, dev="cpu"): + super().__init__(daemon=True) + self.det = det; self.cfg = cfg; self.enh = enh; self.in_q = in_q; self.out_q = out_q + self.running = True; self.dev = dev + def run(self): + while self.running: + try: + img = self.in_q.get(timeout=1.0) + if not valid_img(img): + self.out_q.put((None, np.array([]))) + continue + img_enh = self.enh.enhance(img) + h,w = img.shape[:2]; r = min(self.cfg.yolo_imgsz_max/w, self.cfg.yolo_imgsz_max/h) + ws, hs = make_div(int(w*r)), make_div(int(h*r)) + res = self.det.predict(img_enh, conf=self.cfg.conf_thres, verbose=False, device=self.dev, agnostic_nms=True, imgsz=(hs,ws), iou=self.cfg.yolo_iou) + dets = [] + for r in res: + if hasattr(r, 'boxes') and r.boxes is not None and len(r.boxes)>0: + for b in r.boxes: + if b.cls is not None and b.conf is not None and b.xyxy is not None: + cid = int(b.cls[0]) + if cid in {2,5,7}: + xyxy = b.xyxy[0].cpu().numpy() + conf = float(b.conf[0]) + if xyxy[2]>xyxy[0] and xyxy[3]>xyxy[1] and conf>0: + dets.append([*xyxy, conf, cid]) + self.out_q.put((img, np.array(dets, dtype=np.float32))) + except queue.Empty: continue + except: self.out_q.put((None, np.array([]))) + def stop(self): self.running = False + +# ======================== 可视化工具(优化版) ======================== +class FrameBuf: + def __init__(self, sz=(480,640,3)): + self.df = np.zeros(sz, dtype=np.uint8) + cv2.putText(self.df, "Initializing...", (100,240), cv2.FONT_HERSHEY_SIMPLEX,1,(255,255,255),2) + self.cf = self.df.copy(); self.lock = threading.Lock() + def update(self, f): + if valid_img(f): + with self.lock: self.cf = f.copy() + def get(self): return self.cf.copy() + +class FPS: + def __init__(self, ws=15): + self.ws = ws; self.times = []; self.fps =0.0 + def update(self): + self.times.append(time.time()) + if len(self.times)>self.ws: self.times.pop(0) + if len(self.times)>=2: self.fps = (len(self.times)-1)/(self.times[-1]-self.times[0]) + return self.fps + +# ======================== 核心绘制函数(优化版) ======================== +def draw(img, boxes, ids, cls_ids, tracks, fps=0.0, det_cnt=0, cfg=None, w="clear", perf=None): + """优化版绘制函数,简洁高效,匹配截图样式""" + if not valid_img(img): + return np.zeros((480,640,3), dtype=np.uint8) + + di = img.copy() + + # 1. 绘制顶部信息栏(简洁版) + info_height = 60 + cv2.rectangle(di, (0, 0), (di.shape[1], info_height), (0, 0, 0), -1) + + # FPS和基本信息 + cv2.putText(di, f"FPS: {fps:.1f} | Weather: {w} | Tracks: {len(boxes)}", + (10, 25), cv2.FONT_HERSHEY_SIMPLEX, 0.6, (0, 255, 0), 1, cv2.LINE_AA) + + # 行为统计 + if tracks: + sc = sum(1 for t in tracks if t.is_stopped) + oc = sum(1 for t in tracks if t.is_overtaking) + dc = sum(1 for t in tracks if t.is_dangerous) + cv2.putText(di, f"Stop: {sc} | Overtake: {oc} | Danger: {dc}", + (10, 50), cv2.FONT_HERSHEY_SIMPLEX, 0.6, (0, 255, 0), 1, cv2.LINE_AA) + + # 2. 绘制检测框(简化版,匹配截图) + if boxes is not None and ids is not None: + for i, (b, tid) in enumerate(zip(boxes, ids)): + try: + if len(b) != 4: continue + + x1, y1, x2, y2 = map(int, b) + + # 检查坐标有效性 + if x1 >= x2 or y1 >= y2: continue + + # 蓝色检测框(截图样式) + cv2.rectangle(di, (x1, y1), (x2, y2), (255, 0, 0), 2, cv2.LINE_AA) + + # 在左上角显示ID(白色背景,黑色文字) + id_text = f"{tid}" + text_size = cv2.getTextSize(id_text, cv2.FONT_HERSHEY_SIMPLEX, 0.5, 1)[0] + + # 背景框 + bg_x2 = x1 + text_size[0] + 6 + bg_y2 = y1 + text_size[1] + 6 + cv2.rectangle(di, (x1, y1), (bg_x2, bg_y2), (255, 255, 255), -1) + + # 文字 + cv2.putText(di, id_text, (x1+3, y1+text_size[1]+3), + cv2.FONT_HERSHEY_SIMPLEX, 0.5, (0, 0, 0), 1) + + except Exception as e: continue - d_idx = np.argmax(iou_matrix[:, t_idx]) - if iou_matrix[d_idx, t_idx] > self.iou_threshold and d_idx not in used_dets: - matches.append((t_idx, d_idx)) - used_dets.add(d_idx) - used_tracks.add(t_idx) - - for t_idx, d_idx in matches: - self.tracks[t_idx].update(detections[d_idx][:4]) - - for d_idx in range(len(detections)): - if d_idx not in used_dets: - self.tracks.append(Track(self.next_id, detections[d_idx][:4])) - self.next_id += 1 - - self.tracks = [t for t in self.tracks if t.time_since_update <= self.max_age] - - confirmed = [(t.bbox, t.track_id) for t in self.tracks if t.hits >= self.min_hits] - return np.array([b for b, _ in confirmed]), np.array([i for _, i in confirmed]) - - def _iou_batch(self, boxes1, boxes2): - ious = np.zeros((len(boxes1), len(boxes2))) - for i, b1 in enumerate(boxes1): - for j, b2 in enumerate(boxes2): - ious[i, j] = self._iou(b1, b2) - return ious - - def _iou(self, b1, b2): - x1, y1, x2, y2 = b1 - a1, b1, a2, b2 = b2 - inter = max(0, min(x2, a2) - max(x1, a1)) * max(0, min(y2, b2) - max(y1, b1)) - area1 = (x2 - x1) * (y2 - y1) - area2 = (a2 - a1) * (b2 - b1) - return inter / (area1 + area2 - inter) if (area1 + area2 - inter) > 0 else 0 - -# 工具函数(无需外部utils库) -def draw_bounding_boxes(image, boxes, ids): - """在图像上绘制跟踪框和ID""" - for i, box in enumerate(boxes): - x1, y1, x2, y2 = box.astype(int) - # 绘制边界框 - cv2.rectangle(image, (x1, y1), (x2, y2), (0, 255, 0), 2) - # 绘制跟踪ID - cv2.putText(image, f"ID: {ids[i]}", (x1, y1 - 10), - cv2.FONT_HERSHEY_SIMPLEX, 0.5, (0, 255, 0), 2) - return image - -def clear_actors(world): - """清理CARLA中的所有车辆和传感器""" - for actor in world.get_actors(): - if actor.type_id.startswith('vehicle.') or actor.type_id.startswith('sensor.'): - actor.destroy() - -def camera_callback(image, queue): - """相机数据回调函数""" - img = np.reshape(np.copy(image.raw_data), (image.height, image.width, 4))[:, :, :3] # 去除Alpha通道 - queue.put(img) - + + # 3. 绘制轨迹线(可选,简化版) + if tracks: + for track in tracks: + if len(track.track_hist) >= 2: + # 只绘制最近5帧轨迹 + for j in range(1, min(5, len(track.track_hist))): + try: + pt1 = (int(track.track_hist[-j-1][0]), int(track.track_hist[-j-1][1])) + pt2 = (int(track.track_hist[-j][0]), int(track.track_hist[-j][1])) + cv2.line(di, pt1, pt2, (0, 255, 0), 1, cv2.LINE_AA) + except: + pass + + return di + +# ======================== 工具函数 ======================== +def clear_actors(world, exclude=None): + ei = set(exclude) if exclude else set() + actors = world.get_actors() + actor_id_map = {a.id: a for a in actors if a.is_alive} + for at in ['vehicle.', 'sensor.']: + al = [a for a_id, a in actor_id_map.items() if a.type_id.startswith(at) and a_id not in ei] + for i in range(0, len(al),10): + for a in al[i:i+10]: + try: + if a.is_alive and a.id in actor_id_map: + a.destroy() + del actor_id_map[a.id] + except Exception as e: + logger.warning(f"跳过销毁Actor {a.id}:{str(e)[:30]}") + +def cam_cb(img, q): + try: + ia = np.frombuffer(img.raw_data, dtype=np.uint8).reshape((img.height, img.width,4)) + ir = cv2.GaussianBlur(ia[:,:,:3], (3,3),0) + if q.full(): q.get_nowait() + q.put(ir) + except: pass + +def spawn_npcs(world, num, sp): + bps = [bp for bp in world.get_blueprint_library().filter('vehicle') if int(bp.get_attribute('number_of_wheels'))==4 and not bp.id.endswith(('firetruck','ambulance','police'))] + if not bps: return 0 + cnt, used, max_att = 0, set(), num*3 + for _ in range(max_att): + if cnt>=num or len(used)>=len(sp): break + p = random.choice(sp) + k = (round(p.location.x,2), round(p.location.y,2), round(p.location.z,2)) + if k not in used: + used.add(k) + n = world.try_spawn_actor(random.choice(bps), p) + if n: + try: n.set_autopilot(True, tm_port=8000) + except: n.set_autopilot(True) + cnt +=1 + return cnt + +# 安全生成自车函数 +def safe_spawn_ego(world, spawn_points): + """安全生成自车,避免碰撞""" + ego_bp = random.choice(world.get_blueprint_library().filter('vehicle.tesla.model3')) + ego_bp.set_attribute('color', '255,0,0') + # 遍历生成点,直到找到无碰撞的位置 + for spawn_point in spawn_points: + ego = world.try_spawn_actor(ego_bp, spawn_point) + if ego is not None: + print(f"✅ 自车生成成功,位置:{spawn_point.location}") + return ego + # 若所有生成点都碰撞,随机偏移位置重试 + print("⚠️ 所有默认生成点有碰撞,尝试偏移位置...") + for spawn_point in spawn_points: + # 随机偏移x/y坐标(±2米) + spawn_point.location.x += random.uniform(-2, 2) + spawn_point.location.y += random.uniform(-2, 2) + ego = world.try_spawn_actor(ego_bp, spawn_point) + if ego is not None: + print(f"✅ 自车生成成功(偏移位置):{spawn_point.location}") + return ego + print("❌ 无法生成自车,所有位置都有碰撞") + return None + +# ======================== 主函数 ======================== def main(): - parser = argparse.ArgumentParser(description="CARLA目标跟踪(YOLOv5 + 内置SORT)") - parser.add_argument("--host", default="localhost", help="CARLA服务器地址") - parser.add_argument("--port", type=int, default=2000, help="CARLA服务器端口") - parser.add_argument("--num_npcs", type=int, default=30, help="NPC车辆数量") + parser = argparse.ArgumentParser() + parser.add_argument("--config", help="配置文件路径") + parser.add_argument("--host", help="CARLA主机") + parser.add_argument("--port", type=int, help="CARLA端口") + parser.add_argument("--conf-thres", type=float, help="检测置信度") + parser.add_argument("--weather", help="初始天气") args = parser.parse_args() + + cfg = Config.from_yaml(args.config) + if args.host: cfg.host = args.host + if args.port: cfg.port = args.port + if args.conf_thres: cfg.conf_thres = args.conf_thres + if args.weather and args.weather in WEATHER: cfg.default_weather = args.weather + + # 初始化所有可能用到的变量 + ego = None + cam = None + lidar = None + lidar_proc = None + det_thread = None + vis = None + recorder = Recorder(cfg) - # 初始化CARLA客户端 - client = carla.Client(args.host, args.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) - - # 清理现有Actor - clear_actors(world) - - # 生成主车辆 - bp_lib = world.get_blueprint_library() - ego_bp = bp_lib.find('vehicle.lincoln.mkz_2020') - spawn_points = world.get_map().get_spawn_points() - ego_vehicle = world.try_spawn_actor(ego_bp, random.choice(spawn_points)) - if not ego_vehicle: - print("无法生成主车辆,退出程序") - return - ego_vehicle.set_autopilot(True) - - # 生成相机(挂载到主车辆) - camera_bp = bp_lib.find('sensor.camera.rgb') - camera_bp.set_attribute('image_size_x', '640') - camera_bp.set_attribute('image_size_y', '480') - camera_bp.set_attribute('fov', '90') - camera_transform = carla.Transform(carla.Location(x=1.5, z=2.0)) # 相机位置 - camera = world.spawn_actor(camera_bp, camera_transform, attach_to=ego_vehicle) - - # 图像队列 - image_queue = queue.Queue() - camera.listen(lambda img: camera_callback(img, image_queue)) - - # 生成NPC车辆 - for _ in range(args.num_npcs): - npc_bp = random.choice([bp for bp in bp_lib.filter('vehicle') - if int(bp.get_attribute('number_of_wheels')) == 4]) - npc = world.try_spawn_actor(npc_bp, random.choice(spawn_points)) - if npc: - npc.set_autopilot(True) - - # 初始化YOLOv5检测器和SORT跟踪器 - detector = YOLO("yolov5s.pt") # 自动下载轻量版模型 - tracker = SimpleSORT(max_age=2, min_hits=2, iou_threshold=0.3) - - # 主循环 try: + client = carla.Client(cfg.host, cfg.port) + client.set_timeout(20.0) + world = client.get_world() + try: + tm = client.get_trafficmanager(8000) + tm.set_global_distance_to_leading_vehicle(2.0) + tm.set_respawn_dormant_vehicles(True) + tm.set_hybrid_physics_mode(True) + tm.set_hybrid_physics_radius(50.0) + tm.global_percentage_speed_difference(0) + except: pass + settings = world.get_settings() + settings.synchronous_mode = True + settings.fixed_delta_seconds = 0.05 + settings.substepping = True + settings.max_substep_delta_time = 0.01 + settings.max_substeps = 10 + world.apply_settings(settings) + + # 设置初始天气 + world.set_weather(WEATHER[cfg.default_weather]) + we = WeatherEnhancer(cfg); we.set_weather(cfg.default_weather); cw = cfg.default_weather + + # 获取生成点并安全生成自车 + spawn_points = world.get_map().get_spawn_points() + if not spawn_points: + print("❌ 无可用生成点") + return + + ego = safe_spawn_ego(world, spawn_points) + if ego is None: + return + ego.set_autopilot(True, tm_port=8000) + + # 生成NPC + npc_count = spawn_npcs(world, cfg.num_npcs, spawn_points) + print(f"✅ 生成NPC车辆:{npc_count} 辆") + + # 初始化相机 + cam_bp = world.get_blueprint_library().find('sensor.camera.rgb') + cam_bp.set_attribute('image_size_x', str(cfg.img_width)) + cam_bp.set_attribute('image_size_y', str(cfg.img_height)) + cam_bp.set_attribute('fov', '90') + cam_bp.set_attribute('sensor_tick', '0.05') + cam_t = carla.Transform(carla.Location(x=2.0, z=1.8)) + cam = world.spawn_actor(cam_bp, cam_t, attach_to=ego) + cam_q = queue.Queue(maxsize=1) + cam.listen(lambda img: cam_cb(img, cam_q)) + print(f"✅ 相机传感器启动成功 (ID: {cam.id})") + + # 初始化LiDAR + if cfg.use_lidar: + lidar_proc = LiDARProc(cfg) + lidar_bp = world.get_blueprint_library().find('sensor.lidar.ray_cast') + lidar_bp.set_attribute('channels', str(cfg.lidar_channels)) + lidar_bp.set_attribute('range', str(cfg.lidar_range)) + lidar_bp.set_attribute('points_per_second', str(cfg.lidar_points_per_second)) + lidar_bp.set_attribute('rotation_frequency', '20') + lidar_bp.set_attribute('sensor_tick', '0.05') + lidar_t = carla.Transform(carla.Location(x=0.0, z=2.5)) + lidar = world.spawn_actor(lidar_bp, lidar_t, attach_to=ego) + lidar.listen(lidar_proc.cb) + print(f"✅ LiDAR传感器启动成功 (ID: {lidar.id})") + + # 初始化YOLO + dev = "cuda" if torch.cuda.is_available() else "cpu" + print(f"✅ 使用设备: {dev}") + model = YOLO(cfg.yolo_model) + if cfg.yolo_quantize and dev=="cuda": + model = model.quantize() + print("✅ YOLO模型已量化") + + # 启动检测线程 + in_q = queue.Queue(maxsize=2); out_q = queue.Queue(maxsize=2) + det_thread = DetThread(model, cfg, we, in_q, out_q, dev) + det_thread.start() + print("✅ 推理线程已启动") + + # 初始化跟踪器和可视化 + tracker = SORT(cfg) + fb = FrameBuf((cfg.img_height, cfg.img_width, 3)) + fps_cnt = FPS(cfg.fps_window_size) + cv2.namedWindow("CARLA Object Tracking", cv2.WINDOW_NORMAL) + cv2.resizeWindow("CARLA Object Tracking", cfg.window_width, cfg.window_height) + + # 初始化3D可视化 + if cfg.use_3d_visualization and cfg.use_lidar and lidar_proc is not None: + vis = o3d.visualization.Visualizer() + vis.create_window(window_name="LiDAR Point Cloud", width=cfg.pcd_view_size, height=cfg.pcd_view_size) + print("✅ 3D点云可视化窗口已启动") + else: + vis = None + + # 主循环 + print("🚀 开始跟踪(按ESC退出,按W切换天气)") + fr_cnt = 0 + last_display_time = time.time() while True: - world.tick() # 推进仿真 - - # 调整 spectator 视角到主车辆上方 - ego_transform = ego_vehicle.get_transform() - world.get_spectator().set_transform(carla.Transform( - ego_transform.location + carla.Location(x=-8, z=12), - carla.Rotation(pitch=-45, yaw=ego_transform.rotation.yaw) - )) - - # 处理相机图像 - if not image_queue.empty(): - image = image_queue.get() - height, width, _ = image.shape - - # YOLOv5目标检测(仅保留车辆类) - results = detector.predict(image, conf=0.4) # 置信度阈值0.4 - boxes, scores = [], [] - for result in results: - for box, cls, conf in zip(result.boxes.xyxy, result.boxes.cls, result.boxes.conf): - cls_id = int(cls) - # COCO数据集车辆类:2=car, 5=bus, 7=truck - if cls_id in [2, 5, 7]: - boxes.append(box.numpy()) # [x1, y1, x2, y2] - scores.append(conf.numpy()) - - # 转换为numpy数组 - boxes = np.array(boxes) if boxes else np.array([]) - scores = np.array(scores) if scores else np.array([]) - - # 目标跟踪 - tracked_boxes, track_ids = [], [] - if len(boxes) > 0: - # 跟踪器输入格式:[x1, y1, x2, y2, score] - detections = np.hstack((boxes, scores.reshape(-1, 1))) - tracked_boxes, track_ids = tracker.update(detections) - - # 绘制跟踪结果 - if len(tracked_boxes) > 0: - display_img = image.copy() - display_img = draw_bounding_boxes(display_img, tracked_boxes, track_ids) - cv2.imshow("CARLA Vehicle Tracking (YOLOv5 + SORT)", display_img) - - # 按'q'退出 - if cv2.waitKey(1) & 0xFF == ord('q'): + world.tick() + + # 帧率控制 + current_time = time.time() + elapsed = current_time - last_display_time + target_interval = 1.0 / cfg.display_fps + if elapsed < target_interval: + time.sleep(target_interval - elapsed) + last_display_time = current_time + + # 获取相机图像 + try: + img = cam_q.get(timeout=0.1) + fb.update(img) + except: + img = fb.get() + + # 提交推理任务 + if not in_q.full(): + in_q.put(img.copy()) + + # 获取检测结果 + dets = np.array([]) + try: + _, dets = out_q.get_nowait() + except: + pass + + # LiDAR检测 + lidar_dets = lidar_proc.detect() if (cfg.use_lidar and lidar_proc) else [] + + # 更新跟踪器 + boxes, ids, cls_ids = tracker.update(dets, (cfg.img_width//2, cfg.img_height//2), lidar_dets) + + # 性能计算 + fps = fps_cnt.update() + cpu = psutil.cpu_percent(); mem = psutil.virtual_memory().percent + gpu = torch.cuda.utilization() if (torch.cuda.is_available() and hasattr(torch.cuda,'utilization')) else 0.0 + perf = {'cpu':cpu, 'mem':mem, 'gpu':gpu, 'fps':fps, 'avg_fps':fps} + + # 绘制可视化(使用新的检测框样式) + di = draw(img, boxes, ids, cls_ids, tracker.tracks, fps=fps, det_cnt=len(dets), cfg=cfg, w=cw, perf=perf) + cv2.imshow("CARLA Object Tracking", di) + + # 3D点云更新 + if cfg.use_3d_visualization and vis and lidar_proc: + pcd = lidar_proc.get_3d() + if pcd: + vis.clear_geometries() + vis.add_geometry(pcd) + vis.poll_events() + vis.update_renderer() + + # 数据记录 + recorder.record(tracker.tracks, dets, fps) + if cfg.save_screenshots and fr_cnt%30==0: + recorder.save_ss(di, cw) + + # 键盘事件处理 + key = cv2.waitKey(1) & 0xFF + if key ==27: + print("🛑 用户按下ESC,退出程序") break - - except KeyboardInterrupt: - print("用户中断程序") + elif key == ord('w') or key == ord('W'): + wl = list(WEATHER.keys()) + cwi = wl.index(cw) + cw = wl[(cwi+1)%len(wl)] + world.set_weather(WEATHER[cw]) + we.set_weather(cw) + print(f"🌤️ 已切换天气到: {cw} (可选:{wl})") + fr_cnt +=1 + + except KeyboardInterrupt: + print("🛑 用户中断程序") + except Exception as e: + print(f"❌ 运行错误: {str(e)}") finally: - # 清理资源 - camera.stop() + # 资源清理 + print("🧹 开始清理资源...") + + # 停止检测线程 + if det_thread: + det_thread.stop() + det_thread.join(timeout=2.0) + + # 关闭3D可视化窗口 + if vis is not None: + try: + vis.destroy_window() + except: + pass + + # 关闭OpenCV窗口 + cv2.destroyAllWindows() + + # 关闭数据记录 + recorder.close() + + # 销毁LiDAR + if lidar and lidar.is_alive: + try: + lidar.stop() + lidar.destroy() + print("✅ LiDAR已销毁") + except Exception as e: + print(f"⚠️ 销毁LiDAR失败: {e}") + + # 销毁相机 + if cam and cam.is_alive: + try: + cam.stop() + cam.destroy() + print("✅ 相机已销毁") + except Exception as e: + print(f"⚠️ 销毁相机失败: {e}") + + # 销毁自车 + if ego and ego.is_alive: + try: + ego.destroy() + print("✅ 自车已销毁") + except Exception as e: + print(f"⚠️ 销毁自车失败: {e}") + + # 清理所有NPC和传感器 clear_actors(world) - # 关闭同步模式 + + # 恢复CARLA设置 + settings = world.get_settings() settings.synchronous_mode = False world.apply_settings(settings) - cv2.destroyAllWindows() + print("✅ 所有资源清理完成") if __name__ == "__main__": + # 配置日志 + logger.remove() + logger.add(sys.stdout, format="{time:HH:mm:ss} | {level: <8} | {message}", level="INFO") + logger.add(f"track_log_{datetime.now().strftime('%Y%m%d_%H%M%S')}.log", rotation="100 MB", retention="7 days", format="{time:YYYY-MM-DD HH:mm:ss} | {level: <8} | {message}", level="DEBUG") + + # 启动主程序 main() \ No newline at end of file diff --git a/src/tracking_car/requirements.txt b/src/tracking_car/requirements.txt new file mode 100644 index 0000000000..7765ee6a8d --- /dev/null +++ b/src/tracking_car/requirements.txt @@ -0,0 +1,16 @@ +# 核心依赖 +carla>=0.9.14 +opencv-python>=4.8.0 +numpy>=1.24.0 +torch>=2.0.0 +ultralytics>=8.0.0 +open3d>=0.17.0 +psutil>=5.9.0 +scipy>=1.10.0 +scikit-learn>=1.2.0 +numba>=0.57.0 +loguru>=0.7.0 +pyyaml>=6.0.0 +# 可选依赖 +matplotlib>=3.7.0 +pandas>=2.0.0 \ No newline at end of file