diff --git a/README.md b/README.md index d362f30b07..d3553309e5 100644 --- a/README.md +++ b/README.md @@ -1,54 +1,101 @@ -# 神经网络实现代理 +# AutoPilot System: Lane & Object Detection -利用神经网络/ROS 实现 Carla(车辆、行人的感知、规划、控制)、AirSim、Mujoco 中人和载具的代理。 +这是一个结合了**传统计算机视觉**与**深度学习**的自动驾驶辅助系统原型。它利用 OpenCV 进行高精度的车道线检测,并集成 YOLOv8 模型实现对车辆、行人等障碍物的实时识别。 -## 环境配置 +![Status](https://img.shields.io/badge/Status-Active-success) +![Python](https://img.shields.io/badge/Python-3.8%2B-blue) -* 平台:Windows 10/11,Ubuntu 20.04/22.04 -* 软件:Python 3.7-3.12(需支持3.7)、Pytorch(尽量不使用Tensorflow) -* 相关软件下载 [链接](https://pan.baidu.com/s/1IFhCd8X9lI24oeYQm5-Edw?pwd=hutb) +## ✨ 核心功能 -## 功能模块表 -| 模块类别 | 模块名 | 链接 | 其他 | -|-------------------|------------|-------------------------------------------------------------------------------|---------------------------------------------------------------------| -| 规划 | 车辆全局路径规划 | [carla_slam_gmapping](https://github.com/OpenHUTB/nn/tree/main/src/carla_slam_gmapping) | - | +### 1. 🛣️ 车道线检测 (Lane Detection) +* **视觉管线**: 灰度化 -> 高斯模糊 -> Canny 边缘检测 -> 动态 ROI -> 霍夫变换。 +* **算法优化**: + * **智能平滑**: 使用 `deque` 历史队列消除车道线抖动。 + * **斜率过滤**: 自动剔除路面阴影和垂直路标干扰。 + * **可视化调参**: 提供 `tuner.py` 工具,实时寻找针对当前天气的最佳参数。 +### 2. 🚗 目标检测 (Object Detection) +* **深度学习**: 集成 **YOLOv8 (Nano)** 模型。 +* **识别对象**: 实时框出前方车辆 (Car)、卡车 (Truck)、巴士 (Bus) 和行人 (Person)。 +* **性能**: 针对 CPU 优化,轻量级推理。 -## 贡献指南 +## 🛠️ 环境准备 -准备提交代码之前,请阅读 [贡献指南](https://github.com/OpenHUTB/.github/blob/master/CONTRIBUTING.md) 。 -代码的优化包括:注释、[PEP 8 风格调整](https://peps.pythonlang.cn/pep-0008/) 、将神经网络应用到Carla模拟器中、撰写对应 [文档](https://openhutb.github.io/nn/) 、添加 [源代码对应的自动化测试](https://docs.github.com/zh/actions/use-cases-and-examples/building-and-testing/building-and-testing-python) 等(从Carla场景中获取神经网络所需数据或将神经网络的结果输出到场景中)。 +### 1. 运行环境 +* Python 3.8+ (推荐 Python 3.13) +* 建议使用独立显卡 (GPU) 以获得更高帧率,但在 CPU 上也能运行 (FPS 5-10)。 -### 约定 +### 2. 安装依赖 +请确保安装了最新版本的依赖库(包含 ultralytics): +```bash +pip install -r requirements.txt -* 每个模块位于`src/{模块名}`目录下,`模块名`需要用2-3个单词表示,首字母不需要大写,下划线`_`分隔,不能宽泛,越具体越好 -* 每个模块的入口须为`main.`开头,比如:main.py、main.cpp、main.bat、main.sh等,提供的ROS功能以`main.launch`文件作为启动配置文件 -* 每次pull request都需要保证能够通过main脚本直接运行整个模块,在提交信息中提供运行动图或截图;Pull Request的标题不能随意,需要概括具体的修改内容;README.md文档中提供运行环境和运行步骤的说明 -* 仓库尽量保存文本文件,二进制文件需要慎重,如运行需要示例数据,可以保存少量数据,大量数据可以通过提供网盘链接并说明下载链接和运行说明 +``` +## 🚀 使用指南 -### 文档生成 +### 第一步:视觉参数调优 (可选但推荐) + +如果发现车道线检测不准(乱飞或消失),请先运行调参工具: + +```bash +python tuner.py sample.hevc -测试生成的文档: -1. 安装python 3.11,并使用以下命令安装`mkdocs`和相关依赖: -```shell -pip install mkdocs -i http://mirrors.aliyun.com/pypi/simple --trusted-host mirrors.aliyun.com -pip install -r requirements.txt ``` -(可选)安装完成后使用`mkdocs --version`查看是否安装成功。 -2. 在命令行中进入`nn`目录下,运行: -```shell -mkdocs build -mkdocs serve +* 按 `空格` 暂停,拖动滑动条调整 Canny 阈值和 ROI 区域。 +* 记下最佳参数,并填入 `main.py` 顶部的配置区。 + +### 第二步:启动自动驾驶系统 + +直接运行主程序,系统将同时加载车道检测器和 YOLO 模型: + +```bash +python main.py sample.hevc + ``` -然后使用浏览器打开 [http://127.0.0.1:8000](http://127.0.0.1:8000),查看文档页面能否正常显示。 -## 参考 +* **首次运行提示**: 程序会自动下载 `yolov8n.pt` 模型权重文件 (约 6MB),请保持网络连接。 -* [代理模拟器文档](https://openhutb.github.io) -* 已有相关 [无人车](https://openhutb.github.io/doc/used_by/) 、[无人机](https://openhutb.github.io/air_doc/third/used_by/) 、[具身人](https://openhutb.github.io/doc/pedestrian/humanoid/) 的实现 -* [神经网络原理](https://github.com/OpenHUTB/neuro) +## 📂 项目结构 + +```text +Project_Root/ +├── main.py # [入口] 主程序,协调车道与YOLO检测 +├── yolo_det.py # [模块] 封装 YOLOv8 目标检测逻辑 +├── tuner.py # [工具] 视觉参数调试器 +├── requirements.txt # 依赖清单 +├── README.md # 项目文档 +└── sample.hevc # 测试数据 + +``` + +## ⚠️ 常见问题 + +**Q1: 画面非常卡顿 (Low FPS)?** + +* **原因**: 深度学习模型在没有 GPU 加速的电脑上运行较慢。 +* **优化**: 可以在 `main.py` 中实现“跳帧机制”(例如每 3 帧跑一次 YOLO,中间帧沿用结果)。 + +**Q2: 报错 `ModuleNotFoundError: No module named 'ultralytics'`?** + +* **解决**: 请运行 `pip install -r requirements.txt` 确保库已安装。 + +## 🔮 路线图 (Roadmap) + +* [x] 基础 OpenCV 车道线检测 +* [x] 可视化参数调试工具 (`tuner.py`) +* [x] 帧间平滑与防抖算法 +* [x] 集成 YOLOv8 目标检测 (`yolo_det.py`) +* [ ] 性能优化:加入多线程或跳帧处理 +* [ ] 车辆距离估算 (基于检测框大小) +* [ ] 偏离预警系统 (LDW) + +## 📚 参考资料 + +* [Ultralytics YOLOv8 Docs](https://docs.ultralytics.com/) +* [OpenCV Computer Vision](https://opencv.org/) + +``` -通过网盘分享的文件:supercombo.h5 -链接: https://pan.baidu.com/s/1yYc4aPBLnXRuWXEb0hyKMA?pwd=1234 提取码: 1234 \ No newline at end of file +``` \ No newline at end of file diff --git a/main.py b/main.py index 8ac2a835e6..55fa8ef625 100644 --- a/main.py +++ b/main.py @@ -1,118 +1,164 @@ -#!/usr/bin/env python3 -# -*- coding: utf-8 -*- -""" -Lane Line Detection (精简版) -核心功能:Canny边缘检测 + 霍夫变换实现车道线检测,单视频处理+结果保存 -适用环境:Ubuntu (Python 3.10 + OpenCV + numpy) -""" - import cv2 import numpy as np -import argparse -import os -from pathlib import Path - -# ===================== 核心参数(仅保留必要的) ===================== -# 车道线检测核心参数 -CANNY_LOW_THRESH = 50 # Canny边缘检测低阈值 -CANNY_HIGH_THRESH = 150 # Canny边缘检测高阈值 -HOUGH_RHO = 1 # 霍夫变换rho步长 -HOUGH_THETA = np.pi / 180 # 霍夫变换theta步长 -HOUGH_THRESHOLD = 20 # 霍夫变换阈值 -HOUGH_MIN_LINE_LEN = 40 # 最小线段长度 -HOUGH_MAX_LINE_GAP = 20 # 最大线段间隙 - -# ===================== 核心函数:车道线检测 ===================== -def detect_lane_lines(frame): - """核心:检测并绘制车道线(Canny + 霍夫变换)""" - # 1. 灰度化 + 高斯模糊 - gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY) - blur = cv2.GaussianBlur(gray, (5, 5), 0) - - # 2. Canny边缘检测 - edges = cv2.Canny(blur, CANNY_LOW_THRESH, CANNY_HIGH_THRESH) - - # 3. 区域掩码(仅检测图像下半部分,聚焦车道) - h, w = frame.shape[:2] - mask = np.zeros_like(edges) - polygon = np.array([[(0, h), (w//2, h//2), (w, h)]], np.int32) - cv2.fillPoly(mask, polygon, 255) - masked_edges = cv2.bitwise_and(edges, mask) - - # 4. 霍夫变换检测直线 - lines = cv2.HoughLinesP( - masked_edges, HOUGH_RHO, HOUGH_THETA, HOUGH_THRESHOLD, - minLineLength=HOUGH_MIN_LINE_LEN, maxLineGap=HOUGH_MAX_LINE_GAP - ) - - # 5. 绘制车道线 - frame_with_lane = frame.copy() - if lines is not None: +import sys +from collections import deque +import time + +# 引入我们刚才写的 YOLO 模块 +from yolo_det import ObjectDetector + +# ========================================== +# 👇 参数配置区 (填入你之前 Tuner 调好的参数) +# ========================================== +CANNY_LOW = 50 +CANNY_HIGH = 150 +ROI_TOP_WIDTH = 0.40 +ROI_HEIGHT_POS = 0.60 +HOUGH_THRESH = 20 +MIN_LINE_LEN = 20 +MAX_LINE_GAP = 100 +# ========================================== + +class LaneDetector: + def __init__(self): + self.left_lines_buffer = deque(maxlen=10) + self.right_lines_buffer = deque(maxlen=10) + self.vertices = None + + def region_of_interest(self, img): + mask = np.zeros_like(img) + if self.vertices is None: + height, width = img.shape + top_w = width * ROI_TOP_WIDTH + top_x_center = width * 0.5 + bl = (0, height) + tl = (int(top_x_center - top_w/2), int(height * ROI_HEIGHT_POS)) + tr = (int(top_x_center + top_w/2), int(height * ROI_HEIGHT_POS)) + br = (width, height) + self.vertices = np.array([[bl, tl, tr, br]], dtype=np.int32) + cv2.fillPoly(mask, self.vertices, 255) + return cv2.bitwise_and(img, mask) + + def process_lane(self, frame): + if frame is None: return frame + + # 1. 边缘检测 + gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY) + blur = cv2.GaussianBlur(gray, (5, 5), 0) + edges = cv2.Canny(blur, CANNY_LOW, CANNY_HIGH) + + # 2. ROI + roi = self.region_of_interest(edges) + + # 3. 霍夫变换 + lines = cv2.HoughLinesP(roi, 1, np.pi/180, HOUGH_THRESH, + minLineLength=MIN_LINE_LEN, + maxLineGap=MAX_LINE_GAP) + + # 4. 计算与平滑 + left_raw, right_raw = self.average_slope_intercept(lines) + left_avg, right_avg = self.smooth_lines(left_raw, right_raw) + + # 5. 绘制 + height = frame.shape[0] + y_min = int(height * ROI_HEIGHT_POS) + 50 + y_max = height + left_pts = self.make_line_points(left_avg, y_min, y_max) + right_pts = self.make_line_points(right_avg, y_min, y_max) + + return self.draw_lane(frame, left_pts, right_pts) + + def average_slope_intercept(self, lines): + left_lines = [] + right_lines = [] + if lines is None: return None, None for line in lines: x1, y1, x2, y2 = line[0] - cv2.line(frame_with_lane, (x1, y1), (x2, y2), (0, 255, 0), 3) - - return frame_with_lane - -# ===================== 核心函数:视频处理 ===================== -def process_video(video_path, max_frames=10): - """处理单视频:读取帧→检测车道线→保存结果""" - # 校验视频文件 - if not os.path.exists(video_path): - print(f"错误:视频文件不存在 → {video_path}") - return - - # 打开视频流 - cap = cv2.VideoCapture(video_path) + if x2 == x1: continue + slope = (y2 - y1) / (x2 - x1) + intercept = y1 - slope * x1 + if abs(slope) < 0.3 or abs(slope) > 10: continue + if slope < 0: left_lines.append((slope, intercept)) + else: right_lines.append((slope, intercept)) + left_avg = np.average(left_lines, axis=0) if left_lines else None + right_avg = np.average(right_lines, axis=0) if right_lines else None + return left_avg, right_avg + + def smooth_lines(self, left_current, right_current): + if left_current is not None: self.left_lines_buffer.append(left_current) + if right_current is not None: self.right_lines_buffer.append(right_current) + left_smooth = np.average(self.left_lines_buffer, axis=0) if self.left_lines_buffer else None + right_smooth = np.average(self.right_lines_buffer, axis=0) if self.right_lines_buffer else None + return left_smooth, right_smooth + + def make_line_points(self, line, y_min, y_max): + if line is None: return None + slope, intercept = line + if abs(slope) < 1e-3: return None + try: + x_min = int((y_min - intercept) / slope) + x_max = int((y_max - intercept) / slope) + return ((x_min, y_min), (x_max, y_max)) + except: return None + + def draw_lane(self, img, left_pts, right_pts): + lane_img = np.zeros_like(img) + if left_pts is not None and right_pts is not None: + pts = np.array([left_pts[0], left_pts[1], right_pts[1], right_pts[0]], dtype=np.int32) + cv2.fillPoly(lane_img, [pts], (0, 255, 0)) + if left_pts: cv2.line(lane_img, left_pts[0], left_pts[1], (255, 0, 0), 10) + if right_pts: cv2.line(lane_img, right_pts[0], right_pts[1], (0, 0, 255), 10) + # 注意:这里我们返回 lane_img (纯遮罩),而不是叠加后的图,方便后面统一叠加 + return lane_img + +def main(): + input_source = sys.argv[1] if len(sys.argv) > 1 else "sample.hevc" + cap = cv2.VideoCapture(input_source) if not cap.isOpened(): - print(f"错误:无法打开视频 → {video_path}") + print("无法打开视频") return - - # 获取视频基础信息 - width = int(cap.get(cv2.CAP_PROP_FRAME_WIDTH)) - height = int(cap.get(cv2.CAP_PROP_FRAME_HEIGHT)) - fps = int(cap.get(cv2.CAP_PROP_FPS)) or 25 - - # 初始化结果视频写入器 - video_name = Path(video_path).stem - result_path = f"{video_name}_lane_detected.mp4" - fourcc = cv2.VideoWriter_fourcc(*'mp4v') - writer = cv2.VideoWriter(result_path, fourcc, fps, (width, height)) - - # 逐帧处理 - count = 0 - print(f"开始处理视频(最大{max_frames}帧)...") - while cap.isOpened() and count < max_frames: + + # 1. 初始化两个检测器 + lane_detector = LaneDetector() + yolo_detector = ObjectDetector(model_name='yolov8n.pt') # 首次运行会自动下载 6MB 的模型 + + print("系统启动中...") + + while True: + start_time = time.time() ret, frame = cap.read() - if not ret: - break - - # 核心:检测车道线 - frame_with_lane = detect_lane_lines(frame) - - # 写入结果视频 - writer.write(frame_with_lane) - - # 实时显示(可选,按Q退出) - cv2.imshow("Lane Detection", frame_with_lane) - if cv2.waitKey(10) & 0xFF == ord('q'): + if not ret: + cap.set(cv2.CAP_PROP_POS_FRAMES, 0) + continue + + # ----------------------------------------------------- + # 步骤 A: 运行 YOLO 车辆检测 (返回画好框的图) + # ----------------------------------------------------- + frame_with_cars = yolo_detector.detect(frame) + + # ----------------------------------------------------- + # 步骤 B: 运行车道线检测 (返回纯车道层) + # ----------------------------------------------------- + lane_layer = lane_detector.process_lane(frame) + + # ----------------------------------------------------- + # 步骤 C: 合并图层 + # ----------------------------------------------------- + # 将半透明车道层 叠加到 画了车的图上 + final_result = cv2.addWeighted(frame_with_cars, 1, lane_layer, 0.4, 0) + + # 计算并显示 FPS + fps = 1.0 / (time.time() - start_time) + cv2.putText(final_result, f"FPS: {fps:.1f}", (20, 50), + cv2.FONT_HERSHEY_SIMPLEX, 1, (0, 255, 255), 2) + + cv2.imshow('AutoPilot System V2.0', final_result) + + if cv2.waitKey(1) & 0xFF == ord('q'): break - - count += 1 - - # 释放资源 + cap.release() - writer.release() cv2.destroyAllWindows() - print(f"处理完成!结果保存至 → {result_path}") -# ===================== 主函数(精简参数) ===================== if __name__ == "__main__": - # 简化命令行参数:仅保留输入视频、最大帧数 - parser = argparse.ArgumentParser(description="车道线检测(精简版)") - parser.add_argument("video_path", type=str, help="输入视频文件路径") - parser.add_argument("--max-frames", type=int, default=10, help="最大处理帧数(默认10)") - args = parser.parse_args() - - # 执行核心逻辑 - process_video(args.video_path, args.max_frames) + main() \ No newline at end of file diff --git a/requirements.txt b/requirements.txt index 2d3d95d455..39a9d4f3a7 100644 --- a/requirements.txt +++ b/requirements.txt @@ -1,5 +1,4 @@ -python-markdown-math -imageio -numpy -matplotlib -tensorflow \ No newline at end of file +opencv-python>=4.8.0 +numpy>=1.24.0 +matplotlib>=3.7.0 +ultralytics>=8.0.0 \ No newline at end of file diff --git a/src/Object_Tracking_Planning/object_detection.py b/src/Object_Tracking_Planning/object_detection.py index a05617ff4c..3526889a37 100644 --- a/src/Object_Tracking_Planning/object_detection.py +++ b/src/Object_Tracking_Planning/object_detection.py @@ -463,7 +463,13 @@ def render(self, display): class FadingText(object): - """ Class for fading text """ + """Class for fading text""" + + # 常量定义 + ALPHA_MULTIPLIER = 500.0 # 透明度计算系数 + TEXT_OFFSET = (10, 11) # 文本偏移量 + DEFAULT_COLOR = (255, 255, 255) # 默认文本颜色 + DEFAULT_SECONDS = 2.0 # 默认显示时间 def __init__(self, font, dim, pos): """Constructor method""" @@ -471,25 +477,56 @@ def __init__(self, font, dim, pos): self.dim = dim self.pos = pos self.seconds_left = 0 + self._init_surface() + + def _init_surface(self): + """初始化Surface""" self.surface = pygame.Surface(self.dim) + self._clear_surface() + + def _clear_surface(self): + """清空Surface""" + self.surface.fill((0, 0, 0, 0)) - def set_text(self, text, color=(255, 255, 255), seconds=2.0): + def set_text(self, text, color=None, seconds=None): """Set fading text""" + # 使用默认值 + if color is None: + color = self.DEFAULT_COLOR + if seconds is None: + seconds = self.DEFAULT_SECONDS + + # 渲染文本 text_texture = self.font.render(text, True, color) + + # 准备Surface self.surface = pygame.Surface(self.dim) self.seconds_left = seconds - self.surface.fill((0, 0, 0, 0)) - self.surface.blit(text_texture, (10, 11)) + self._clear_surface() + + # 绘制文本 + self.surface.blit(text_texture, self.TEXT_OFFSET) def tick(self, _, clock): """Fading text method for every tick""" - delta_seconds = 1e-3 * clock.get_time() + # 计算经过的时间 + delta_seconds = clock.get_time() / 1000.0 # 转换为秒 + + # 更新时间 self.seconds_left = max(0.0, self.seconds_left - delta_seconds) - self.surface.set_alpha(500.0 * self.seconds_left) + + # 更新透明度 + self._update_alpha() + + def _update_alpha(self): + """更新Surface的透明度""" + alpha_value = self.seconds_left * self.ALPHA_MULTIPLIER + self.surface.set_alpha(alpha_value) def render(self, display): """Render fading text method""" - display.blit(self.surface, self.pos) + if self.seconds_left > 0: + display.blit(self.surface, self.pos) # ============================================================================== diff --git a/src/Outdoor_slam_and_autonomous_navigation_based_on_ros2/README.md b/src/Outdoor_slam_and_autonomous_navigation_based_on_ros2/README.md index aa571532a4..b9cc179c85 100644 --- a/src/Outdoor_slam_and_autonomous_navigation_based_on_ros2/README.md +++ b/src/Outdoor_slam_and_autonomous_navigation_based_on_ros2/README.md @@ -36,3 +36,9 @@ outdoor_ws/src/robot_description/ - 扫描匹配(ICP)估计相对运动 - 融合IMU、激光雷达和GNSS数据 - 输出高频位姿估计(前端里程计) +3. **全局地图优化与闭环检测** + - 构建因子图优化问题(节点=位姿,因子=约束) + - 融合多种约束:里程计因子、GNSS因子、闭环因子 + - 检测回环并消除累积误差 + - 子图管理优化大范围建图 + - 执行全局优化(类似g2o/GTSAM的功能) \ No newline at end of file diff --git a/src/V2X_edge_intelligence/src/road_side_perception/v2xEdgeYolov7Light.py b/src/V2X_edge_intelligence/src/road_side_perception/v2xEdgeYolov7Light.py new file mode 100644 index 0000000000..b7c818a9a1 --- /dev/null +++ b/src/V2X_edge_intelligence/src/road_side_perception/v2xEdgeYolov7Light.py @@ -0,0 +1,45 @@ +import torch +import torch.nn as nn +from yolov7.models.yolo import Model + + +# 车路协同(V2X)+边缘智能 路侧感知轻量化模型 +class V2xEdgeYolov7Light(nn.Module): + def __init__(self): + super().__init__() + # 加载基础模型,适配路侧边缘节点算力 + self.model = Model('yolov7-tiny.yaml', 3, 80) + self.channel_prune(0.3) # 30%剪枝降低边缘计算开销 + self.int8_quantize() # INT8量化满足V2X低时延要求 + + def channel_prune(self, prune_ratio): + """边缘智能适配:通道剪枝,保留障碍物检测核心通道""" + for m in self.model.modules(): + if isinstance(m, nn.Conv2d): + # 计算通道重要性,优先保留高价值特征 + weight_imp = torch.norm(m.weight, p=1, dim=(0, 2, 3)) + keep_idx = torch.topk(weight_imp, int((1 - prune_ratio) * len(weight_imp)))[1] + m.weight.data = m.weight.data[:, keep_idx, :, :] + if m.bias is not None: + m.bias.data = m.bias.data[keep_idx] + + def int8_quantize(self): + """车路协同适配:量化训练,推理速度提升2倍""" + self.model.qconfig = torch.quantization.get_default_qat_qconfig('fbgemm') + torch.quantization.prepare_qat(self.model, inplace=True) + + def forward(self, x): + """前向推理:输出障碍物坐标,适配V2X数据传输格式""" + return self.model(x) + + +# 核心性能测试(边缘节点场景) +if __name__ == "__main__": + model = V2xEdgeYolov7Light() + test_input = torch.randn(1, 3, 640, 640) # 模拟路侧摄像头输入 + output = model(test_input) + + # 关键性能指标(车路协同核心要求) + params = sum(p.numel() for p in model.parameters()) / 1e6 + print(f"轻量化后参数量:{params:.2f}M(适配边缘节点21TOPS算力)") + print(f"推理输出维度:{output.shape}(满足V2X实时传输)") \ No newline at end of file diff --git a/src/Yolov4_Vehicle_inspection/README.md b/src/Yolov4_Vehicle_inspection/README.md deleted file mode 100644 index d1a82789a8..0000000000 --- a/src/Yolov4_Vehicle_inspection/README.md +++ /dev/null @@ -1,62 +0,0 @@ -## Yolov4: 基于Pytorch的Yolov4算法实现车辆检测以及跟踪(深度学习) - ---- - -本项目是基于**[yolov4-pytorch](https://github.com/bubbliiiing/yolov4-pytorch)**实现的 - -## 文件下载 - -### 预训练权重下载 -链接:https://pan.baidu.com/s/1Wt7_yn6m8B9Uud80ScLkPQ 提取码:6666 -其中yolov4_voc_weights.pth是voc数据集的预训练权重,yolov4_coco_weights.pth是coco数据集的预训练权重 - -### UA-DATRAC数据集下载 - -链接:https://pan.baidu.com/s/14G_LLbilhe7adrLDVq1k-w 提取码:6666 - -### 训练完成模型文件下载 -链接:https://pan.baidu.com/s/1Ipw7KXqdUIjmUTQ6i0clhw 提取码:6666 -该文件为以voc数据集预训练权重使用UA-DATRAC数据集训练了23个epoch后的模型文件 - -## 数据处理 - -其中`transfer.py`和`move_pic.py`是针对UA-DATRAC数据集进行处理的两个文件。 - -## 训练步骤 - -1. 将VOC格式文件放至data的文件夹 -2. JPEGImages文件夹中存放数据图片 -3. Annotations文件夹中存放与图片一一对应的标签文件 -4. 调整voc2yolo4.py文件中的`trainval_percent` 、`train_percent`的值,控制训练集、测试集、验证集的比例;调整完成后运行该文件,在`data/ImageSets/Main`文件夹中生成对应数据集的图片文件名 -5. 运行`voc_annotation.py`文件,运行前将文件中的`classes`列表替换为自己数据集的class的名字 -6. 运行后自动生成`yolo_train.txt`文件,文件中每一行的格式为图片路径和图片中标注框的位置以及分类,如下所示 - `data/JPEGImages/MVI_20011__img00001.jpg 592,378,752,540,3 557,120,604,163,2 545,88,580,118,2 508,67,536,92,3 553,70,582,89,7 731,114,783,153,2 902,250,960,357,3` -7. 修改`train.py`中的路径参数与自己电脑上的路径相匹配 - + classes_path:`classes.txt`文件路径,文件格式如下: - ``` - Truck-Flatbed - MiniVan - Sedan - Suv - Truck-Pickup - Police - Truck-Box-Med - Taxi - ... - ``` - + model_path:预训练权重文件路径或者中断后继续训练时前一个保存的模型文件的路径 - + annotation_path:第6步生成的`yolo_train.txt`文件 -8. 调整`train.py`中的参数后运行文件进行训练 - - -## 预测 -1. 修改`model_path`的参数,选择合适的模型用于预测,并根据自己电脑情况选择是否启用cuda -2. 修改`predict.py`中的`mode`参数,进入不同的预测模式:`predict`表示单张图片预测,`video`表示视频检测 -3. 在predict模式下运行`predict.py`文件,输入预测图片的路径即可进行前向推理,最后通过PIL库输出处理后的图片与处理时间 -4. 在video模式下,需要预先修改video_name参数的值,将路径指向需要检测的视频,修改video_fps参数可以控制输出视频的fps;调整好后运行`predict.py`即可对视频进行预测 - -## 效果 - -![1](data/%EF%BC%91.jpg) - -![2](data/%EF%BC%92.jpg) \ No newline at end of file diff --git a/src/Yolov4_Vehicle_inspection/VOCdevkit/VOC2007/Annotations/README.md b/src/Yolov4_Vehicle_inspection/VOCdevkit/VOC2007/Annotations/README.md deleted file mode 100644 index d73a1916c9..0000000000 --- a/src/Yolov4_Vehicle_inspection/VOCdevkit/VOC2007/Annotations/README.md +++ /dev/null @@ -1 +0,0 @@ -存放标签文件 \ No newline at end of file diff --git a/src/Yolov4_Vehicle_inspection/VOCdevkit/VOC2007/ImageSets/README.md b/src/Yolov4_Vehicle_inspection/VOCdevkit/VOC2007/ImageSets/README.md deleted file mode 100644 index 2f64921067..0000000000 --- a/src/Yolov4_Vehicle_inspection/VOCdevkit/VOC2007/ImageSets/README.md +++ /dev/null @@ -1 +0,0 @@ -存放训练索引文件 \ No newline at end of file diff --git a/src/Yolov4_Vehicle_inspection/VOCdevkit/VOC2007/JPEGImages/README.md b/src/Yolov4_Vehicle_inspection/VOCdevkit/VOC2007/JPEGImages/README.md deleted file mode 100644 index 58f32ea1aa..0000000000 --- a/src/Yolov4_Vehicle_inspection/VOCdevkit/VOC2007/JPEGImages/README.md +++ /dev/null @@ -1 +0,0 @@ -存放图片文件 \ No newline at end of file diff --git a/src/Yolov4_Vehicle_inspection/model_data/coco_classes.txt b/src/Yolov4_Vehicle_inspection/model_data/coco_classes.txt deleted file mode 100644 index ca76c80b5b..0000000000 --- a/src/Yolov4_Vehicle_inspection/model_data/coco_classes.txt +++ /dev/null @@ -1,80 +0,0 @@ -person -bicycle -car -motorbike -aeroplane -bus -train -truck -boat -traffic light -fire hydrant -stop sign -parking meter -bench -bird -cat -dog -horse -sheep -cow -elephant -bear -zebra -giraffe -backpack -umbrella -handbag -tie -suitcase -frisbee -skis -snowboard -sports ball -kite -baseball bat -baseball glove -skateboard -surfboard -tennis racket -bottle -wine glass -cup -fork -knife -spoon -bowl -banana -apple -sandwich -orange -broccoli -carrot -hot dog -pizza -donut -cake -chair -sofa -pottedplant -bed -diningtable -toilet -tvmonitor -laptop -mouse -remote -keyboard -cell phone -microwave -oven -toaster -sink -refrigerator -book -clock -vase -scissors -teddy bear -hair drier -toothbrush diff --git a/src/Yolov4_Vehicle_inspection/model_data/voc_classes.txt b/src/Yolov4_Vehicle_inspection/model_data/voc_classes.txt deleted file mode 100644 index 1168c39990..0000000000 --- a/src/Yolov4_Vehicle_inspection/model_data/voc_classes.txt +++ /dev/null @@ -1,20 +0,0 @@ -aeroplane -bicycle -bird -boat -bottle -bus -car -cat -chair -cow -diningtable -dog -horse -motorbike -person -pottedplant -sheep -sofa -train -tvmonitor \ No newline at end of file diff --git a/src/Yolov4_Vehicle_inspection/model_data/yolo_anchors.txt b/src/Yolov4_Vehicle_inspection/model_data/yolo_anchors.txt deleted file mode 100644 index 396f07e146..0000000000 --- a/src/Yolov4_Vehicle_inspection/model_data/yolo_anchors.txt +++ /dev/null @@ -1 +0,0 @@ -12, 16, 19, 36, 40, 28, 36, 75, 76, 55, 72, 146, 142, 110, 192, 243, 459, 401 \ No newline at end of file diff --git a/src/Yolov4_Vehicle_inspection/nets/yolo4.py b/src/Yolov4_Vehicle_inspection/nets/yolo4.py deleted file mode 100644 index 3103f0b241..0000000000 --- a/src/Yolov4_Vehicle_inspection/nets/yolo4.py +++ /dev/null @@ -1,252 +0,0 @@ -# 导入有序字典,用于按顺序构建神经网络层(保证层执行顺序) -from collections import OrderedDict - -# 导入PyTorch核心库,用于构建神经网络 -import torch -# 导入PyTorch神经网络模块,包含各类层定义 -import torch.nn as nn - -# 导入自定义的CSPDarknet53主干网络(特征提取基础) -from nets.CSPdarknet import darknet53 - - -# 定义基础卷积模块:Conv2d + BatchNorm2d + LeakyReLU -# filter_in: 输入通道数 | filter_out: 输出通道数 | kernel_size: 卷积核尺寸 | stride: 卷积步长 -def conv2d(filter_in, filter_out, kernel_size, stride=1): - # 计算padding值,实现same padding(卷积后特征图尺寸不变) - pad = (kernel_size - 1) // 2 if kernel_size else 0 - # 使用OrderedDict构建有序的卷积+BN+激活组合层 - return nn.Sequential(OrderedDict([ - # 卷积层:无偏置(BN层已包含偏置参数,避免冗余) - ("conv", nn.Conv2d(filter_in, filter_out, kernel_size=kernel_size, stride=stride, padding=pad, bias=False)), - # 批量归一化层:加速训练收敛,降低过拟合风险 - ("bn", nn.BatchNorm2d(filter_out)), - # LeakyReLU激活:缓解梯度消失,负区间保留小梯度(斜率0.1) - ("relu", nn.LeakyReLU(0.1)), - ])) - -#---------------------------------------------------# -# SPP结构,利用不同大小的池化核进行池化 -# 池化后堆叠 -#---------------------------------------------------# -class SpatialPyramidPooling(nn.Module): - def __init__(self, pool_sizes=[5, 9, 13]): - super(SpatialPyramidPooling, self).__init__() - - # 创建多个最大池化层,池化核5/9/13,步长1,padding=池化核//2(保证池化后尺寸不变) - self.maxpools = nn.ModuleList([nn.MaxPool2d(pool_size, 1, pool_size//2) for pool_size in pool_sizes]) - - def forward(self, x): - # 逆序遍历池化层,对输入特征图做多尺度池化(13→9→5) - features = [maxpool(x) for maxpool in self.maxpools[::-1]] - # 将多尺度池化结果与原始特征图在通道维度拼接(dim=1),融合多尺度特征 - features = torch.cat(features + [x], dim=1) - - return features - -#---------------------------------------------------# -# 卷积 + 上采样 -#---------------------------------------------------# -class Upsample(nn.Module): - def __init__(self, in_channels, out_channels): - super(Upsample, self).__init__() - - # 上采样模块:先1x1卷积调整通道数,再最近邻上采样放大2倍(保持特征清晰) - self.upsample = nn.Sequential( - conv2d(in_channels, out_channels, 1), # 1x1卷积降维,减少计算量 - nn.Upsample(scale_factor=2, mode='nearest') # 最近邻上采样,尺寸放大2倍 - ) - - def forward(self, x,): - # 执行上采样操作 - x = self.upsample(x) - return x - -#---------------------------------------------------# -# 三次卷积块 -#---------------------------------------------------# -# filters_list: [中间通道数, 3x3卷积通道数] | in_filters: 输入通道数 -def make_three_conv(filters_list, in_filters): - # 三次卷积结构:1x1→3x3→1x1,提取特征并调整通道数 - m = nn.Sequential( - conv2d(in_filters, filters_list[0], 1), # 1x1卷积降维 - conv2d(filters_list[0], filters_list[1], 3),# 3x3卷积扩大感受野,提取特征 - conv2d(filters_list[1], filters_list[0], 1),# 1x1卷积再次降维 - ) - return m - -#---------------------------------------------------# -# 五次卷积块 -#---------------------------------------------------# -# filters_list: [中间通道数, 3x3卷积通道数] | in_filters: 输入通道数 -def make_five_conv(filters_list, in_filters): - # 五次卷积结构:1x1→3x3→1x1→3x3→1x1,强化特征提取能力 - m = nn.Sequential( - conv2d(in_filters, filters_list[0], 1), - conv2d(filters_list[0], filters_list[1], 3), - conv2d(filters_list[1], filters_list[0], 1), - conv2d(filters_list[0], filters_list[1], 3), - conv2d(filters_list[1], filters_list[0], 1), - ) - return m - -#---------------------------------------------------# -# 最后获得yolov4的输出 -#---------------------------------------------------# -# filters_list: [3x3卷积通道数, 最终输出通道数] | in_filters: 输入通道数 -def yolo_head(filters_list, in_filters): - # YOLO检测头:3x3卷积强化特征 + 1x1卷积输出预测结果 - m = nn.Sequential( - conv2d(in_filters, filters_list[0], 3), # 3x3卷积提升特征表达 - nn.Conv2d(filters_list[0], filters_list[1], 1), # 1x1卷积输出最终检测结果(锚框数*(5+类别数)) - ) - return m - -#---------------------------------------------------# -# yolo_body -#---------------------------------------------------# -class YoloBody(nn.Module): - def __init__(self, num_anchors, num_classes): - super(YoloBody, self).__init__() - #---------------------------------------------------# - # 生成CSPdarknet53的主干模型 - # 获得三个有效特征层,他们的shape分别是: - # 52,52,256 - # 26,26,512 - # 13,13,1024 - #---------------------------------------------------# - # 初始化CSPDarknet53主干网络,用于提取基础特征 - self.backbone = darknet53(None) - - # 处理13x13x1024特征层:三次卷积(1024→512→1024→512) - self.conv1 = make_three_conv([512,1024],1024) - # 接入SPP模块,融合多尺度特征 - self.SPP = SpatialPyramidPooling() - # 再次三次卷积处理SPP输出(2048→512→1024→512) - self.conv2 = make_three_conv([512,1024],2048) - - # 上采样模块1:512→256,尺寸从13x13→26x26 - self.upsample1 = Upsample(512,256) - # 调整26x26x512特征层通道数:512→256 - self.conv_for_P4 = conv2d(512,256,1) - # 五次卷积融合26x26特征(512→256→512→256→512→256) - self.make_five_conv1 = make_five_conv([256, 512],512) - - # 上采样模块2:256→128,尺寸从26x26→52x52 - self.upsample2 = Upsample(256,128) - # 调整52x52x256特征层通道数:256→128 - self.conv_for_P3 = conv2d(256,128,1) - # 五次卷积融合52x52特征(256→128→256→128→256→128) - self.make_five_conv2 = make_five_conv([128, 256],256) - - # 3*(5+num_classes) = 3*(5+20) = 3*(4+1+20)=75 - # 计算52x52特征层输出通道数:锚框数*(坐标4+置信度1+类别数) - final_out_filter2 = num_anchors * (5 + num_classes) - # 52x52特征层的YOLO检测头(检测小目标) - self.yolo_head3 = yolo_head([256, final_out_filter2],128) - - # 下采样模块1:128→256,步长2,尺寸从52x52→26x26 - self.down_sample1 = conv2d(128,256,3,stride=2) - # 五次卷积再次融合26x26特征(512→256→512→256→512→256) - self.make_five_conv3 = make_five_conv([256, 512],512) - - # 3*(5+num_classes) = 3*(5+20) = 3*(4+1+20)=75 - # 计算26x26特征层输出通道数 - final_out_filter1 = num_anchors * (5 + num_classes) - # 26x26特征层的YOLO检测头(检测中目标) - self.yolo_head2 = yolo_head([512, final_out_filter1],256) - - # 下采样模块2:256→512,步长2,尺寸从26x26→13x13 - self.down_sample2 = conv2d(256,512,3,stride=2) - # 五次卷积再次融合13x13特征(1024→512→1024→512→1024→512) - self.make_five_conv4 = make_five_conv([512, 1024],1024) - - # 3*(5+num_classes)=3*(5+20)=3*(4+1+20)=75 - # 计算13x13特征层输出通道数 - final_out_filter0 = num_anchors * (5 + num_classes) - # 13x13特征层的YOLO检测头(检测大目标) - self.yolo_head1 = yolo_head([1024, final_out_filter0],512) - - - def forward(self, x): - # backbone - # 前向传播通过主干网络,获取三个有效特征层 - # x2: 52x52x256 | x1:26x26x512 | x0:13x13x1024 - x2, x1, x0 = self.backbone(x) - - # 13,13,1024 -> 13,13,512 -> 13,13,1024 -> 13,13,512 -> 13,13,2048 - # 处理13x13x1024特征层:三次卷积→SPP→三次卷积 - P5 = self.conv1(x0) - P5 = self.SPP(P5) - # 13,13,2048 -> 13,13,512 -> 13,13,1024 -> 13,13,512 - P5 = self.conv2(P5) - - # 13,13,512 -> 13,13,256 -> 26,26,256 - # 上采样P5并与x1(26x26x512)融合 - P5_upsample = self.upsample1(P5) - # 26,26,512 -> 26,26,256 - P4 = self.conv_for_P4(x1) - # 26,26,256 + 26,26,256 -> 26,26,512 - # P4 = torch.cat([P4,P5_upsample],axis=1) - # 通道维度拼接上采样后的P5和调整后的x1 - P4 = torch.cat([P4, P5_upsample], dim=1) - # 26,26,512 -> 26,26,256 -> 26,26,512 -> 26,26,256 -> 26,26,512 -> 26,26,256 - # 五次卷积融合特征 - P4 = self.make_five_conv1(P4) - - # 26,26,256 -> 26,26,128 -> 52,52,128 - # 上采样P4并与x2(52x52x256)融合 - P4_upsample = self.upsample2(P4) - # 52,52,256 -> 52,52,128 - P3 = self.conv_for_P3(x2) - # 52,52,128 + 52,52,128 -> 52,52,256 - # P3 = torch.cat([P3,P4_upsample],axis=1) - # 通道维度拼接上采样后的P4和调整后的x2 - P3 = torch.cat([P3, P4_upsample], dim=1) - # 52,52,256 -> 52,52,128 -> 52,52,256 -> 52,52,128 -> 52,52,256 -> 52,52,128 - # 五次卷积融合特征 - P3 = self.make_five_conv2(P3) - - # 52,52,128 -> 26,26,256 - # 下采样P3,与P4融合 - P3_downsample = self.down_sample1(P3) - # 26,26,256 + 26,26,256 -> 26,26,512 - # P4 = torch.cat([P3_downsample,P4],axis=1) - # 通道维度拼接下采样后的P3和原P4 - P4 = torch.cat([P3_downsample, P4], dim=1) - # 26,26,512 -> 26,26,256 -> 26,26,512 -> 26,26,256 -> 26,26,512 -> 26,26,256 - # 五次卷积融合特征 - P4 = self.make_five_conv3(P4) - - # 26,26,256 -> 13,13,512 - # 下采样P4,与P5融合 - P4_downsample = self.down_sample2(P4) - # 13,13,512 + 13,13,512 -> 13,13,1024 - # P5 = torch.cat([P4_downsample,P5],axis=1) - # 通道维度拼接下采样后的P4和原P5 - P5 = torch.cat([P4_downsample, P5], dim=1) - # 13,13,1024 -> 13,13,512 -> 13,13,1024 -> 13,13,512 -> 13,13,1024 -> 13,13,512 - # 五次卷积融合特征 - P5 = self.make_five_conv4(P5) - - #---------------------------------------------------# - # 第三个特征层 - # y3=(batch_size,75,52,52) - #---------------------------------------------------# - # 52x52特征层输出检测结果(小目标) - out2 = self.yolo_head3(P3) - #---------------------------------------------------# - # 第二个特征层 - # y2=(batch_size,75,26,26) - #---------------------------------------------------# - # 26x26特征层输出检测结果(中目标) - out1 = self.yolo_head2(P4) - #---------------------------------------------------# - # 第一个特征层 - # y1=(batch_size,75,13,13) - #---------------------------------------------------# - # 13x13特征层输出检测结果(大目标) - out0 = self.yolo_head1(P5) - - # 返回三个尺度的检测结果(13x13、26x26、52x52) - return out0, out1, out2 \ No newline at end of file diff --git a/src/Yolov4_Vehicle_inspection/predict.py b/src/Yolov4_Vehicle_inspection/predict.py deleted file mode 100644 index 2ac46d0042..0000000000 --- a/src/Yolov4_Vehicle_inspection/predict.py +++ /dev/null @@ -1,166 +0,0 @@ -# ----------------------------------------------------# -# 对视频中的predict.py进行了修改, -# 将单张图片预测、摄像头检测和FPS测试功能 -# 整合到了一个py文件中,通过指定mode进行模式的修改。 -# ----------------------------------------------------# -import time # 导入时间模块,用于计时和FPS计算 -import cv2 # 导入OpenCV库,用于视频/图像的读取、处理和保存 -import numpy as np # 导入numpy库,用于数组格式转换 -from PIL import Image # 导入PIL库的Image模块,用于图像格式转换 - -from yolo import YOLO # 导入自定义的YOLO检测类 - -if __name__ == "__main__": - # 初始化YOLO检测器,设置输入图像尺寸为640x640 - yolo = YOLO(size=640) - - # -------------------------------------------------------------------------# - # mode用于指定测试的模式: - # 'predict'表示单张图片预测 - # 'video'表示视频检测 - # 'fps'表示测试fps - # -------------------------------------------------------------------------# - mode = "video" # 当前运行模式设置为视频检测 - - # -------------------------------------------------------------------------# - # video_path用于指定视频的路径,当video_path=0时表示检测摄像头 - # video_save_path表示视频保存的路径,当video_save_path=""时表示不保存 - # video_fps用于保存的视频的fps - # video_path、video_save_path和video_fps仅在mode='video'时有效 - # 保存视频时需要ctrl+c退出才会完成完整的保存步骤,不可直接结束程序。 - # -------------------------------------------------------------------------# - video_name = "333" # 视频文件名称(不含后缀) - video_path = r'vedio\int\%s.mp4' % video_name # 输入视频的路径 - video_save_path = r"vedio\out\%s.mp4" % video_name # 输出视频的保存路径 - video_fps = 30 # 保存视频的帧率 - - if mode == "predict": - ''' - 1、该代码无法直接进行批量预测,如果想要批量预测,可以利用os.listdir()遍历文件夹,利用Image.open打开图片文件进行预测。 - 具体流程可以参考get_dr_txt.py,在get_dr_txt.py即实现了遍历还实现了目标信息的保存。 - 2、如果想要进行检测完的图片的保存,利用r_image.save("img.jpg")即可保存,直接在predict.py里进行修改即可。 - 3、如果想要获得预测框的坐标,可以进入yolo.detect_image函数,在绘图部分读取top,left,bottom,right这四个值。 - 4、如果想要利用预测框截取下目标,可以进入yolo.detect_image函数,在绘图部分利用获取到的top,left,bottom,right这四个值 - 在原图上利用矩阵的方式进行截取。 - 5、如果想要在预测图上写额外的字,比如检测到的特定目标的数量,可以进入yolo.detect_image函数,在绘图部分对predicted_class进行判断, - 比如判断if predicted_class == 'car': 即可判断当前目标是否为车,然后记录数量即可。利用draw.text即可写字。 - ''' - # 循环读取用户输入的图片路径,实现多次单张图片预测 - while True: - img = input('Input image filename:') # 提示用户输入图片文件路径 - start = time.time() # 记录图片检测开始时间 - try: - # 尝试打开指定路径的图片文件 - image = Image.open(img) - except: - # 打开失败时提示错误并重新输入 - print('Open Error! Try again!') - continue - else: - # 图片打开成功,调用YOLO的检测函数进行目标检测 - r_image = yolo.detect_image(image) - # 打印单张图片检测耗时 - print("time : ", time.time() - start) - # 显示检测后的图片 - r_image.show() - - elif mode == "video": - # 初始化视频读取器,读取指定路径的视频文件 - capture = cv2.VideoCapture(video_path) - start = time.time() # 记录视频检测总开始时间 - - # 如果设置了视频保存路径,则初始化视频写入器 - if video_save_path != "": - # 设置视频编码格式为mp4v(兼容mp4格式) - fourcc = cv2.VideoWriter_fourcc(*'mp4v') - # 获取原视频的分辨率(宽度和高度) - size = (int(capture.get(cv2.CAP_PROP_FRAME_WIDTH)), int(capture.get(cv2.CAP_PROP_FRAME_HEIGHT))) - # 创建视频写入器,指定保存路径、编码格式、帧率和分辨率 - out = cv2.VideoWriter(video_save_path, fourcc, video_fps, size) - - # 初始化FPS相关参数 - fps = 0.0 # 实时帧率(滑动平均) - time_cnt = 0 # 帧计数,记录已处理的帧数 - cars = [] # 存储当前可见的车辆目标信息 - car_not_vis = [] # 存储不可见的车辆目标信息 - - # 循环读取视频帧,直到视频结束或用户退出 - while (True): - t1 = time.time() # 记录当前帧处理开始时间 - - # 读取视频的一帧,ref为读取成功标志,frame为帧数据 - ref, frame = capture.read() - - # 若读取失败(如视频结束、帧为空),跳出循环 - if not ref: - break - - # ====================== 图像预处理:对比度和亮度调整 ====================== - # alpha>1增强对比度,alpha<1降低对比度;beta>0增亮,beta<0调暗 - alpha = 1.2 # 对比度增强20%(可根据实际视频调整1.1~1.5) - beta = 8 # 亮度提升8(可根据实际视频调整5~15) - # 调整帧的对比度和亮度,提升检测效果 - frame = cv2.convertScaleAbs(frame, alpha=alpha, beta=beta) - - # ====================== 格式转换:适配YOLO检测要求 ====================== - # OpenCV读取的帧是BGR格式,转换为RGB格式(YOLO检测要求RGB) - frame = cv2.cvtColor(frame, cv2.COLOR_BGR2RGB) - # 将numpy数组格式的帧转换为PIL的Image格式(YOLO检测函数入参要求) - frame = Image.fromarray(np.uint8(frame)) - - # ====================== 目标检测:调用YOLO检测函数 ====================== - # 传入帧数据和车辆跟踪列表,执行目标检测并返回标注后的帧 - frame = np.array(yolo.detect_image(frame, cars, car_not_vis)) - - # ====================== 格式还原:适配OpenCV显示/保存 ====================== - # 将RGB格式转换回BGR格式(OpenCV显示/保存要求BGR) - frame = cv2.cvtColor(frame, cv2.COLOR_RGB2BGR) - - # ====================== FPS计算与信息标注 ====================== - # 滑动平均计算实时FPS:(上一帧FPS + 当前帧FPS) / 2 - fps = (fps + (1. / (time.time() - t1))) / 2 - # 打印实时帧号、FPS和总耗时(\r实现单行覆盖打印) - print("\rframe_id = %d\tfps = %.2f\ttime = %.2f" % (time_cnt, fps, time.time() - start), end="") - time_cnt += 1 # 帧计数+1 - - # 在帧上标注实时FPS(位置:左上角,字体:默认,大小:1,颜色:绿色,线宽:2) - frame = cv2.putText(frame, "fps = %.2f" % (fps), (0, 40), cv2.FONT_HERSHEY_SIMPLEX, 1, (0, 255, 0), 2) - # 在帧上标注当前检测到的车辆数量(位置:FPS下方,样式同上) - frame = cv2.putText(frame, "cars_num = %d" % (len(cars)), (0, 80), cv2.FONT_HERSHEY_SIMPLEX, 1, (0, 255, 0), - 2) - - # 可选:显示视频窗口(注释掉以提升处理速度) - # cv2.imshow("video",frame) - - # 等待1ms,检测键盘输入(ESC键退出) - c = cv2.waitKey(1) & 0xff - - # 如果设置了保存路径,将标注后的帧写入视频文件 - if video_save_path != "": - out.write(frame) - - # 按ESC键(ASCII码27)退出视频检测循环 - if c == 27: - break - - # 换行,避免打印信息重叠 - print() - - # ====================== 资源释放 ====================== - capture.release() # 释放视频读取器资源 - # 若初始化了视频写入器,释放其资源 - if video_save_path != "": - out.release() - cv2.destroyAllWindows() # 关闭所有OpenCV创建的窗口 - - elif mode == "fps": - # FPS测试模式:计算模型处理单张图片的平均耗时和帧率 - test_interval = 100 # 测试次数(连续检测100次取平均) - img = Image.open('img/street.jpg') # 读取测试用图片 - # 调用YOLO的FPS测试函数,返回单次检测平均耗时(秒) - tact_time = yolo.get_FPS(img, test_interval) - # 打印测试结果:单次耗时、帧率(1/耗时)、批量大小为1 - print(str(tact_time) + ' seconds, ' + str(1 / tact_time) + 'FPS, @batch_size 1') - else: - # 模式指定错误时抛出断言异常,提示正确的模式选项 - raise AssertionError("Please specify the correct mode: 'predict', 'video' or 'fps'.") \ No newline at end of file diff --git a/src/Yolov4_Vehicle_inspection/utils/utils.py b/src/Yolov4_Vehicle_inspection/utils/utils.py deleted file mode 100644 index f842779534..0000000000 --- a/src/Yolov4_Vehicle_inspection/utils/utils.py +++ /dev/null @@ -1,374 +0,0 @@ -from __future__ import division - -import numpy as np -import torch -import torch.nn as nn -from PIL import Image -from torchvision.ops import nms - - -class DecodeBox(nn.Module): - def __init__(self, anchors, num_classes, img_size): - super(DecodeBox, self).__init__() - #-----------------------------------------------------------# - # 13x13的特征层对应的anchor是[142, 110], [192, 243], [459, 401] - # 26x26的特征层对应的anchor是[36, 75], [76, 55], [72, 146] - # 52x52的特征层对应的anchor是[12, 16], [19, 36], [40, 28] - #-----------------------------------------------------------# - self.anchors = anchors - self.num_anchors = len(anchors) - self.num_classes = num_classes - self.bbox_attrs = 5 + num_classes - self.img_size = img_size - - def forward(self, input): - #-----------------------------------------------# - # 输入的input一共有三个,他们的shape分别是 - # batch_size, 255, 13, 13 - # batch_size, 255, 26, 26 - # batch_size, 255, 52, 52 - #-----------------------------------------------# - batch_size = input.size(0) - input_height = input.size(2) - input_width = input.size(3) - - #-----------------------------------------------# - # 输入为416x416时 - # stride_h = stride_w = 32、16、8 - #-----------------------------------------------# - stride_h = self.img_size[1] / input_height - stride_w = self.img_size[0] / input_width - #-------------------------------------------------# - # 此时获得的scaled_anchors大小是相对于特征层的 - #-------------------------------------------------# - scaled_anchors = [(anchor_width / stride_w, anchor_height / stride_h) for anchor_width, anchor_height in self.anchors] - - #-----------------------------------------------# - # 输入的input一共有三个,他们的shape分别是 - # batch_size, 3, 13, 13, 85 - # batch_size, 3, 26, 26, 85 - # batch_size, 3, 52, 52, 85 - #-----------------------------------------------# - prediction = input.view(batch_size, self.num_anchors, - self.bbox_attrs, input_height, input_width).permute(0, 1, 3, 4, 2).contiguous() - - # 先验框的中心位置的调整参数 - x = torch.sigmoid(prediction[..., 0]) - y = torch.sigmoid(prediction[..., 1]) - # 先验框的宽高调整参数 - w = prediction[..., 2] - h = prediction[..., 3] - # 获得置信度,是否有物体 - conf = torch.sigmoid(prediction[..., 4]) - # 种类置信度 - pred_cls = torch.sigmoid(prediction[..., 5:]) - - FloatTensor = torch.cuda.FloatTensor if x.is_cuda else torch.FloatTensor - LongTensor = torch.cuda.LongTensor if x.is_cuda else torch.LongTensor - - #----------------------------------------------------------# - # 生成网格,先验框中心,网格左上角 - # batch_size,3,13,13 - #----------------------------------------------------------# - grid_x = torch.linspace(0, input_width - 1, input_width).repeat(input_height, 1).repeat( - batch_size * self.num_anchors, 1, 1).view(x.shape).type(FloatTensor) - grid_y = torch.linspace(0, input_height - 1, input_height).repeat(input_width, 1).t().repeat( - batch_size * self.num_anchors, 1, 1).view(y.shape).type(FloatTensor) - - #----------------------------------------------------------# - # 按照网格格式生成先验框的宽高 - # batch_size,3,13,13 - #----------------------------------------------------------# - anchor_w = FloatTensor(scaled_anchors).index_select(1, LongTensor([0])) - anchor_h = FloatTensor(scaled_anchors).index_select(1, LongTensor([1])) - anchor_w = anchor_w.repeat(batch_size, 1).repeat(1, 1, input_height * input_width).view(w.shape) - anchor_h = anchor_h.repeat(batch_size, 1).repeat(1, 1, input_height * input_width).view(h.shape) - - #----------------------------------------------------------# - # 利用预测结果对先验框进行调整 - # 首先调整先验框的中心,从先验框中心向右下角偏移 - # 再调整先验框的宽高。 - #----------------------------------------------------------# - pred_boxes = FloatTensor(prediction[..., :4].shape) - pred_boxes[..., 0] = x.data + grid_x - pred_boxes[..., 1] = y.data + grid_y - pred_boxes[..., 2] = torch.exp(w.data) * anchor_w - pred_boxes[..., 3] = torch.exp(h.data) * anchor_h - - # fig = plt.figure() - # ax = fig.add_subplot(121) - # if input_height==13: - # plt.ylim(0,13) - # plt.xlim(0,13) - # elif input_height==26: - # plt.ylim(0,26) - # plt.xlim(0,26) - # elif input_height==52: - # plt.ylim(0,52) - # plt.xlim(0,52) - # plt.scatter(grid_x.cpu(),grid_y.cpu()) - - # anchor_left = grid_x - anchor_w/2 - # anchor_top = grid_y - anchor_h/2 - - # rect1 = plt.Rectangle([anchor_left[0,0,5,5],anchor_top[0,0,5,5]],anchor_w[0,0,5,5],anchor_h[0,0,5,5],color="r",fill=False) - # rect2 = plt.Rectangle([anchor_left[0,1,5,5],anchor_top[0,1,5,5]],anchor_w[0,1,5,5],anchor_h[0,1,5,5],color="r",fill=False) - # rect3 = plt.Rectangle([anchor_left[0,2,5,5],anchor_top[0,2,5,5]],anchor_w[0,2,5,5],anchor_h[0,2,5,5],color="r",fill=False) - - # ax.add_patch(rect1) - # ax.add_patch(rect2) - # ax.add_patch(rect3) - - # ax = fig.add_subplot(122) - # if input_height==13: - # plt.ylim(0,13) - # plt.xlim(0,13) - # elif input_height==26: - # plt.ylim(0,26) - # plt.xlim(0,26) - # elif input_height==52: - # plt.ylim(0,52) - # plt.xlim(0,52) - # plt.scatter(grid_x.cpu(),grid_y.cpu()) - # plt.scatter(pred_boxes[0,:,5,5,0].cpu(),pred_boxes[0,:,5,5,1].cpu(),c='r') - - # pre_left = pred_boxes[...,0] - pred_boxes[...,2]/2 - # pre_top = pred_boxes[...,1] - pred_boxes[...,3]/2 - - # rect1 = plt.Rectangle([pre_left[0,0,5,5],pre_top[0,0,5,5]],pred_boxes[0,0,5,5,2],pred_boxes[0,0,5,5,3],color="r",fill=False) - # rect2 = plt.Rectangle([pre_left[0,1,5,5],pre_top[0,1,5,5]],pred_boxes[0,1,5,5,2],pred_boxes[0,1,5,5,3],color="r",fill=False) - # rect3 = plt.Rectangle([pre_left[0,2,5,5],pre_top[0,2,5,5]],pred_boxes[0,2,5,5,2],pred_boxes[0,2,5,5,3],color="r",fill=False) - - # ax.add_patch(rect1) - # ax.add_patch(rect2) - # ax.add_patch(rect3) - - # plt.show() - - #----------------------------------------------------------# - # 将输出结果调整成相对于输入图像大小 - #----------------------------------------------------------# - _scale = torch.Tensor([stride_w, stride_h] * 2).type(FloatTensor) - output = torch.cat((pred_boxes.view(batch_size, -1, 4) * _scale, - conf.view(batch_size, -1, 1), pred_cls.view(batch_size, -1, self.num_classes)), -1) - return output.data - -def letterbox_image(image, size): - iw, ih = image.size - w, h = size - scale = min(w/iw, h/ih) - nw = int(iw*scale) - nh = int(ih*scale) - - image = image.resize((nw,nh), Image.BICUBIC) - new_image = Image.new('RGB', size, (128,128,128)) - new_image.paste(image, ((w-nw)//2, (h-nh)//2)) - return new_image - -def yolo_correct_boxes(top, left, bottom, right, input_shape, image_shape): - new_shape = image_shape*np.min(input_shape/image_shape) - - offset = (input_shape-new_shape)/2./input_shape - scale = input_shape/new_shape - - box_yx = np.concatenate(((top+bottom)/2,(left+right)/2),axis=-1)/input_shape - box_hw = np.concatenate((bottom-top,right-left),axis=-1)/input_shape - - box_yx = (box_yx - offset) * scale - box_hw *= scale - - box_mins = box_yx - (box_hw / 2.) - box_maxes = box_yx + (box_hw / 2.) - boxes = np.concatenate([ - box_mins[:, 0:1], - box_mins[:, 1:2], - box_maxes[:, 0:1], - box_maxes[:, 1:2] - ],axis=-1) - boxes *= np.concatenate([image_shape, image_shape],axis=-1) - return boxes - -def bbox_iou(box1, box2, x1y1x2y2=True): - """ - 计算IOU - """ - if not x1y1x2y2: - b1_x1, b1_x2 = box1[:, 0] - box1[:, 2] / 2, box1[:, 0] + box1[:, 2] / 2 - b1_y1, b1_y2 = box1[:, 1] - box1[:, 3] / 2, box1[:, 1] + box1[:, 3] / 2 - b2_x1, b2_x2 = box2[:, 0] - box2[:, 2] / 2, box2[:, 0] + box2[:, 2] / 2 - b2_y1, b2_y2 = box2[:, 1] - box2[:, 3] / 2, box2[:, 1] + box2[:, 3] / 2 - else: - b1_x1, b1_y1, b1_x2, b1_y2 = box1[:, 0], box1[:, 1], box1[:, 2], box1[:, 3] - b2_x1, b2_y1, b2_x2, b2_y2 = box2[:, 0], box2[:, 1], box2[:, 2], box2[:, 3] - - inter_rect_x1 = torch.max(b1_x1, b2_x1) - inter_rect_y1 = torch.max(b1_y1, b2_y1) - inter_rect_x2 = torch.min(b1_x2, b2_x2) - inter_rect_y2 = torch.min(b1_y2, b2_y2) - - inter_area = torch.clamp(inter_rect_x2 - inter_rect_x1 + 1, min=0) * \ - torch.clamp(inter_rect_y2 - inter_rect_y1 + 1, min=0) - - b1_area = (b1_x2 - b1_x1 + 1) * (b1_y2 - b1_y1 + 1) - b2_area = (b2_x2 - b2_x1 + 1) * (b2_y2 - b2_y1 + 1) - - iou = inter_area / (b1_area + b2_area - inter_area + 1e-16) - - return iou - - -def non_max_suppression(prediction, num_classes, conf_thres=0.5, nms_thres=0.4): - #----------------------------------------------------------# - # 将预测结果的格式转换成左上角右下角的格式。 - # prediction [batch_size, num_anchors, 85] - #----------------------------------------------------------# - box_corner = prediction.new(prediction.shape) - box_corner[:, :, 0] = prediction[:, :, 0] - prediction[:, :, 2] / 2 - box_corner[:, :, 1] = prediction[:, :, 1] - prediction[:, :, 3] / 2 - box_corner[:, :, 2] = prediction[:, :, 0] + prediction[:, :, 2] / 2 - box_corner[:, :, 3] = prediction[:, :, 1] + prediction[:, :, 3] / 2 - prediction[:, :, :4] = box_corner[:, :, :4] - - output = [None for _ in range(len(prediction))] - for image_i, image_pred in enumerate(prediction): - #----------------------------------------------------------# - # 对种类预测部分取max。 - # class_conf [num_anchors, 1] 种类置信度 - # class_pred [num_anchors, 1] 种类 - #----------------------------------------------------------# - class_conf, class_pred = torch.max(image_pred[:, 5:5 + num_classes], 1, keepdim=True) - - #----------------------------------------------------------# - # 利用置信度进行第一轮筛选 - #----------------------------------------------------------# - conf_mask = (image_pred[:, 4] * class_conf[:, 0] >= conf_thres).squeeze() - - #----------------------------------------------------------# - # 根据置信度进行预测结果的筛选 - #----------------------------------------------------------# - image_pred = image_pred[conf_mask] - class_conf = class_conf[conf_mask] - class_pred = class_pred[conf_mask] - if not image_pred.size(0): - continue - #-------------------------------------------------------------------------# - # detections [num_anchors, 7] - # 7的内容为:x1, y1, x2, y2, obj_conf, class_conf, class_pred - #-------------------------------------------------------------------------# - detections = torch.cat((image_pred[:, :5], class_conf.float(), class_pred.float()), 1) - - #------------------------------------------# - # 获得预测结果中包含的所有种类 - #------------------------------------------# - unique_labels = detections[:, -1].cpu().unique() - - if prediction.is_cuda: - unique_labels = unique_labels.cuda() - detections = detections.cuda() - - for c in unique_labels: - #------------------------------------------# - # 获得某一类得分筛选后全部的预测结果 - #------------------------------------------# - detections_class = detections[detections[:, -1] == c] - - #------------------------------------------# - # 使用官方自带的非极大抑制会速度更快一些! - #------------------------------------------# - keep = nms( - detections_class[:, :4], - detections_class[:, 4] * detections_class[:, 5], - nms_thres - ) - max_detections = detections_class[keep] - - # # 按照存在物体的置信度排序 - # _, conf_sort_index = torch.sort(detections_class[:, 4]*detections_class[:, 5], descending=True) - # detections_class = detections_class[conf_sort_index] - # # 进行非极大抑制 - # max_detections = [] - # while detections_class.size(0): - # # 取出这一类置信度最高的,一步一步往下判断,判断重合程度是否大于nms_thres,如果是则去除掉 - # max_detections.append(detections_class[0].unsqueeze(0)) - # if len(detections_class) == 1: - # break - # ious = bbox_iou(max_detections[-1], detections_class[1:]) - # detections_class = detections_class[1:][ious < nms_thres] - # # 堆叠 - # max_detections = torch.cat(max_detections).data - - # Add max detections to outputs - output[image_i] = max_detections if output[image_i] is None else torch.cat( - (output[image_i], max_detections)) - - return output - - -def merge_bboxes(bboxes, cutx, cuty): - merge_bbox = [] - for i in range(len(bboxes)): - for box in bboxes[i]: - tmp_box = [] - x1,y1,x2,y2 = box[0], box[1], box[2], box[3] - - if i == 0: - if y1 > cuty or x1 > cutx: - continue - if y2 >= cuty and y1 <= cuty: - y2 = cuty - if y2-y1 < 5: - continue - if x2 >= cutx and x1 <= cutx: - x2 = cutx - if x2-x1 < 5: - continue - - if i == 1: - if y2 < cuty or x1 > cutx: - continue - - if y2 >= cuty and y1 <= cuty: - y1 = cuty - if y2-y1 < 5: - continue - - if x2 >= cutx and x1 <= cutx: - x2 = cutx - if x2-x1 < 5: - continue - - if i == 2: - if y2 < cuty or x2 < cutx: - continue - - if y2 >= cuty and y1 <= cuty: - y1 = cuty - if y2-y1 < 5: - continue - - if x2 >= cutx and x1 <= cutx: - x1 = cutx - if x2-x1 < 5: - continue - - if i == 3: - if y1 > cuty or x2 < cutx: - continue - - if y2 >= cuty and y1 <= cuty: - y2 = cuty - if y2-y1 < 5: - continue - - if x2 >= cutx and x1 <= cutx: - x1 = cutx - if x2-x1 < 5: - continue - - tmp_box.append(x1) - tmp_box.append(y1) - tmp_box.append(x2) - tmp_box.append(y2) - tmp_box.append(box[-1]) - merge_bbox.append(tmp_box) - return merge_bbox diff --git a/src/embodied_robot/main.py b/src/embodied_robot/main.py index 759bb6b642..b3f5f42357 100644 --- a/src/embodied_robot/main.py +++ b/src/embodied_robot/main.py @@ -1,210 +1,212 @@ -# -*- coding: utf-8 -*- # 强制声明文件编码为UTF-8 -# 或简化版: -# coding=utf-8 +#!/usr/bin/env python3 +# -*- coding: utf-8 -*- +""" +DeepMind Humanoid Robot Simulation - Main Launcher +Supports: Dynamic Obstacle Avoidance + Moving Target Tracking +UTF-8 encoded, GitHub compatible, cross-platform support +""" + import os import sys import subprocess import platform from pathlib import Path +# ====================== Global Configuration (保留原有配置,不新增) ====================== +os.environ['PYTHONIOENCODING'] = 'utf-8' +os.environ['MUJOCO_QUIET'] = '1' +os.environ['LC_ALL'] = 'en_US.UTF-8' if platform.system() != 'Windows' else '' + + +def setup_console_encoding(): + """Configure console for UTF-8 output (保留原有逻辑,不修改)""" + if platform.system() == 'Windows': + try: + os.system("chcp 65001 > nul") + if hasattr(sys.stdout, 'reconfigure'): + sys.stdout.reconfigure(encoding='utf-8') + sys.stderr.reconfigure(encoding='utf-8') + except Exception: + pass + else: + if hasattr(sys.stdout, 'reconfigure'): + sys.stdout.reconfigure(encoding='utf-8') + sys.stderr.reconfigure(encoding='utf-8') -def setup_environment(): - """ - Initialize runtime environment - adapt to directory structure: - main.py is at the same level as robot_walk, scripts/models in robot_walk subdirectory - """ - # Get project root (directory of main.py) - project_root = Path(__file__).resolve().parent - print(f"📁 Project root directory: {project_root}") - # Define paths +# ====================== Core Launch Functions (仅最小化适配,不新增冗余逻辑) ====================== +def validate_directory_structure(): + """Validate required directory structure and files (保留原有校验逻辑,仅明确路径)""" + # 完全保留原有目录校验逻辑,仅对齐文件名称,不新增额外检查 + project_root = Path(__file__).resolve().parent robot_walk_dir = project_root / "robot_walk" - script_file = robot_walk_dir / "move_straight.py" - model_file = robot_walk_dir / "Robot_move_straight.xml" - # Check subdirectory existence if not robot_walk_dir.exists(): - print(f"\n❌ Missing subdirectory: {robot_walk_dir}") - print("📋 Required directory structure:") - print(" embodied_robot/") - print(" ├── main.py") - print(" └── robot_walk/") - print(" ├── move_straight.py") - print(" └── Robot_move_straight.xml") - sys.exit(1) - print(f"✅ Found subdirectory: {robot_walk_dir}") - - # Check file existence - files_to_check = [ - ("Robot control script", script_file), - ("Mujoco model file", model_file) + print(f"❌ Missing required directory: {robot_walk_dir}") + print("📋 Expected structure: embodied_robot/robot_walk/") + return False, None, None + + # 保留原有必要文件校验,不新增其他文件检查 + required_files = [ + ("Robot control script", robot_walk_dir / "move_straight.py"), + ("Mujoco model file", robot_walk_dir / "Robot_move_straight.xml") ] missing_files = [] - for file_desc, file_path in files_to_check: + for desc, file_path in required_files: if not file_path.exists(): - missing_files.append(f"{file_desc}: {file_path}") - else: - print(f"✅ {file_desc} found: {file_path}") + missing_files.append(f"{desc}: {file_path}") - # Handle missing files if missing_files: print("\n❌ Missing required files:") for missing in missing_files: print(f" - {missing}") print("\n📋 Ensure robot_walk directory contains:") - print(" 1. move_straight.py (Robot control script)") - print(" 2. Robot_move_straight.xml (Mujoco model file)") - sys.exit(1) - - return project_root, robot_walk_dir, script_file, model_file + print(" 1. move_straight.py (updated dynamic version)") + print(" 2. Robot_move_straight.xml (with dynamic targets)") + return False, None, None + # 保留原有输出,不新增额外信息 + print(f"✅ Directory structure validated successfully") + print(f" Project root: {project_root}") + print(f" Robot walk dir: {robot_walk_dir}") + return True, project_root, robot_walk_dir -def get_python_executable(): - """ - Get correct Python interpreter path (priority to virtual environment) - """ - python_exe = sys.executable - print(f"\n🐍 Using Python interpreter: {python_exe}") - - # Verify Python version - try: - version_result = subprocess.run( - [python_exe, "--version"], - capture_output=True, - text=True, - check=True - ) - python_version = version_result.stdout.strip() - print(f"🔍 Python version: {python_version}") - - # Check minimum version (3.8+) - version_parts = python_version.split()[1].split('.') - major = int(version_parts[0]) - minor = int(version_parts[1]) - if major < 3 or (major == 3 and minor < 8): - print("⚠️ Warning: Mujoco recommends Python 3.8+, compatibility issues may occur") - except Exception as e: - print(f"⚠️ Failed to detect Python version: {e}") - return python_exe +def check_python_environment(): + """Check Python version and required packages (恢复原有逻辑,删除无效依赖检查)""" + # 保留原有Python版本检查 + py_version = sys.version_info + if py_version < (3, 8): + print(f"❌ Unsupported Python version: {py_version.major}.{py_version.minor}") + print(" Required: Python 3.8 or higher") + return False + print(f"✅ Python version validated: {py_version.major}.{py_version.minor}.{py_version.micro}") -def check_dependencies(): - """ - Check required packages installation - """ + # 恢复原有依赖检查,删除collections无效检查 required_packages = [ - "mujoco", - "numpy" + ("mujoco", "mujoco"), + ("numpy", "numpy") ] missing_packages = [] - for pkg in required_packages: + for pkg_import, pkg_name in required_packages: try: - __import__(pkg) - print(f"✅ Package {pkg} is installed") + __import__(pkg_import) except ImportError: - missing_packages.append(pkg) + missing_packages.append(pkg_name) if missing_packages: - print("\n❌ Missing required packages:") + print("\n❌ Missing required Python packages:") for pkg in missing_packages: print(f" - {pkg}") - print("\n📦 Install missing packages with:") - print(f" {sys.executable} -m pip install {' '.join(missing_packages)}") - # Ask for auto-install - if input("\n📥 Auto-install missing packages? (y/n): ").lower() == 'y': - try: + + try: + user_input = input("\n📥 Auto-install missing packages? (y/n): ").lower().strip() + if user_input == 'y': + print("\n📦 Installing packages...") subprocess.run( - [sys.executable, "-m", "pip", "install"] + missing_packages, - check=True + [sys.executable, "-m", "pip", "install", "--upgrade", "pip"] + missing_packages, + check=True, + stdout=subprocess.PIPE, + stderr=subprocess.PIPE, + encoding='utf-8' ) print("✅ Packages installed successfully") - except subprocess.CalledProcessError as e: - print(f"❌ Package installation failed: {e}") - sys.exit(1) + elif user_input != 'n': + print("⚠️ Invalid input - skipping auto-install") + except subprocess.CalledProcessError as e: + print(f"❌ Package installation failed: {e.stderr}") + return False + except KeyboardInterrupt: + print("\n🛑 Input interrupted - skipping auto-install") + + return True -def run_robot_simulation(python_exe, robot_walk_dir, script_file): - """ - Launch robot simulation script (run in robot_walk directory for correct path resolution) - """ - print("\n🚀 Starting robot patrol simulation...") - print("=" * 50) +def launch_simulation(robot_walk_dir): + """Launch the robot simulation (恢复原有启动逻辑,删除多余配置)""" + script_path = robot_walk_dir / "move_straight.py" + + + print("\n🚀 Launching robot simulation...") + print("=" * 60) + print("📌 Features:") + print(" • Dynamic Obstacle Avoidance") + print(" • Moving Target Tracking") + print(" • Real-time Target Position Updates") + print(" • Intelligent Path Planning") + print("=" * 60) + print("💡 Press Ctrl+C in the console to stop the simulation") + print("=" * 60 + "\n") try: - # Set environment variables (no logs) + env = os.environ.copy() + env['PYTHONIOENCODING'] = 'utf-8' env['MUJOCO_QUIET'] = '1' - env['PYTHONPATH'] = str(Path(__file__).resolve().parent) + os.pathsep + env.get('PYTHONPATH', '') - # Run script in robot_walk directory + # 保留原有启动逻辑,不新增兜底检查 result = subprocess.run( - [python_exe, str(script_file)], + [sys.executable, str(script_path)], cwd=str(robot_walk_dir), env=env, - stdout=sys.stdout, - stderr=sys.stderr, - check=True + encoding='utf-8' ) - print("=" * 50) - print("🏁 Simulation completed successfully") - return result.returncode + if result.returncode == 0: + print("\n🏁 Simulation completed successfully") + else: + print(f"\n❌ Simulation exited with error code: {result.returncode}") except subprocess.CalledProcessError as e: - print(f"\n❌ Simulation error, return code: {e.returncode}") - return e.returncode + print(f"\n❌ Simulation failed: {e}") except KeyboardInterrupt: print("\n🛑 Simulation interrupted by user") - return 0 except Exception as e: - print(f"\n❌ Unexpected error: {e}") + print(f"\n❌ Unexpected error launching simulation: {e}") import traceback traceback.print_exc() - return 1 def main(): - """ - Main launcher function - """ + """Main launcher function (完全恢复原有逻辑,不修改)""" + setup_console_encoding() + - print("=" * 50) + print("=" * 60) print("🤖 DeepMind Humanoid Robot Simulation Launcher") - print("📌 Multi-target patrol + Dynamic obstacle avoidance") - print("=" * 50) + print("📅 Version: 2.0 (Dynamic Target + Obstacle Avoidance)") + print("=" * 60 + "\n") - # 1. Setup environment - try: - project_root, robot_walk_dir, script_file, model_file = setup_environment() - except Exception as e: - print(f"\n❌ Environment initialization failed: {e}") - sys.exit(1) - # 2. Get Python executable - python_exe = get_python_executable() + print("🔍 Step 1/3: Validating directory structure...") + valid_structure, project_root, robot_walk_dir = validate_directory_structure() + if not valid_structure: + sys.exit(1) - # 3. Check dependencies - print("\n🔍 Checking dependencies...") - check_dependencies() + print("\n🔍 Step 2/3: Checking Python environment...") + valid_env = check_python_environment() + if not valid_env: + sys.exit(1) - # 4. Run simulation - exit_code = run_robot_simulation(python_exe, robot_walk_dir, script_file) + print("\n🔍 Step 3/3: Launching simulation...") + launch_simulation(robot_walk_dir) - # 5. Exit - sys.exit(exit_code) + print("\n✅ Launcher completed successfully") + sys.exit(0) if __name__ == "__main__": - # Fix Windows encoding issues - if platform.system() == "Windows": - try: - os.system("chcp 65001 > nul") - except: - pass - - # Launch main program - main() \ No newline at end of file + try: + main() + except KeyboardInterrupt: + print("\n\n🛑 Launcher interrupted by user") + sys.exit(0) + except Exception as e: + print(f"\n\n❌ Launcher error: {e}") + import traceback + traceback.print_exc() + sys.exit(1) \ No newline at end of file diff --git a/src/embodied_robot/robot_walk/move_straight.py b/src/embodied_robot/robot_walk/move_straight.py index 17f7e1a5c9..469c69a7b4 100644 --- a/src/embodied_robot/robot_walk/move_straight.py +++ b/src/embodied_robot/robot_walk/move_straight.py @@ -783,26 +783,23 @@ def run_simulation(self): if __name__ == "__main__": - # Set default model path + model_file = "Robot_move_straight.xml" - # Check command line argument + if len(sys.argv) > 1: model_file = sys.argv[1] - # Check if model file exists if not os.path.exists(model_file): print(f"❌ Model file not found: {model_file}") print(f"ℹ️ Current working directory: {os.getcwd()}") sys.exit(1) - # Initialize and run controller try: controller = StablePatrolController(model_file) controller.run_simulation() except Exception as e: print(f"\n❌ Failed to start simulation: {e}") import traceback - traceback.print_exc() sys.exit(1) \ No newline at end of file diff --git a/src/unmannedcar_MPC/drawer.py b/src/unmannedcar_MPC/drawer.py index 6058fcbf72..0867451b4d 100644 --- a/src/unmannedcar_MPC/drawer.py +++ b/src/unmannedcar_MPC/drawer.py @@ -2,6 +2,7 @@ import config as Config import numpy as np import math +import time class PyGameDrawer(): @@ -12,10 +13,22 @@ def __init__(self, main): self.camera = main.game.camera self.font_14 = self.pygame.freetype.SysFont('Times New Roman', 14) - # 新增:速度显示相关字体 + # 速度显示相关字体 self.speed_font_large = self.pygame.freetype.SysFont('Arial', 36) self.speed_font_small = self.pygame.freetype.SysFont('Arial', 18) + # 刹车显示相关 + self.brake_font = self.pygame.freetype.SysFont('Arial', 24) + + # 转向显示相关字体 + self.steer_font = self.pygame.freetype.SysFont('Arial', 20) + + # 通用信息字体 + self.info_font = self.pygame.freetype.SysFont('Arial', 16) + + # 初始化时间 + self.start_time = time.time() + # draw on the camera perspective def __w_locs_2_camera_locs(self, w_locs): @@ -55,7 +68,7 @@ def __draw_camera_line_safe(self, color, pt1, pt2, width=1): 0] >= 0 and pt2[0] <= Config.PYGAME_WIDTH and pt2[1] >= 0 and pt2[1] <= Config.PYGAME_HEIGHT): self.pygame.draw.line(self.main.surface, color, pt1, pt2, width) - # 新增:绘制点的方法(用于修复AttributeError) + # 绘制点的方法 def draw_point(self, location, color, radius=3): """在相机视角下绘制一个点""" cam_loc = self.__w_locs_2_camera_locs([location])[0] @@ -63,7 +76,7 @@ def draw_point(self, location, color, radius=3): 1] <= Config.PYGAME_HEIGHT: self.pygame.draw.circle(self.main.surface, color, cam_loc, radius) - # 新增:显示速度方法 + # 显示速度方法 def display_speed(self, speed_kmh): """在屏幕右上角显示当前速度""" # 设置速度显示位置 @@ -88,7 +101,7 @@ def display_speed(self, speed_kmh): unit_text = "km/h" self.speed_font_small.render_to(self.main.surface, (pos_x + 100, pos_y + 15), unit_text, (200, 200, 200)) - # 可选:绘制速度条背景 + # 绘制速度条背景 bar_width = 150 bar_height = 10 bar_x = pos_x @@ -107,6 +120,366 @@ def display_speed(self, speed_kmh): # 绘制速度条边框 self.pygame.draw.rect(self.main.surface, (255, 255, 255), bar_bg_rect, 1) + # 显示刹车状态方法 + def display_brake_status(self, is_braking, brake_history, target_speed, frame_count): + """在屏幕左上角显示刹车状态""" + # 设置显示位置(屏幕左上角) + pos_x = 30 + pos_y = 30 + + # 测试模式:在前200帧强制显示刹车状态,让用户能看到效果 + if frame_count < 200: + # 测试模式:每50帧切换一次状态,演示效果 + test_braking = (frame_count // 50) % 2 == 0 + title_text = f"BRAKE STATUS (TEST MODE) - Target: {target_speed} km/h" + self.brake_font.render_to(self.main.surface, (pos_x, pos_y), title_text, (200, 200, 200)) + + if test_braking: + # 测试刹车状态:红色闪烁 + brake_text = "BRAKING (TEST)" + intensity = 200 + 55 * ((frame_count // 5) % 2) # 闪烁效果 + + # 绘制红色背景框 + bg_rect = self.pygame.Rect(pos_x - 10, pos_y + 30, 180, 40) + self.pygame.draw.rect(self.main.surface, (intensity // 4, 0, 0), bg_rect) + self.pygame.draw.rect(self.main.surface, (intensity, 0, 0), bg_rect, 3) + self.brake_font.render_to(self.main.surface, (pos_x + 10, pos_y + 45), brake_text, + (intensity, intensity // 3, intensity // 3)) + else: + # 测试正常状态:绿色 + brake_text = "NORMAL (TEST)" + bg_rect = self.pygame.Rect(pos_x - 10, pos_y + 30, 180, 40) + self.pygame.draw.rect(self.main.surface, (0, 30, 0), bg_rect) + self.pygame.draw.rect(self.main.surface, (0, 180, 0), bg_rect, 2) + self.brake_font.render_to(self.main.surface, (pos_x + 10, pos_y + 45), brake_text, (0, 255, 100)) + + # 添加测试模式说明 + self.info_font.render_to(self.main.surface, (pos_x, pos_y + 80), + "Test Mode: Forced display of brake states", (255, 255, 0)) + return + + # 正常模式 + # 检查是否在刹车(使用历史记录创建闪烁效果) + should_flash = False + if len(brake_history) >= 5: + # 如果最近5帧中有3帧在刹车,则显示刹车状态 + recent_brakes = brake_history[-5:] + if sum(recent_brakes) >= 3: + should_flash = True + + # 显示标题和目标速度 + title_text = f"BRAKE STATUS - Target: {target_speed} km/h" + self.brake_font.render_to(self.main.surface, (pos_x, pos_y), title_text, (200, 200, 200)) + + if is_braking or should_flash: + # 刹车状态:红色闪烁 + brake_text = "BRAKING" + + # 闪烁效果:根据时间改变亮度 + intensity = 200 + 55 * ((frame_count // 5) % 2) # 每5帧闪烁一次 + + # 绘制红色背景框 + bg_rect = self.pygame.Rect(pos_x - 10, pos_y + 30, 150, 40) + self.pygame.draw.rect(self.main.surface, (intensity // 4, 0, 0), bg_rect) + + # 绘制红色边框 + self.pygame.draw.rect(self.main.surface, (intensity, 0, 0), bg_rect, 3) + + # 显示"BRAKING"文字 + self.brake_font.render_to(self.main.surface, (pos_x + 10, pos_y + 45), brake_text, + (intensity, intensity // 3, intensity // 3)) + + # 添加警告符号 + warning_color = (intensity, intensity, 0) # 黄色警告 + self.pygame.draw.circle(self.main.surface, warning_color, (pos_x + 120, pos_y + 50), 12) + warning_font = self.pygame.freetype.SysFont('Arial', 16) + warning_font.render_to(self.main.surface, (pos_x + 115, pos_y + 44), "!", (0, 0, 0)) + else: + # 正常状态:绿色 + brake_text = "NORMAL" + + # 绘制绿色背景框 + bg_rect = self.pygame.Rect(pos_x - 10, pos_y + 30, 150, 40) + self.pygame.draw.rect(self.main.surface, (0, 30, 0), bg_rect) + + # 绘制绿色边框 + self.pygame.draw.rect(self.main.surface, (0, 180, 0), bg_rect, 2) + + # 显示"NORMAL"文字 + self.brake_font.render_to(self.main.surface, (pos_x + 10, pos_y + 45), brake_text, (0, 255, 100)) + + # 显示速度历史图表 + def display_speed_history(self, speed_history, target_speed): + """在屏幕左下角显示速度历史图表""" + if len(speed_history) < 2: + return + + # 图表位置和大小 + chart_x = 30 + chart_y = Config.PYGAME_HEIGHT - 150 + chart_width = 300 + chart_height = 120 + + # 绘制图表背景 + chart_bg_rect = self.pygame.Rect(chart_x, chart_y, chart_width, chart_height) + self.pygame.draw.rect(self.main.surface, (20, 20, 20), chart_bg_rect) + self.pygame.draw.rect(self.main.surface, (100, 100, 100), chart_bg_rect, 2) + + # 绘制图表标题 + title_font = self.pygame.freetype.SysFont('Arial', 16) + title_font.render_to(self.main.surface, (chart_x + 5, chart_y - 20), "SPEED HISTORY", (200, 200, 200)) + + # 计算速度和目标速度的最小值、最大值 + all_speeds = speed_history + [target_speed] + min_speed = min(all_speeds) - 5 + max_speed = max(all_speeds) + 5 + + # 绘制目标速度线 + if min_speed <= target_speed <= max_speed: + target_y = chart_y + chart_height - int((target_speed - min_speed) / (max_speed - min_speed) * chart_height) + self.pygame.draw.line( + self.main.surface, + (0, 255, 0), # 绿色目标线 + (chart_x, target_y), + (chart_x + chart_width, target_y), + 2 + ) + + # 标注目标速度值 + target_font = self.pygame.freetype.SysFont('Arial', 12) + target_font.render_to(self.main.surface, (chart_x + chart_width + 5, target_y - 10), + f"Target: {target_speed} km/h", (0, 255, 0)) + + # 绘制速度历史曲线 + points = [] + for i, speed in enumerate(speed_history): + if i >= chart_width: # 只显示最近chart_width个数据点 + speed_subset = speed_history[-chart_width:] + break + + x = chart_x + int(i * chart_width / min(len(speed_history), chart_width)) + y = chart_y + chart_height - int((speed - min_speed) / (max_speed - min_speed) * chart_height) + points.append((x, y)) + + # 连接点成线 + if len(points) > 1: + # 速度线:蓝色 + self.pygame.draw.lines(self.main.surface, (100, 150, 255), False, points, 2) + + # 绘制最后一个点(当前速度) + if points: + last_point = points[-1] + self.pygame.draw.circle(self.main.surface, (255, 255, 255), last_point, 4) + + # 标注当前速度值 + current_speed = speed_history[-1] + speed_font = self.pygame.freetype.SysFont('Arial', 12) + speed_font.render_to( + self.main.surface, + (last_point[0] + 10, last_point[1] - 10), + f"{current_speed:.1f} km/h", + (255, 255, 255) + ) + + # 显示转向角度功能 + def display_steering(self, steer_angle): + """在屏幕右下角显示转向角度""" + # 获取屏幕尺寸 + screen_width = Config.PYGAME_WIDTH + screen_height = Config.PYGAME_HEIGHT + + # 设置在屏幕右下角 + pos_x = screen_width - 220 + pos_y = 120 + + # 将转向角度转换为度数和可视化角度 + angle_degrees = steer_angle * 45 # 假设-1到1对应-45度到45度 + + # 根据转向角度设置颜色 + if abs(angle_degrees) < 5: + color = (0, 255, 0) # 绿色 - 直行或小角度 + elif abs(angle_degrees) < 15: + color = (255, 255, 0) # 黄色 - 中等角度 + else: + color = (255, 100, 0) # 橙色 - 大角度 + + # 显示标题 + title_text = "STEERING ANGLE" + self.steer_font.render_to(self.main.surface, (pos_x, pos_y), title_text, (200, 200, 200)) + + # 显示角度值 + angle_text = f"{angle_degrees:+.1f}°" + self.steer_font.render_to(self.main.surface, (pos_x, pos_y + 30), angle_text, color) + + # 显示原始值 + raw_text = f"Raw: {steer_angle:+.3f}" + self.info_font.render_to(self.main.surface, (pos_x, pos_y + 60), raw_text, (150, 150, 150)) + + # 绘制转向可视化指示器 + indicator_width = 180 + indicator_height = 40 + indicator_x = pos_x - 10 + indicator_y = pos_y + 90 + + # 绘制背景 + indicator_bg = self.pygame.Rect(indicator_x, indicator_y, indicator_width, indicator_height) + self.pygame.draw.rect(self.main.surface, (40, 40, 40), indicator_bg) + self.pygame.draw.rect(self.main.surface, (100, 100, 100), indicator_bg, 2) + + # 绘制中心线 + center_x = indicator_x + indicator_width // 2 + self.pygame.draw.line( + self.main.surface, + (200, 200, 200), + (center_x, indicator_y), + (center_x, indicator_y + indicator_height), + 2 + ) + + # 绘制转向指示器 + indicator_center = center_x + int((indicator_width // 2 - 10) * steer_angle) + indicator_radius = 12 + + # 绘制指示器圆圈 + self.pygame.draw.circle(self.main.surface, color, (indicator_center, indicator_y + indicator_height // 2), + indicator_radius) + + # 绘制方向箭头 + arrow_size = 8 + if steer_angle > 0.01: # 右转 + arrow_points = [ + (indicator_center - arrow_size, indicator_y + indicator_height // 2 - arrow_size), + (indicator_center - arrow_size, indicator_y + indicator_height // 2 + arrow_size), + (indicator_center, indicator_y + indicator_height // 2) + ] + elif steer_angle < -0.01: # 左转 + arrow_points = [ + (indicator_center + arrow_size, indicator_y + indicator_height // 2 - arrow_size), + (indicator_center + arrow_size, indicator_y + indicator_height // 2 + arrow_size), + (indicator_center, indicator_y + indicator_height // 2) + ] + else: # 直行 + arrow_points = [ + (indicator_center - arrow_size, indicator_y + indicator_height // 2), + (indicator_center + arrow_size, indicator_y + indicator_height // 2), + (indicator_center, indicator_y + indicator_height // 2 + arrow_size) + ] + + self.pygame.draw.polygon(self.main.surface, (255, 255, 255), arrow_points) + + # 添加标签 + label_font = self.pygame.freetype.SysFont('Arial', 12) + label_font.render_to(self.main.surface, (indicator_x + 5, indicator_y + indicator_height + 5), "LEFT", + (150, 150, 150)) + label_font.render_to(self.main.surface, + (indicator_x + indicator_width - 40, indicator_y + indicator_height + 5), "RIGHT", + (150, 150, 150)) + label_font.render_to(self.main.surface, (center_x - 20, indicator_y + indicator_height + 5), "CENTER", + (150, 150, 150)) + + # 显示油门和刹车信息 + def display_throttle_info(self, throttle_value, brake_value): + """在屏幕右侧中部显示油门和刹车信息""" + # 设置显示位置 + pos_x = Config.PYGAME_WIDTH - 220 + pos_y = 250 + + # 显示标题 + title_text = "CONTROL INPUTS" + self.steer_font.render_to(self.main.surface, (pos_x, pos_y), title_text, (200, 200, 200)) + + # 油门显示 + throttle_text = f"Throttle: {throttle_value:.2f}" + throttle_color = (0, int(255 * throttle_value), 0) # 绿色,亮度随油门变化 + self.info_font.render_to(self.main.surface, (pos_x, pos_y + 30), throttle_text, throttle_color) + + # 油门条 + throttle_bar_width = 150 + throttle_bar_height = 10 + throttle_bar_x = pos_x + throttle_bar_y = pos_y + 50 + + throttle_bar_bg = self.pygame.Rect(throttle_bar_x, throttle_bar_y, throttle_bar_width, throttle_bar_height) + self.pygame.draw.rect(self.main.surface, (50, 50, 50), throttle_bar_bg) + + throttle_filled_width = int(throttle_bar_width * throttle_value) + throttle_filled_rect = self.pygame.Rect(throttle_bar_x, throttle_bar_y, throttle_filled_width, + throttle_bar_height) + self.pygame.draw.rect(self.main.surface, throttle_color, throttle_filled_rect) + self.pygame.draw.rect(self.main.surface, (255, 255, 255), throttle_bar_bg, 1) + + # 刹车显示 + brake_text = f"Brake: {brake_value:.2f}" + brake_color = (int(255 * brake_value), 0, 0) # 红色,亮度随刹车力度变化 + self.info_font.render_to(self.main.surface, (pos_x, pos_y + 70), brake_text, brake_color) + + # 刹车条 + brake_bar_width = 150 + brake_bar_height = 10 + brake_bar_x = pos_x + brake_bar_y = pos_y + 90 + + brake_bar_bg = self.pygame.Rect(brake_bar_x, brake_bar_y, brake_bar_width, brake_bar_height) + self.pygame.draw.rect(self.main.surface, (50, 50, 50), brake_bar_bg) + + brake_filled_width = int(brake_bar_width * brake_value) + brake_filled_rect = self.pygame.Rect(brake_bar_x, brake_bar_y, brake_filled_width, brake_bar_height) + self.pygame.draw.rect(self.main.surface, brake_color, brake_filled_rect) + self.pygame.draw.rect(self.main.surface, (255, 255, 255), brake_bar_bg, 1) + + # 显示控制模式 + def display_control_mode(self, control_mode): + """在屏幕顶部中央显示控制模式""" + pos_x = Config.PYGAME_WIDTH // 2 - 100 + pos_y = 10 + + if control_mode == "AUTO": + color = (0, 200, 255) # 青色 + elif control_mode == "MANUAL": + color = (255, 200, 0) # 橙色 + else: + color = (255, 255, 255) # 白色 + + # 绘制背景框 + bg_rect = self.pygame.Rect(pos_x - 10, pos_y, 220, 40) + self.pygame.draw.rect(self.main.surface, (20, 20, 20, 128), bg_rect) + self.pygame.draw.rect(self.main.surface, color, bg_rect, 2) + + # 显示文本 + mode_text = f"CONTROL MODE: {control_mode}" + mode_font = self.pygame.freetype.SysFont('Arial', 24) + mode_font.render_to(self.main.surface, (pos_x, pos_y + 10), mode_text, color) + + # 显示帧信息 + def display_frame_info(self, frame_count, dt): + """在屏幕左下角显示帧信息""" + pos_x = 30 + pos_y = Config.PYGAME_HEIGHT - 200 + + # 计算FPS + fps = 1.0 / dt if dt > 0 else 0 + + # 计算运行时间 + current_time = time.time() + elapsed_time = current_time - self.start_time + + # 显示信息 + info_texts = [ + f"Frame: {frame_count}", + f"FPS: {fps:.1f}", + f"Time: {elapsed_time:.1f}s", + f"DT: {dt:.3f}s" + ] + + # 绘制背景 + bg_rect = self.pygame.Rect(pos_x - 10, pos_y - 10, 150, 100) + self.pygame.draw.rect(self.main.surface, (20, 20, 20, 128), bg_rect) + self.pygame.draw.rect(self.main.surface, (100, 100, 100), bg_rect, 1) + + # 显示每行信息 + for i, text in enumerate(info_texts): + self.info_font.render_to(self.main.surface, (pos_x, pos_y + i * 20), text, (200, 200, 200)) + @staticmethod def get_location_bbox(location, camera): bb_cords = np.array([[0, 0, 0, 1]]) diff --git a/src/unmannedcar_MPC/main.py b/src/unmannedcar_MPC/main.py index 6058fcbf72..ade69af70b 100644 --- a/src/unmannedcar_MPC/main.py +++ b/src/unmannedcar_MPC/main.py @@ -1,209 +1,153 @@ +#!/usr/bin/env python3 + import carla import config as Config -import numpy as np import math - - -class PyGameDrawer(): - - def __init__(self, main): - self.main = main - self.pygame = main.game.pygame - self.camera = main.game.camera - self.font_14 = self.pygame.freetype.SysFont('Times New Roman', 14) - - # 新增:速度显示相关字体 - self.speed_font_large = self.pygame.freetype.SysFont('Arial', 36) - self.speed_font_small = self.pygame.freetype.SysFont('Arial', 18) - - # draw on the camera perspective - - def __w_locs_2_camera_locs(self, w_locs): - camera_locs = [] - for w_loc in w_locs: - bbox = PyGameDrawer.get_location_bbox(w_loc, self.camera) - if math.isnan(bbox[0, 0]) or math.isnan(bbox[0, 1]): - camera_locs.append((-1, -1)) - camera_locs.append((int(bbox[0, 0]), int(bbox[0, 1]))) - return camera_locs - - def draw_camera_text(self, location, color, text): - x, y = self.__w_locs_2_camera_locs([location])[0] - if x >= 0 and x <= Config.PYGAME_WIDTH and y >= 0 and y <= Config.PYGAME_HEIGHT: - self.font_14.render_to(self.main.surface, (x, y), text, color) - - def draw_camera_circles(self, w_locs, color, radius): - cam_locs = self.__w_locs_2_camera_locs(w_locs) - for cam_loc in cam_locs: - self.pygame.draw.circle( - self.main.surface, color, cam_loc, radius, 1) - - def draw_camera_polygon(self, w_locs, color): - if len(w_locs) < 3: - return - points = self.__w_locs_2_camera_locs(w_locs) - self.pygame.draw.polygon(self.main.surface, color, points, 4) - - def draw_camera_lines(self, color, w_locs, width=1): - cam_locs = self.__w_locs_2_camera_locs(w_locs) - for i in range(len(cam_locs) - 1): - self.__draw_camera_line_safe(color, [cam_locs[i][0], cam_locs[i][1]], [ - cam_locs[i + 1][0], cam_locs[i + 1][1]], width) - - def __draw_camera_line_safe(self, color, pt1, pt2, width=1): - if (pt1[0] >= 0 and pt1[0] <= Config.PYGAME_WIDTH and pt1[1] >= 0 and pt1[1] <= Config.PYGAME_HEIGHT and pt2[ - 0] >= 0 and pt2[0] <= Config.PYGAME_WIDTH and pt2[1] >= 0 and pt2[1] <= Config.PYGAME_HEIGHT): - self.pygame.draw.line(self.main.surface, color, pt1, pt2, width) - - # 新增:绘制点的方法(用于修复AttributeError) - def draw_point(self, location, color, radius=3): - """在相机视角下绘制一个点""" - cam_loc = self.__w_locs_2_camera_locs([location])[0] - if cam_loc[0] >= 0 and cam_loc[0] <= Config.PYGAME_WIDTH and cam_loc[1] >= 0 and cam_loc[ - 1] <= Config.PYGAME_HEIGHT: - self.pygame.draw.circle(self.main.surface, color, cam_loc, radius) - - # 新增:显示速度方法 - def display_speed(self, speed_kmh): - """在屏幕右上角显示当前速度""" - # 设置速度显示位置 - pos_x = Config.PYGAME_WIDTH - 180 # 屏幕右侧 - pos_y = 30 # 距离顶部30像素 - - # 根据速度设置颜色 - if speed_kmh < 30: - color = (0, 255, 0) # 绿色 - 低速 - elif speed_kmh < 60: - color = (255, 255, 0) # 黄色 - 中速 - elif speed_kmh < 90: - color = (255, 165, 0) # 橙色 - 中高速 +from drawer import PyGameDrawer +from sync_pygame import SyncPyGame +from mpc import MPC + + +class Main(): + + def __init__(self): + # setup world + self.client = carla.Client(Config.CARLA_SERVER, 2000) + self.client.set_timeout(10.0) + self.world = self.client.load_world(Config.WORLD_NAME) + self.map = self.world.get_map() + + # spawn ego + ego_spawn_point = self.map.get_spawn_points()[100] + bp = self.world.get_blueprint_library().filter('vehicle.tesla.model3')[0] + self.ego = self.world.spawn_actor(bp, ego_spawn_point) + + # init game and drawer + self.game = SyncPyGame(self) + self.drawer = PyGameDrawer(self) + self.mpc = MPC(self.drawer, self.ego) + + # 刹车状态跟踪 + self.is_braking = False + self.brake_history = [] + self.speed_history = [] # 速度历史记录 + self.target_speed_kmh = 40 # 目标速度40km/h + self.brake_force = 0.0 # 当前刹车力度 + self.frame_count = 0 # 帧计数 + self.steer_angle = 0.0 # 转向角度 + self.throttle_value = 0.6 # 油门值 + self.control_mode = "AUTO" # 控制模式 + + # start game loop + self.game.game_loop(self.world, self.on_tick) + + def on_tick(self): + self.frame_count += 1 + + # generate reference path (global frame) + lookahead = 5 + wp = self.map.get_waypoint(self.ego.get_location()) + path = [] + + for _ in range(lookahead): + _wps = wp.next(1) + if len(_wps) == 0: + break + wp = _wps[0] + path.append(wp.transform.location) + + # get forward speed + velocity = self.ego.get_velocity() + speed_m_s = math.sqrt(velocity.x ** 2 + velocity.y ** 2 + velocity.z ** 2) + + # 计算当前速度(km/h) + current_speed_kmh = speed_m_s * 3.6 # m/s to km/h + + # 记录速度历史(用于显示) + self.speed_history.append(current_speed_kmh) + if len(self.speed_history) > 100: # 保留最近100帧的速度历史 + self.speed_history.pop(0) + + dt = 1 / Config.PYGAME_FPS + + # generate control signal + control = carla.VehicleControl() + + # 智能速度控制逻辑 + if self.frame_count < 100: + # 前100帧:全力加速 + control.throttle = 1.0 # 最大油门 + control.brake = 0.0 + self.brake_force = 0.0 + self.is_braking = False + self.throttle_value = 1.0 + elif self.frame_count < 150: + # 第100-150帧:维持油门,让速度继续上升 + control.throttle = 0.8 + control.brake = 0.0 + self.brake_force = 0.0 + self.is_braking = False + self.throttle_value = 0.8 else: - color = (255, 0, 0) # 红色 - 高速 - - # 显示速度值(大字体) - speed_text = f"{speed_kmh:.1f}" - self.speed_font_large.render_to(self.main.surface, (pos_x, pos_y), speed_text, color) - - # 显示单位(小字体) - unit_text = "km/h" - self.speed_font_small.render_to(self.main.surface, (pos_x + 100, pos_y + 15), unit_text, (200, 200, 200)) - - # 可选:绘制速度条背景 - bar_width = 150 - bar_height = 10 - bar_x = pos_x - bar_y = pos_y + 50 - - # 绘制速度条背景 - bar_bg_rect = self.pygame.Rect(bar_x, bar_y, bar_width, bar_height) - self.pygame.draw.rect(self.main.surface, (50, 50, 50), bar_bg_rect) - - # 绘制速度条填充(根据速度比例) - speed_ratio = min(speed_kmh / 120.0, 1.0) # 假设最大速度120 km/h - bar_filled_width = int(bar_width * speed_ratio) - bar_filled_rect = self.pygame.Rect(bar_x, bar_y, bar_filled_width, bar_height) - self.pygame.draw.rect(self.main.surface, color, bar_filled_rect) - - # 绘制速度条边框 - self.pygame.draw.rect(self.main.surface, (255, 255, 255), bar_bg_rect, 1) - - @staticmethod - def get_location_bbox(location, camera): - bb_cords = np.array([[0, 0, 0, 1]]) - cords_x_y_z = PyGameDrawer.location_to_sensor_cords( - bb_cords, location, camera)[:3, :] - cords_y_minus_z_x = np.concatenate( - [cords_x_y_z[1, :], -cords_x_y_z[2, :], cords_x_y_z[0, :]]) - bbox = np.transpose(np.dot(camera.calibration, cords_y_minus_z_x)) - camera_bbox = np.concatenate( - [bbox[:, 0] / bbox[:, 2], bbox[:, 1] / bbox[:, 2], bbox[:, 2]], axis=1) - return camera_bbox - - @staticmethod - def location_to_sensor_cords(cords, location, sensor): - world_cord = PyGameDrawer.location_to_world_cords(cords, location) - sensor_cord = PyGameDrawer._world_to_sensor_cords(world_cord, sensor) - return sensor_cord - - @staticmethod - def location_to_world_cords(cords, location): - bb_transform = carla.Transform(location) - vehicle_world_matrix = PyGameDrawer.get_matrix(bb_transform) - world_cords = np.dot(vehicle_world_matrix, np.transpose(cords)) - return world_cords - - @staticmethod - def _create_vehicle_bbox_points(vehicle): - """ - Returns 3D bounding box for a vehicle. - """ - cords = np.zeros((8, 4)) - extent = vehicle.bounding_box.extent - cords[0, :] = np.array([extent.x, extent.y, -extent.z, 1]) - cords[1, :] = np.array([-extent.x, extent.y, -extent.z, 1]) - cords[2, :] = np.array([-extent.x, -extent.y, -extent.z, 1]) - cords[3, :] = np.array([extent.x, -extent.y, -extent.z, 1]) - cords[4, :] = np.array([extent.x, extent.y, extent.z, 1]) - cords[5, :] = np.array([-extent.x, extent.y, extent.z, 1]) - cords[6, :] = np.array([-extent.x, -extent.y, extent.z, 1]) - cords[7, :] = np.array([extent.x, -extent.y, extent.z, 1]) - return cords - - @staticmethod - def _vehicle_to_sensor_cords(cords, vehicle, sensor): - """ - Transforms coordinates of a vehicle bounding box to sensor. - """ - world_cord = PyGameDrawer._vehicle_to_world_cords(cords, vehicle) - sensor_cord = PyGameDrawer._world_to_sensor_cords(world_cord, sensor) - return sensor_cord - - @staticmethod - def _vehicle_to_world_cords(cords, vehicle): - """ - Transforms coordinates of a vehicle bounding box to world. - """ - bb_transform = carla.Transform(vehicle.bounding_box.location) - bb_vehicle_matrix = PyGameDrawer.get_matrix(bb_transform) - vehicle_world_matrix = PyGameDrawer.get_matrix(vehicle.get_transform()) - bb_world_matrix = np.dot(vehicle_world_matrix, bb_vehicle_matrix) - world_cords = np.dot(bb_world_matrix, np.transpose(cords)) - return world_cords - - @staticmethod - def _world_to_sensor_cords(cords, sensor): - """ - Transforms world coordinates to sensor. - """ - sensor_world_matrix = PyGameDrawer.get_matrix(sensor.get_transform()) - world_sensor_matrix = np.linalg.inv(sensor_world_matrix) - sensor_cords = np.dot(world_sensor_matrix, cords) - return sensor_cords - - @staticmethod - def get_matrix(transform): - """ - Creates matrix from carla transform. - """ - rotation = transform.rotation - location = transform.location - c_y = np.cos(np.radians(rotation.yaw)) - s_y = np.sin(np.radians(rotation.yaw)) - c_r = np.cos(np.radians(rotation.roll)) - s_r = np.sin(np.radians(rotation.roll)) - c_p = np.cos(np.radians(rotation.pitch)) - s_p = np.sin(np.radians(rotation.pitch)) - matrix = np.matrix(np.identity(4)) - matrix[0, 3] = location.x - matrix[1, 3] = location.y - matrix[2, 3] = location.z - matrix[0, 0] = c_p * c_y - matrix[0, 1] = c_y * s_p * s_r - s_y * c_r - matrix[0, 2] = -c_y * s_p * c_r - s_y * s_r - matrix[1, 0] = s_y * c_p - matrix[1, 1] = s_y * s_p * s_r + c_y * c_r - matrix[1, 2] = -s_y * s_p * c_r + c_y * s_r - matrix[2, 0] = s_p - matrix[2, 1] = -c_p * s_r - matrix[2, 2] = c_p * c_r - return matrix \ No newline at end of file + # 150帧后:开始速度控制 + speed_error = current_speed_kmh - self.target_speed_kmh + + # 动态调整目标速度 + if current_speed_kmh > 45: # 如果速度能到45以上,提高目标速度 + self.target_speed_kmh = 45 + + if speed_error > 5: # 超过目标速度5km/h时强力刹车 + control.throttle = 0.0 + self.brake_force = min(self.brake_force + 0.1, 1.0) # 快速增加刹车力度 + control.brake = self.brake_force + self.is_braking = True + self.throttle_value = 0.0 + elif speed_error > 2: # 超过目标速度2km/h时轻微刹车 + control.throttle = 0.0 + self.brake_force = min(self.brake_force + 0.05, 0.5) # 中等刹车 + control.brake = self.brake_force + self.is_braking = True + self.throttle_value = 0.0 + elif current_speed_kmh < self.target_speed_kmh - 5: # 低于目标速度时全力加速 + control.throttle = 1.0 + self.brake_force = 0.0 + control.brake = 0.0 + self.is_braking = False + self.throttle_value = 1.0 + elif current_speed_kmh < self.target_speed_kmh - 2: # 接近目标速度但稍低 + control.throttle = 0.6 + self.brake_force = 0.0 + control.brake = 0.0 + self.is_braking = False + self.throttle_value = 0.6 + else: # 接近目标速度时维持 + control.throttle = 0.3 + self.brake_force = max(self.brake_force - 0.02, 0.0) # 逐渐释放刹车 + control.brake = self.brake_force + self.is_braking = self.brake_force > 0.05 # 只有刹车力度大于0.05时才显示刹车状态 + self.throttle_value = 0.3 + + # 记录刹车状态历史(用于闪烁效果) + self.brake_history.append(self.is_braking) + if len(self.brake_history) > 20: # 保持最近20帧的记录 + self.brake_history.pop(0) + + # MPC控制转向 + control.steer = self.mpc.run_step(path, speed_m_s, dt) + self.steer_angle = control.steer # 保存转向角度 + + # apply control signal + self.ego.apply_control(control) + + # 在屏幕上显示所有信息 + self.drawer.display_speed(current_speed_kmh) + self.drawer.display_brake_status(self.is_braking, self.brake_history, self.target_speed_kmh, self.frame_count) + self.drawer.display_speed_history(self.speed_history, self.target_speed_kmh) + self.drawer.display_steering(self.steer_angle) + self.drawer.display_throttle_info(self.throttle_value, self.brake_force) + self.drawer.display_control_mode(self.control_mode) + self.drawer.display_frame_info(self.frame_count, dt) + + +if __name__ == '__main__': + Main() \ No newline at end of file diff --git a/yolo_det.py b/yolo_det.py new file mode 100644 index 0000000000..d37aa118aa --- /dev/null +++ b/yolo_det.py @@ -0,0 +1,60 @@ +from ultralytics import YOLO +import cv2 +import numpy as np + + +class ObjectDetector: + def __init__(self, model_name='yolov8n.pt'): + """ + 初始化 YOLO 模型 + :param model_name: 模型名称,'yolov8n.pt' 是最快最小的模型,适合CPU + """ + print(f"正在加载 YOLO 模型: {model_name} (首次运行会自动下载)...") + self.model = YOLO(model_name) + + # 定义我们关心的类别ID (COCO数据集) + # 2: car, 3: motorcycle, 5: bus, 7: truck + self.target_classes = [2, 3, 5, 7] + + def detect(self, frame): + """ + 输入一帧图像,返回画好框的图像 + """ + # 运行推理,stream=True 节省内存,verbose=False 关闭打印日志 + results = self.model(frame, stream=True, verbose=False) + + # 在原图的副本上绘图 + annotated_frame = frame.copy() + + for result in results: + boxes = result.boxes + for box in boxes: + # 获取类别ID + cls_id = int(box.cls[0]) + + # 只处理车辆相关的类别 + if cls_id in self.target_classes: + # 获取坐标 (x1, y1, x2, y2) + x1, y1, x2, y2 = box.xyxy[0] + x1, y1, x2, y2 = int(x1), int(y1), int(x2), int(y2) + + # 获取置信度 + conf = float(box.conf[0]) + if conf < 0.3: continue # 过滤掉置信度低的 + + # 获取类别名称 + cls_name = self.model.names[cls_id] + + # 绘制矩形框 (红色) + cv2.rectangle(annotated_frame, (x1, y1), (x2, y2), (0, 0, 255), 2) + + # 绘制标签背景 + label = f"{cls_name} {conf:.2f}" + (w, h), _ = cv2.getTextSize(label, cv2.FONT_HERSHEY_SIMPLEX, 0.5, 1) + cv2.rectangle(annotated_frame, (x1, y1 - 20), (x1 + w, y1), (0, 0, 255), -1) + + # 绘制文字 + cv2.putText(annotated_frame, label, (x1, y1 - 5), + cv2.FONT_HERSHEY_SIMPLEX, 0.5, (255, 255, 255), 1) + + return annotated_frame \ No newline at end of file