From 96f06c546b715294a0e935fef53aaad761f4836f Mon Sep 17 00:00:00 2001 From: Xinyue-cao <648343923@qq.com> Date: Tue, 16 Dec 2025 18:36:12 +0800 Subject: [PATCH 1/4] =?UTF-8?q?=E6=9B=B4=E6=96=B0src/box/README.md?= =?UTF-8?q?=E5=86=85=E5=AE=B9?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- src/box/README.md | 103 +++++++++++++++++++++++----------------------- 1 file changed, 51 insertions(+), 52 deletions(-) diff --git a/src/box/README.md b/src/box/README.md index a4f32c0853..d1d4fe9282 100644 --- a/src/box/README.md +++ b/src/box/README.md @@ -1,60 +1,59 @@ -user-in-the-box-simulator -基于Gymnasium和MuJoCo的仿真器,集成生物力学模型、感知模块与强化学习任务,支持分层强化学习与可视化渲染 +**box — 仿真与强化学习实验箱** -一、环境准备 -1.虚拟环境创建与激活 -# 切换到项目根目录 -cd C:\Users\86186\user-in-the-box +简介 +- `src/box` 目录包含基于 Gymnasium 和 MuJoCo 的仿真环境与相关辅助脚本,用于开发和测试生物力学/机器人仿真、感知模块与强化学习任务。 -# 创建Python 3.9虚拟环境(兼容性最优) -python -m venv venv --python=3.9 - -# 激活虚拟环境(Windows PowerShell) -\venv\Scripts\Activate.ps1 -2.依赖安装 使用国内镜像源加速安装: -# 核心依赖 -pip install gymnasium==1.2.1 mujoco==2.3.5 stable-baselines3==2.2.1 pygame==2.5.2 opencv-python==4.9.0.80 -i https://pypi.tuna.tsinghua.edu.cn/simple - -# 辅助依赖 -pip install numpy==1.26.4 scipy==1.11.4 matplotlib==3.8.4 ruamel.yaml==0.18.6 certifi -i https://pypi.tuna.tsinghua.edu.cn/simple +目录结构(示例) +- `simulator.py`:仿真环境核心(通常继承 `gym.Env`)。 +- `test_simulator.py`:示例运行脚本,用于启动仿真并可视化。 +- `main.py`:辅助脚本(例如证书或配置检查)。 +- `README.md`:本文件,说明目录用途与快速上手指南。 -二、核心文件说明 +快速上手 +1. 创建并激活虚拟环境(以 Windows 为例): -1.simulator.py(仿真器核心) -功能:继承 gym.Env,实现仿真环境的初始化、步骤推进(step)、环境重置(reset)和可视化渲染(render),集成生物力学模型、感知模块和任务逻辑。 运行方式:需通过调用脚本(如 test_simulator.py)运行,示例见 “三、运行步骤”。 -2.main.py(辅助脚本) 功能:基于 certifi 查询 CA 证书信息(路径或内容),用于验证网络请求的安全性。 -运行方式: -# 查看证书路径 -python main.py +```powershell +cd <项目根目录> +python -m venv venv --python=3.9 +.\\venv\\Scripts\\Activate.ps1 +``` + +2. 安装依赖(建议使用清华镜像加速): + +```powershell +pip install -r requirements.txt -i https://pypi.tuna.tsinghua.edu.cn/simple +``` + +如果仓库没有完整的 `requirements.txt`,可参考下列核心库: + +```text +gymnasium +mujoco +stable-baselines3 +pygame +opencv-python +numpy +scipy +matplotlib +ruamel.yaml +certifi +``` + +运行示例 +- 启动仿真: + +```powershell +python test_simulator.py +``` -# 查看证书内容 -python main.py -c -三、运行步骤 +运行后应弹出可视化窗口(若使用 Pygame/SDL),并在终端输出仿真日志。 -仿真器运行(simulator.py) -步骤 1:运行脚本 test_simulator.py +贡献与问题反馈 +- 若需添加说明或示例,请提交 Pull Request。 +- 遇到环境或依赖问题,请在 Issue 中描述操作系统、Python 版本与错误日志。 -步骤 2:执行脚本 +更多信息 +- 若目录中包含更详细的子模块文档,请参阅相应文件(如 `simulator.py` 顶部注释或同目录下的文档)。 -python test_simulator.py -此时会弹出 Pygame 窗口,展示仿真过程(如机械臂运动、感知模块渲染)。 2. 辅助脚本运行(main.py) 在终端执行以下命令,查看证书信息: - -# 查看证书路径 -python main.py - -# 查看证书内容 -python main.py -c - -四、依赖清单 -|库名称 |版本 |用途| -|------|-------|----| -|gymnasium| 1.2.1 |强化学习环境接口| -|mujoco|2.3.5|物理仿真引擎| -|stable-baselines3|2.2.1|强化学习算法库| -|pygame |2.5.2|可视化渲染| -|opencv-python|4.9.0.80|图像感知处理| -|numpy| 1.26.4| 数值计算| -|scipy| 1.11.4|科学计算| -|matplotlib|3.8.4|数据可视化| -|ruamel.yaml|0.18.6 |配置文件解析| -|certifi |2025.10.10 |CA 证书管理| \ No newline at end of file +--- +(此 README 为目录概览,具体实现与文件名以代码库为准) \ No newline at end of file From a0077bb92bc0db55b74d8a18b35803a5622bb665 Mon Sep 17 00:00:00 2001 From: Xinyue-cao <648343923@qq.com> Date: Tue, 16 Dec 2025 19:55:24 +0800 Subject: [PATCH 2/4] =?UTF-8?q?=E6=B7=BB=E5=8A=A0=20MoBL=20ARMS=20?= =?UTF-8?q?=E4=B8=8A=E8=82=A2=E6=A8=A1=E5=9E=8B?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- src/Content_Creation_Map/README.md | 53 +- .../gesture_data_collector.py | 308 +++++ src/MMAP-DRL-Nav/carla_environment.py | 306 +++-- src/Neuro_Mujoco/main.py | 49 +- src/Unmanned_vehicle_AD_DQN/Model.py | 498 +------- src/Unmanned_vehicle_AD_DQN/Test.py | 1 + .../TrainingStrategies.py | 501 ++++++++ src/Unmanned_vehicle_AD_DQN/main.py | 9 +- src/box/moblARMslndex.py | 270 +++++ src/drone_perception/dron.py | 561 --------- src/drone_perception/environment_3d.py | 1007 +++++++++++++++++ src/drone_perception/forecast.py | 134 +-- src/drone_perception/main.py | 984 ++++++++++++---- src/drone_perception/path_planning.py | 579 ++++++++++ src/enhance_pedestrian_safety/main.py | 845 ++++++++++---- src/image_object_detection/camera_detector.py | 119 +- src/image_object_detection/image_detector.py | 86 +- src/image_object_detection/main.py | 5 +- src/image_object_detection/ui_handler.py | 94 +- .../carla_environment.py | 21 +- .../run_simulation.py | 17 +- 21 files changed, 4514 insertions(+), 1933 deletions(-) create mode 100644 src/Drone_hand_gesture_project/gesture_data_collector.py create mode 100644 src/Unmanned_vehicle_AD_DQN/TrainingStrategies.py create mode 100644 src/box/moblARMslndex.py delete mode 100644 src/drone_perception/dron.py create mode 100644 src/drone_perception/environment_3d.py create mode 100644 src/drone_perception/path_planning.py diff --git a/src/Content_Creation_Map/README.md b/src/Content_Creation_Map/README.md index 9a6afc1fe0..70d8c947c0 100644 --- a/src/Content_Creation_Map/README.md +++ b/src/Content_Creation_Map/README.md @@ -527,4 +527,55 @@ OpenStreetMap(简称OSM)是一个免费且开源的世界地图数据库, 5. **在弹出的窗口中找到从OpenStreetMap官网导出的后缀为.osm的文件打开** ![](https://github.com/2382613701lzy-netizen/image/blob/main/20251201112324_70_69.jpg) 6. **导入结果如下图** - ![](https://github.com/2382613701lzynetizen/image/blob/main/20251201112350_71_69.png) + ![](https://github.com/2382613701lzy-netizen/image/blob/main/20251201112350_71_69.png) + +# QGIS 下载、安装与中文界面设置完整指南 +以下是QGIS软件从下载、安装到完成中文界面设置的完整步骤。整个过程主要分为下载、安装、设置语言以及获取学习数据四个部分。 + +### 第一步:下载QGIS +前往QGIS官方网站的下载页面 https://qgis.org/download/ 。页面会根据你的操作系统推荐合适的版本。 +![](https://github.com/2382613701lzy-netizen/image/blob/main/20251216121655_88_69.png) +操作系统:软件支持 Windows、macOS、Linux、Android 和 iOS。 + +版本选择: + +最新版:页面会提供最新的稳定版本(例如 QGIS 3.36.x),包含最新功能。 + +长期支持版 (LTR):对于追求稳定性的用户或组织部署,推荐选择 QGIS 3.34 LTR,它经过了更长时间的测试。 + +Windows用户注意:官方自QGIS 3.20起,仅提供64位安装包。 + +### 第二步:安装QGIS +根据你的操作系统,参考以下步骤完成安装。 + +#### *Windows* + +安装步骤 +1. 运行下载的 .exe 安装程序。 +2. 在安装类型界面,普通用户建议选择“标准安装(Standard Install)”。 +3. 按照安装向导提示完成后续步骤。 +#### *macOS* +1. 双击下载的 .dmg 磁盘映像文件。 +2. 将QGIS图标拖拽到“应用程序(Applications)”文件夹中。 +3. 首次启动需右键点击应用并选择“打开”,以通过系统安全验证。 +#### *Linux* +根据你的发行版,使用对应的软件包管理器(如apt、yum)或从官方仓库安装。 +### 第三步:设置为中文界面 +QGIS内置了多语言支持,切换为中文无需安装额外语言包。 + +打开QGIS,在顶部菜单栏点击 Settings (设置) → Options (选项)。 + +在弹出的选项窗口中,在左侧选择 General (常规) 选项卡。 + +在右侧找到 Locale (区域设置) 板块,你会看到 Override system locale (覆盖系统区域设置) 的选项。 + +勾选此选项,然后在下方 Language (语言) 的下拉菜单中,选择 中文(简体) 或 中文(繁体)。 + +点击右下角的 OK (确定) 保存设置。 + +完全关闭并重新启动QGIS,界面就会切换为中文了。 + +注:如果重启后界面未改变,请检查是否成功保存了设置,并确认在Language下拉菜单中正确选择了“中文(中国)”。 + +最后的应用界面如下图说明你的QGIS已经成功配置可以使用了 +![](https://github.com/2382613701lzy-netizen/image/blob/main/20251216121909_89_69.png) diff --git a/src/Drone_hand_gesture_project/gesture_data_collector.py b/src/Drone_hand_gesture_project/gesture_data_collector.py new file mode 100644 index 0000000000..8db9e2863f --- /dev/null +++ b/src/Drone_hand_gesture_project/gesture_data_collector.py @@ -0,0 +1,308 @@ +import cv2 +import numpy as np +import os +import time +import json +from datetime import datetime +import mediapipe as mp +import pickle +from PIL import Image, ImageDraw, ImageFont +import sys + + +class GestureDataCollector: + """手势数据收集器""" + + def __init__(self, data_dir="dataset"): + self.data_dir = data_dir + self.create_directories() + + # 初始化MediaPipe + self.mp_hands = mp.solutions.hands + self.mp_drawing = mp.solutions.drawing_utils + self.hands = self.mp_hands.Hands( + static_image_mode=False, + max_num_hands=1, + min_detection_confidence=0.5, + min_tracking_confidence=0.5 + ) + + # 定义手势类别 + self.gesture_classes = [ + "张开手掌", # open_palm + "握拳", # closed_fist + "胜利手势", # victory + "大拇指向上", # thumb_up + "大拇指向下", # thumb_down + "食指上指", # pointing_up + "食指向下", # pointing_down + "OK手势", # ok_sign + "无手" # no_hand + ] + + # 手势类别的英文标识(用于保存文件) + self.gesture_english_names = [ + "open_palm", + "closed_fist", + "victory", + "thumb_up", + "thumb_down", + "pointing_up", + "pointing_down", + "ok_sign", + "no_hand" + ] + + # 数据存储 + self.collected_data = [] + self.current_gesture = None + self.collection_active = False + + # 尝试加载中文字体 + self.font = None + try: + # Windows系统字体路径 + if sys.platform == 'win32': + font_path = 'C:/Windows/Fonts/simhei.ttf' # 黑体 + # Linux系统字体路径 + elif sys.platform == 'linux': + font_path = '/usr/share/fonts/truetype/wqy/wqy-microhei.ttc' # 文泉驿微米黑 + # macOS系统字体路径 + elif sys.platform == 'darwin': + font_path = '/System/Library/Fonts/PingFang.ttc' # 苹方字体 + else: + font_path = None + + if font_path and os.path.exists(font_path): + self.font = ImageFont.truetype(font_path, 30) + print(f"已加载中文字体: {font_path}") + else: + print("未找到中文字体,将使用默认字体") + self.font = ImageFont.load_default() + except Exception as e: + print(f"加载字体失败: {e}") + self.font = ImageFont.load_default() + + def create_directories(self): + """创建必要的目录""" + directories = [ + os.path.join(self.data_dir, "raw"), + os.path.join(self.data_dir, "processed"), + os.path.join(self.data_dir, "models"), + os.path.join(self.data_dir, "raw", "images"), + os.path.join(self.data_dir, "raw", "landmarks") + ] + + for directory in directories: + os.makedirs(directory, exist_ok=True) + + def extract_landmarks(self, image): + """从图像中提取手部关键点""" + image_rgb = cv2.cvtColor(image, cv2.COLOR_BGR2RGB) + results = self.hands.process(image_rgb) + + landmarks_data = [] + if results.multi_hand_landmarks: + for hand_landmarks in results.multi_hand_landmarks: + landmarks = [] + for lm in hand_landmarks.landmark: + landmarks.extend([lm.x, lm.y, lm.z]) + landmarks_data.append(landmarks) + + return landmarks_data, results.multi_hand_landmarks + + def collect_sample(self, gesture_class, image, landmarks): + """收集一个样本""" + timestamp = datetime.now().strftime("%Y%m%d_%H%M%S_%f") + + # 获取对应的英文手势名称用于保存文件 + if gesture_class in self.gesture_classes: + english_name = self.gesture_english_names[self.gesture_classes.index(gesture_class)] + else: + english_name = gesture_class + + # 保存图像 + img_filename = f"{english_name}_{timestamp}.jpg" + img_path = os.path.join(self.data_dir, "raw", "images", img_filename) + cv2.imwrite(img_path, image) + + # 保存关键点数据 + data_entry = { + 'timestamp': timestamp, + 'gesture_class': gesture_class, + 'gesture_english': english_name, + 'image_path': img_path, + 'landmarks': landmarks[0] if landmarks else [], # 取第一个手 + 'num_landmarks': 21 if landmarks else 0, + 'has_hand': len(landmarks) > 0 + } + + self.collected_data.append(data_entry) + return data_entry + + def start_collection(self, gesture_class): + """开始收集指定手势的数据""" + self.current_gesture = gesture_class + self.collection_active = True + print(f"开始收集手势: {gesture_class}") + print("按 'c' 收集样本,按 's' 停止") + + def save_dataset(self, filename="gesture_dataset.pkl"): + """保存收集的数据集""" + dataset_path = os.path.join(self.data_dir, "processed", filename) + + # 转换为特征和标签 + X = [] + y = [] + + for data in self.collected_data: + if data['has_hand'] and len(data['landmarks']) == 63: # 21个点 * 3坐标 + X.append(data['landmarks']) + y.append(self.gesture_classes.index(data['gesture_class'])) + + dataset = { + 'features': np.array(X), + 'labels': np.array(y), + 'gesture_classes': self.gesture_classes, + 'gesture_english_names': self.gesture_english_names, + 'num_samples': len(X) + } + + with open(dataset_path, 'wb') as f: + pickle.dump(dataset, f) + + print(f"数据集已保存到 {dataset_path}") + print(f"总样本数: {len(X)}") + print(f"各类样本数:") + for i, class_name in enumerate(self.gesture_classes): + count = np.sum(y == i) + print(f" {class_name}: {count}") + + return dataset + + def put_chinese_text(self, img, text, position, font_color=(255, 255, 255)): + """在图像上绘制中文文本""" + # 将OpenCV图像转换为PIL图像 + img_pil = Image.fromarray(cv2.cvtColor(img, cv2.COLOR_BGR2RGB)) + draw = ImageDraw.Draw(img_pil) + + # 绘制文本 + draw.text(position, text, font=self.font, fill=font_color) + + # 将PIL图像转换回OpenCV图像 + return cv2.cvtColor(np.array(img_pil), cv2.COLOR_RGB2BGR) + + def run_collection_app(self): + """运行数据收集应用程序""" + cap = cv2.VideoCapture(1) + collecting_for = None + sample_count = 0 + + print("手势数据收集工具") + print("=" * 50) + print("命令:") + print(" 1-9: 选择手势类别") + print(" c: 收集当前样本") + print(" s: 停止收集当前手势") + print(" w: 保存数据集") + print(" q: 退出") + print("=" * 50) + print("手势类别:") + for i, gesture in enumerate(self.gesture_classes): + print(f" {i + 1}: {gesture}") + print("=" * 50) + + while True: + ret, frame = cap.read() + if not ret: + print("无法读取摄像头画面") + break + + frame = cv2.flip(frame, 1) + + # 创建显示帧的副本用于绘制 + display_frame = frame.copy() + + # 提取关键点 + landmarks, hand_landmarks = self.extract_landmarks(frame) + + # 绘制手部关键点 + if hand_landmarks: + for hand_landmarks in hand_landmarks: + self.mp_drawing.draw_landmarks( + display_frame, hand_landmarks, self.mp_hands.HAND_CONNECTIONS) + + # 在图像上添加中文文本 + try: + # 第一行:当前手势 + if collecting_for: + status_text = f"当前手势: {collecting_for}" + else: + status_text = "当前手势: 无" + display_frame = self.put_chinese_text(display_frame, status_text, (10, 30), (0, 255, 0)) + + # 第二行:当前手势收集样本数 + display_frame = self.put_chinese_text(display_frame, f"当前收集样本数: {sample_count}", + (10, 70), (0, 255, 0)) + + # 第三行:总样本数 + display_frame = self.put_chinese_text(display_frame, f"总样本数: {len(self.collected_data)}", + (10, 110), (0, 255, 0)) + + # 第四行:操作提示 + display_frame = self.put_chinese_text(display_frame, "按1-9选择手势,c收集样本", (10, 150), + (255, 255, 255)) + + # 第五行:操作提示 + display_frame = self.put_chinese_text(display_frame, "按w保存,q退出", (10, 190), + (255, 255, 255)) + + except Exception as e: + print(f"绘制文字时出错: {e}") + # 如果无法绘制中文,使用英文 + status_text = f"Gesture: {collecting_for if collecting_for else 'None'}" + cv2.putText(display_frame, status_text, (10, 30), + cv2.FONT_HERSHEY_SIMPLEX, 1, (0, 255, 0), 2) + + cv2.imshow('手势数据收集', display_frame) + + key = cv2.waitKey(1) & 0xFF + + # 处理按键 + if key == ord('q'): + break + elif ord('1') <= key <= ord('9'): + gesture_idx = key - ord('1') + if gesture_idx < len(self.gesture_classes): + collecting_for = self.gesture_classes[gesture_idx] + sample_count = 0 + print(f"开始收集手势: {collecting_for}") + elif key == ord('c') and collecting_for: + if landmarks: + self.collect_sample(collecting_for, frame, landmarks) + sample_count += 1 + print(f"已收集 {collecting_for} 样本 #{sample_count}") + else: + print("未检测到手部!") + elif key == ord('s'): + collecting_for = None + sample_count = 0 + print("停止收集") + elif key == ord('w'): + if self.collected_data: + dataset = self.save_dataset() + print(f"数据集已保存,总样本数: {dataset['num_samples']}") + else: + print("没有数据可保存") + + cap.release() + cv2.destroyAllWindows() + + # 保存最终数据集 + if self.collected_data: + self.save_dataset() + + +if __name__ == "__main__": + collector = GestureDataCollector() + collector.run_collection_app() \ No newline at end of file diff --git a/src/MMAP-DRL-Nav/carla_environment.py b/src/MMAP-DRL-Nav/carla_environment.py index ab6cf28dfd..a61fec162d 100644 --- a/src/MMAP-DRL-Nav/carla_environment.py +++ b/src/MMAP-DRL-Nav/carla_environment.py @@ -25,7 +25,7 @@ def is_port_used(port): s.close() if not is_port_used(2000): - raise RuntimeError("❌ 2000端口未被占用,请先启动CARLA模拟器") + raise RuntimeError("2000端口未被占用,请先启动CARLA模拟器") max_retry = 3 retry_count = 0 @@ -35,10 +35,10 @@ def is_port_used(port): break except RuntimeError as e: retry_count += 1 - print(f"⚠️ 连接失败,重试第{retry_count}次...") + print(f"连接失败,重试第{retry_count}次...") time.sleep(5) else: - raise RuntimeError("❌ CARLA连接超时(3次重试失败),请检查模拟器是否正常启动") + raise RuntimeError("CARLA连接超时(3次重试失败),请检查模拟器是否启动") self.blueprint_library = self.world.get_blueprint_library() @@ -56,26 +56,45 @@ def is_port_used(port): self.npc_pedestrian_list = [] self.hit_vehicle = False self.hit_pedestrian = False + self.hit_static = False # 碰撞静态物体标记 + self.collision_penalty_applied = False # 碰撞惩罚是否已执行 + + # ========== 1. 红绿灯奖惩配置 ========== + self.red_light_penalty = -8.0 # 闯红灯一次性重罚 + self.green_light_reward = 10.0 # 绿灯通过高奖励 + self.red_light_stop_reward = 0.3 # 红灯停车奖励(高于前进奖励) + self.traffic_light_cooldown = 10.0 + self.last_traffic_light_time = 0 + self.traffic_light_trigger_distance = 10.0 + self.traffic_light_reset_distance = 15.0 + self.has_triggered_red = False + self.has_triggered_green = False + + # ========== 2. 超速奖惩配置 ========== + self.speed_limit_urban = 30.0 # 城区限速30km/h + self.over_speed_light_penalty = -1.0 # 轻度超速(30-40km/h)/秒 + self.over_speed_heavy_penalty = -4.0 # 重度超速(>40km/h)/秒 + self.over_speed_cooldown = 1.0 + self.last_over_speed_time = 0 - # ========== 红绿灯奖惩核心配置(长间隔防重复) ========== - self.red_light_penalty = -10.0 # 闯红灯扣分 - self.green_light_reward = 5.0 # 绿灯加分 - self.traffic_light_cooldown = 10.0 # 延长冷却到10秒(核心修改) - self.last_traffic_light_time = 0 # 上次奖惩时间 - self.traffic_light_trigger_distance = 10.0 # 触发距离 - self.traffic_light_reset_distance = 15.0 # 离开15米才重置标记 - self.has_triggered_red = False # 红灯触发标记 - self.has_triggered_green = False # 绿灯触发标记 - - # ========== 新增:超速检测配置 ========== - self.speed_limit_urban = 30.0 # 城区限速(km/h) - self.speed_limit_highway = 50.0 # 高速/郊区限速(km/h) - self.over_speed_light_penalty = -1.0 # 轻度超速(超10km/h内)扣分/帧 - self.over_speed_heavy_penalty = -3.0 # 重度超速(超10km/h以上)扣分/帧 - self.last_over_speed_time = 0 # 上次超速扣分时间(避免刷屏) - self.over_speed_cooldown = 1.0 # 超速扣分冷却(1秒/次) - - # 出生点碰撞检测配置 + # ========== 3. 车道偏离奖惩配置(核心调整) ========== + self.lane_keep_reward = 0.05 # 保持车道内奖励/帧 + self.lane_light_penalty = -0.2 # 轻微偏离单次扣分 + self.lane_heavy_penalty = -1.0 # 严重偏离单次扣分 + self.lane_offset_light = 0.2 # 轻微偏离阈值(米) + self.lane_offset_heavy = 0.4 # 严重偏离阈值(米) + self.lane_check_interval = 3.0 # 车道检测间隔:3秒 + self.lane_log_interval = 10.0 # 车道日志输出间隔:10秒 + self.last_lane_check_time = 0 # 上次车道检测时间戳 + self.last_lane_log_time = 0 # 上次车道日志输出时间戳 + self.enable_lane_log = True # 开启车道偏离日志 + + # ========== 4. 碰撞奖惩配置 ========== + self.collision_vehicle_penalty = -50.0 # 碰撞车辆惩罚 + self.collision_pedestrian_penalty = -100.0 # 碰撞行人惩罚 + self.collision_static_penalty = -15.0 # 碰撞静态物体惩罚 + + # 基础配置 self.spawn_retry_times = 20 self.spawn_safe_radius = 2.0 @@ -98,15 +117,9 @@ def is_port_used(port): self.view_distance = 8.0 self.z_offset = 0.5 - # ========== 核心修复:红绿灯判定(长间隔+防重复) ========== + # ========== 红绿灯判定(移除特殊符号) ========== def _check_traffic_light(self): - """ - 优化逻辑: - 1. 冷却时间延长到10秒,单次闯红灯仅扣1次分 - 2. 车辆离开15米范围才重置标记,低速行驶不重复判定 - """ current_time = time.time() - # 10秒冷却内不判定 if current_time - self.last_traffic_light_time < self.traffic_light_cooldown: return 0.0 @@ -116,50 +129,50 @@ def _check_traffic_light(self): vehicle_loc = self.vehicle.get_transform().location traffic_lights = self.world.get_actors().filter('traffic.traffic_light') reward = 0.0 - has_near_light = False # 是否有近距离灯 + has_near_light = False + + # 获取车辆速度(判断是否停车) + velocity = self.vehicle.get_velocity() + speed_m_s = np.sqrt(velocity.x**2 + velocity.y**2 + velocity.z**2) + is_stopped = speed_m_s < 0.1 for light in traffic_lights: dist = vehicle_loc.distance(light.get_transform().location) - # 标记:有10米内的灯 if dist <= self.traffic_light_trigger_distance: has_near_light = True light_state = light.state - # 红灯:仅首次触发扣分 - if light_state == carla.TrafficLightState.Red and not self.has_triggered_red: - reward = self.red_light_penalty - print(f"⚠️ 闯红灯!扣分{self.red_light_penalty}") - self.has_triggered_red = True - self.has_triggered_green = False - self.last_traffic_light_time = current_time + + # 红灯逻辑:停车奖励 / 闯灯惩罚 + if light_state == carla.TrafficLightState.Red: + if not is_stopped and not self.has_triggered_red: + reward = self.red_light_penalty + print(f"闯红灯!扣分{self.red_light_penalty}") + self.has_triggered_red = True + self.last_traffic_light_time = current_time + elif is_stopped: + reward = self.red_light_stop_reward break - # 绿灯:仅首次触发加分 + + # 绿灯逻辑:通过奖励 elif light_state == carla.TrafficLightState.Green and not self.has_triggered_green: reward = self.green_light_reward - print(f"✅ 绿灯合规通过!加分{self.green_light_reward}") + print(f"绿灯合规通过!加分{self.green_light_reward}") self.has_triggered_green = True - self.has_triggered_red = False self.last_traffic_light_time = current_time break - # 离开15米范围,才重置触发标记 + elif dist > self.traffic_light_reset_distance: self.has_triggered_red = False self.has_triggered_green = False - # 无近距离灯时,也重置标记(避免卡标记) if not has_near_light: self.has_triggered_red = False self.has_triggered_green = False return reward - # ========== 新增:超速检测与惩罚 ========== + # ========== 超速检测(无日志) ========== def _check_over_speed(self): - """ - 超速检测逻辑: - 1. 将车辆速度从m/s转换为km/h(1 m/s = 3.6 km/h) - 2. 区分城区/高速限速(默认按城区30km/h) - 3. 轻度/重度超速分级惩罚,1秒冷却避免刷屏 - """ current_time = time.time() if current_time - self.last_over_speed_time < self.over_speed_cooldown: return 0.0 @@ -167,30 +180,81 @@ def _check_over_speed(self): if not self.vehicle or not self.vehicle.is_alive: return 0.0 - # 获取车辆速度(m/s)并转换为km/h + # 速度计算(m/s → km/h) velocity = self.vehicle.get_velocity() speed_m_s = np.sqrt(velocity.x**2 + velocity.y**2 + velocity.z**2) speed_km_h = speed_m_s * 3.6 - # 简化:默认按城区限速30km/h(可扩展为按车道/地图区分) - speed_limit = self.speed_limit_urban - over_speed = speed_km_h - speed_limit - + # 超速判定 + over_speed = speed_km_h - self.speed_limit_urban reward = 0.0 if over_speed > 0: - # 轻度超速(0~10km/h) if 0 < over_speed <= 10: reward = self.over_speed_light_penalty - print(f"⚠️ 轻度超速!当前{speed_km_h:.1f}km/h,限速{speed_limit}km/h,扣分{self.over_speed_light_penalty}") - # 重度超速(>10km/h) else: reward = self.over_speed_heavy_penalty - print(f"❌ 重度超速!当前{speed_km_h:.1f}km/h,限速{speed_limit}km/h,扣分{self.over_speed_heavy_penalty}") self.last_over_speed_time = current_time return reward - # ========== 安全生成车辆(保留) ========== + # ========== 车道偏离检测(3秒检测 + 10秒日志 + 无特殊符号) ========== + def _check_lane_offset(self): + current_time = time.time() + # 仅每3秒执行一次检测 + if current_time - self.last_lane_check_time < self.lane_check_interval: + return 0.0 + + # 更新检测时间戳 + self.last_lane_check_time = current_time + + if not self.vehicle or not self.vehicle.is_alive: + return 0.0 + + # 获取当前车道waypoint和车辆位置 + vehicle_transform = self.vehicle.get_transform() + waypoint = self.world.get_map().get_waypoint(vehicle_transform.location, project_to_road=True) + + # 完全偏离道路(无可用waypoint) + if not waypoint: + # 仅每10秒输出一次日志 + if self.enable_lane_log and current_time - self.last_lane_log_time >= self.lane_log_interval: + print(f"完全偏离道路!扣分{self.lane_heavy_penalty}") + self.last_lane_log_time = current_time + return self.lane_heavy_penalty + + # 计算车辆与车道中心线的偏移量(投影到车道垂直方向) + lane_center = waypoint.transform.location + vehicle_loc = vehicle_transform.location + yaw_rad = np.radians(waypoint.transform.rotation.yaw) + + # 偏移量计算:消除车道方向的影响,仅保留垂直偏移 + offset_x = vehicle_loc.x - lane_center.x + offset_y = vehicle_loc.y - lane_center.y + offset = np.abs(offset_x * np.sin(yaw_rad) - offset_y * np.cos(yaw_rad)) + + # 按偏移量分级判定(3秒检测一次,10秒日志一次) + reward = 0.0 + if offset < self.lane_offset_light: + # 保持车道内:奖励(无日志) + reward = self.lane_keep_reward + elif offset < self.lane_offset_heavy: + # 轻微偏离:单次扣0.2分 + reward = self.lane_light_penalty + # 仅每10秒输出一次日志 + if self.enable_lane_log and current_time - self.last_lane_log_time >= self.lane_log_interval: + print(f"轻微偏离车道(偏移{offset:.2f}m)!扣分{self.lane_light_penalty}") + self.last_lane_log_time = current_time + else: + # 严重偏离:单次扣1.0分 + reward = self.lane_heavy_penalty + # 仅每10秒输出一次日志 + if self.enable_lane_log and current_time - self.last_lane_log_time >= self.lane_log_interval: + print(f"严重偏离车道(偏移{offset:.2f}m)!扣分{self.lane_heavy_penalty}") + self.last_lane_log_time = current_time + + return reward + + # ========== 安全生成车辆(移除特殊符号) ========== def _spawn_vehicle_safely(self, vehicle_bp): spawn_points = self.world.get_map().get_spawn_points() if not spawn_points: @@ -206,15 +270,15 @@ def _spawn_vehicle_safely(self, vehicle_bp): try: vehicle = self.world.try_spawn_actor(vehicle_bp, spawn_point) if vehicle is not None: - print(f"✅ 车辆生成成功(重试{attempt}次)") + print(f"车辆生成成功(重试{attempt}次)") return vehicle except RuntimeError as e: - print(f"⚠️ 出生点碰撞,重试第{attempt+1}次...") + print(f"出生点碰撞,重试第{attempt+1}次...") continue - raise RuntimeError("❌ 所有出生点都有碰撞,无法生成车辆!") + raise RuntimeError("所有出生点都有碰撞,无法生成车辆!") - # ========== 优化NPC生成(保留) ========== + # ========== 优化NPC生成 ========== def _spawn_small_npc(self): for v in self.npc_vehicle_list: if v.is_alive: @@ -273,7 +337,7 @@ def _spawn_small_npc(self): except: continue - # ========== 初始化碰撞传感器(保留) ========== + # ========== 初始化碰撞传感器 ========== def _init_collision_sensor(self): collision_bp = self.blueprint_library.find('sensor.other.collision') collision_transform = carla.Transform(carla.Location(x=0, y=0, z=0)) @@ -282,7 +346,19 @@ def _init_collision_sensor(self): ) self.collision_sensor.listen(lambda event: self._collision_callback(event)) - # ========== 初始化相机(保留) ========== + # ========== 碰撞回调(仅标记状态) ========== + def _collision_callback(self, event): + self.has_collision = True + other_actor = event.other_actor + other_actor_type = other_actor.type_id + if 'vehicle' in other_actor_type: + self.hit_vehicle = True + elif 'walker' in other_actor_type: + self.hit_pedestrian = True + elif 'static' in other_actor_type or 'building' in other_actor_type or 'guardrail' in other_actor_type: + self.hit_static = True + + # ========== 初始化相机 ========== def _init_camera(self): camera_bp = self.blueprint_library.find('sensor.camera.rgb') camera_bp.set_attribute('image_size_x', '128') @@ -293,7 +369,7 @@ def _init_camera(self): self.camera = self.world.spawn_actor(camera_bp, camera_transform, attach_to=self.vehicle) self.camera.listen(lambda img: self._camera_callback(img)) - # ========== 重置环境(保留+重置超速标记) ========== + # ========== 重置环境(重置所有标记) ========== def reset(self): # 清理旧资源 if self.vehicle is not None and self.vehicle.is_alive: @@ -303,13 +379,15 @@ def reset(self): if self.collision_sensor is not None and self.collision_sensor.is_alive: self.collision_sensor.destroy() self.image_data = None + + # 重置所有碰撞标记 self.has_collision = False - - # 重置碰撞标记 self.hit_vehicle = False self.hit_pedestrian = False + self.hit_static = False + self.collision_penalty_applied = False - # 重置红绿灯触发标记 + # 重置红绿灯标记 self.last_traffic_light_time = 0 self.has_triggered_red = False self.has_triggered_green = False @@ -317,6 +395,10 @@ def reset(self): # 重置超速标记 self.last_over_speed_time = 0 + # 重置车道检测/日志标记 + self.last_lane_check_time = 0 + self.last_lane_log_time = 0 + # 生成车辆 vehicle_bp = self.blueprint_library.filter('vehicle.tesla.model3')[0] self.vehicle = self._spawn_vehicle_safely(vehicle_bp) @@ -340,21 +422,7 @@ def reset(self): self.world.tick() return self.image_data.copy() if self.image_data is not None else np.zeros((128,128,3), dtype=np.uint8) - # ========== 碰撞回调(保留) ========== - def _collision_callback(self, event): - self.has_collision = True - other_actor_type = event.other_actor.type_id - if 'vehicle' in other_actor_type: - self.hit_vehicle = True - elif 'walker' in other_actor_type: - self.hit_pedestrian = True - - # ========== 相机回调(保留) ========== - def _camera_callback(self, image): - array = np.frombuffer(image.raw_data, dtype=np.uint8) - self.image_data = array.reshape((image.height, image.width, 4))[:, :, :3] - - # ========== 视角跟随(保留) ========== + # ========== 视角跟随 ========== def follow_vehicle(self): spectator = self.world.get_spectator() if not spectator or not self.vehicle or not self.vehicle.is_alive: @@ -373,11 +441,16 @@ def follow_vehicle(self): carla.Rotation(pitch=self.view_pitch, yaw=vehicle_tf.rotation.yaw, roll=0.0) )) - # ========== 获取观测(保留) ========== + # ========== 获取观测 ========== def get_observation(self): return self.image_data.copy() if self.image_data is not None else np.zeros((128, 128, 3), dtype=np.uint8) - # ========== 执行单步动作(修改:加入超速奖惩) ========== + # ========== 相机回调 ========== + def _camera_callback(self, image): + array = np.frombuffer(image.raw_data, dtype=np.uint8) + self.image_data = array.reshape((image.height, image.width, 4))[:, :, :3] + + # ========== 核心:奖励函数(碰撞仅扣一次分 + 移除特殊符号) ========== def step(self, action): if self.vehicle is None or not self.vehicle.is_alive: raise RuntimeError("车辆未初始化/已销毁,请先调用reset()") @@ -408,23 +481,60 @@ def step(self, action): self.world.tick() self.follow_vehicle() - # 计算总奖励(基础+红绿灯+超速) + # ========== 1. 基础行驶奖励 ========== base_reward = 0.1 if throttle > 0 else (-0.1 if throttle < 0 else 0.0) + + # ========== 2. 红绿灯奖惩 ========== traffic_light_reward = self._check_traffic_light() - over_speed_reward = self._check_over_speed() # 新增:超速奖惩 - total_reward = base_reward + traffic_light_reward + over_speed_reward + + # ========== 3. 超速奖惩 ========== + over_speed_reward = self._check_over_speed() + + # ========== 4. 车道偏离奖惩(3秒检测 + 10秒日志) ========== + lane_reward = self._check_lane_offset() + + # ========== 5. 碰撞奖惩(仅扣一次分 + 移除特殊符号) ========== + collision_reward = 0.0 + done = False + if self.has_collision and not self.collision_penalty_applied: + # 仅当碰撞发生且未执行过惩罚时,才扣分 + if self.hit_pedestrian: + collision_reward = self.collision_pedestrian_penalty + print(f"碰撞行人!扣分{self.collision_pedestrian_penalty},终止训练") + done = True + elif self.hit_vehicle: + collision_reward = self.collision_vehicle_penalty + print(f"碰撞车辆!扣分{self.collision_vehicle_penalty},终止训练") + done = True + elif self.hit_static: + collision_reward = self.collision_static_penalty + print(f"碰撞静态物体!扣分{self.collision_static_penalty}") + done = False # 碰撞静态物体不终止训练 + + # 标记惩罚已执行,避免重复扣分 + self.collision_penalty_applied = True + + # ========== 总奖励计算 ========== + total_reward = ( + base_reward # 基础行驶 + + traffic_light_reward # 红绿灯 + + over_speed_reward # 超速 + + lane_reward # 车道偏离 + + collision_reward # 碰撞(仅一次) + ) next_state = self.get_observation() - done = self.hit_vehicle return next_state, total_reward, done, { "base_reward": base_reward, "traffic_light_reward": traffic_light_reward, - "over_speed_reward": over_speed_reward, # 新增:返回超速奖励 + "over_speed_reward": over_speed_reward, + "lane_reward": lane_reward, + "collision_reward": collision_reward, "total_reward": total_reward } - # ========== 关闭环境(保留) ========== + # ========== 关闭环境(移除特殊符号) ========== def close(self): # 清理NPC for v in self.npc_vehicle_list: @@ -442,26 +552,26 @@ def close(self): self.sync_settings.synchronous_mode = False self.world.apply_settings(self.sync_settings) except Exception as e: - print(f"⚠️ 恢复异步模式时警告:{e}") + print(f"恢复异步模式时警告:{e}") # 销毁核心对象 try: if self.vehicle is not None and self.vehicle.is_alive: self.vehicle.destroy() except Exception as e: - print(f"⚠️ 销毁车辆时警告:{e}") + print(f"销毁车辆时警告:{e}") try: if self.camera is not None and self.camera.is_alive: self.camera.destroy() except Exception as e: - print(f"⚠️ 销毁相机时警告:{e}") + print(f"销毁相机时警告:{e}") try: if self.collision_sensor is not None and self.collision_sensor.is_alive: self.collision_sensor.destroy() except Exception as e: - print(f"⚠️ 销毁碰撞传感器时警告:{e}") + print(f"销毁碰撞传感器时警告:{e}") time.sleep(0.5) - print("✅ CARLA环境已关闭(同步模式已恢复为异步)") \ No newline at end of file + print("CARLA环境已关闭(同步模式已恢复为异步)") \ No newline at end of file diff --git a/src/Neuro_Mujoco/main.py b/src/Neuro_Mujoco/main.py index 1c240566dd..1aa90433a3 100644 --- a/src/Neuro_Mujoco/main.py +++ b/src/Neuro_Mujoco/main.py @@ -1,9 +1,3 @@ -#! /usr/bin/env python -# -*- coding: utf-8 -*- -""" -MuJoCo功能整合工具(支持强化学习策略控制 + ROS 1通信) -优化点:新增基于PyTorch的策略网络推理,自动生成控制指令 -""" import os import sys import time @@ -189,29 +183,8 @@ def visualize(model_path: str, use_ros: bool = False, policy_path: Optional[str] :param model_path: 模型文件/目录路径 :param use_ros: 是否启用ROS模式 :param policy_path: 预训练策略模型路径(.pth) - 可视化模型并运行模拟(支持ROS 1模式) - - 参数: - model_path: 模型文件/目录路径 - use_ros: 是否启用ROS模式(默认False) - """ - # 新增:模型路径智能校验(支持目录自动找XML/MJB文件) - if os.path.isdir(model_path): - # 遍历目录找第一个XML/MJB文件 - model_files = [] - for file in os.listdir(model_path): - if file.endswith(('.xml', '.mjb')): - model_files.append(os.path.join(model_path, file)) - if not model_files: - logger.error(f"目录 {model_path} 中未找到.xml/.mjb模型文件") - return - model_path = model_files[0] - logger.info(f"自动选择目录中的模型文件: {model_path}") - - model_path: 模型文件路径 - use_ros: 是否启用ROS模式(默认False) """ - # 智能校验模型路径(支持目录自动找XML/MJB) + # 模型路径智能校验(支持目录自动找XML/MJB文件) if os.path.isdir(model_path): model_files = [] for file in os.listdir(model_path): @@ -332,7 +305,7 @@ def ctrl_callback(msg: Float32MultiArray): # 映射到实际控制范围 if ctrl_range is not None: - action = ctrl_range[:, 0] + (ctrl_range[:, 1] - ctrl_range[:, 0]) * (action + 1) / 2 + action = ctrl_range[:, 0] + (ctrl_range[:, 1] - ctrl_range[:, 0]) * (action + 1) / 2 # 核心映射:[-1,1]→[ctrl_min,ctrl_max] 线性缩放,保证指令符合执行器物理极限 data.ctrl[:] = action @@ -388,8 +361,6 @@ def ctrl_callback(msg: Float32MultiArray): logger.error(f"可视化过程出错: {str(e)}", exc_info=True) # ===================== 主函数(命令行入口) ===================== -# ===================== 主函数(仅优化model参数help+保持其他逻辑不变)===================== -# ===================== 主函数(完全保持原有逻辑不变)===================== def main() -> None: parser = argparse.ArgumentParser( description="MuJoCo功能整合工具(支持强化学习策略控制 + ROS 1通信)", @@ -397,20 +368,14 @@ def main() -> None: ) subparsers = parser.add_subparsers(dest="command", required=True) - # 1. 可视化命令 - viz_parser = subparsers.add_parser("visualize", help="可视化模型并运行模拟") - viz_parser.add_argument("model", help="模型文件路径/目录(支持.xml/.mjb)") - viz_parser.add_argument("--ros", action="store_true", help="启用ROS 1模式") - viz_parser.add_argument("--policy", help="预训练策略模型路径(.pth文件)") - # 1. 可视化命令(优化model参数help为通用提示,移除硬编码路径) + # 1. 可视化命令(修复重复添加model参数问题) viz_parser = subparsers.add_parser("visualize", help="可视化模型并运行模拟") - viz_parser.add_argument("model", help="模型文件路径或包含模型的目录(支持.xml/.mjb格式)") - viz_parser.add_argument("model", help="/home/lan/桌面/nn/mujoco_menagerie/anybotics_anymal_b") viz_parser.add_argument( - "--ros", - action="store_true", - help="启用ROS模式(发布关节状态/基座姿态,订阅控制指令)" + "model", + help="模型文件路径/目录(示例:/home/lan/桌面/nn/mujoco_menagerie/anybotics_anymal_b)" ) + viz_parser.add_argument("--ros", action="store_true", help="启用ROS 1模式(发布/订阅关节控制话题)") + viz_parser.add_argument("--policy", help="预训练策略模型路径(.pth文件)") # 2. 速度测试命令 speed_parser = subparsers.add_parser("testspeed", help="测试模型模拟速度(多线程)") diff --git a/src/Unmanned_vehicle_AD_DQN/Model.py b/src/Unmanned_vehicle_AD_DQN/Model.py index c419ae6c35..1f563dd392 100644 --- a/src/Unmanned_vehicle_AD_DQN/Model.py +++ b/src/Unmanned_vehicle_AD_DQN/Model.py @@ -19,9 +19,7 @@ from threading import Thread from Environment import * from Hyperparameters import * -import pickle -import json -from datetime import datetime +from TrainingStrategies import * # 自定义TensorBoard类 @@ -56,500 +54,6 @@ def update_stats(self, **stats): self.writer.flush() -# 优先经验回放缓冲区 -class PrioritizedReplayBuffer: - def __init__(self, max_size=REPLAY_MEMORY_SIZE, alpha=0.6, beta_start=0.4, beta_frames=100000): - self.max_size = max_size - self.alpha = alpha # 优先级程度 (0 = 均匀采样, 1 = 完全优先级) - self.beta_start = beta_start # 重要性采样权重起始值 - self.beta_frames = beta_frames # beta线性增长的帧数 - self.frame = 1 - - # 使用循环缓冲区 - self.buffer = deque(maxlen=max_size) - self.priorities = deque(maxlen=max_size) - - def __len__(self): - return len(self.buffer) - - def beta(self): - """线性递增的beta值,用于重要性采样权重""" - return min(1.0, self.beta_start + self.frame * (1.0 - self.beta_start) / self.beta_frames) - - def add(self, experience, error=None): - """添加经验到缓冲区""" - if error is None: - priority = max(self.priorities) if self.priorities else 1.0 - else: - priority = (abs(error) + 1e-5) ** self.alpha - - self.buffer.append(experience) - self.priorities.append(priority) - - def sample(self, batch_size): - """从缓冲区中采样一批经验""" - if len(self.buffer) == 0: - return [], [], [], [] - - # 计算采样概率 - priorities = np.array(self.priorities, dtype=np.float32) - probs = priorities ** self.alpha - probs /= probs.sum() - - # 采样索引 - indices = np.random.choice(len(self.buffer), batch_size, p=probs) - - # 获取样本 - samples = [self.buffer[i] for i in indices] - - # 计算重要性采样权重 - total = len(self.buffer) - weights = (total * probs[indices]) ** (-self.beta()) - weights /= weights.max() # 归一化 - - # 更新帧计数器 - self.frame += 1 - - return indices, samples, weights - - def update_priorities(self, indices, errors): - """更新采样经验的优先级""" - for idx, error in zip(indices, errors): - if 0 <= idx < len(self.priorities): - self.priorities[idx] = (abs(error) + 1e-5) ** self.alpha - - -# 课程学习管理器 -class CurriculumManager: - def __init__(self, env): - self.env = env - self.current_stage = 0 - self.stage_thresholds = [0.3, 0.5, 0.7, 0.85] # 成功率阈值 - self.stage_configs = [ - # 阶段0: 入门 - { - 'pedestrian_cross': 4, # 十字路口行人数量 - 'pedestrian_normal': 2, # 普通路段行人数量 - 'pedestrian_speed_min': 0.5, # 行人最低速度 - 'pedestrian_speed_max': 1.0, # 行人最高速度 - 'max_episode_steps': 1200, # 最大步数 (20秒 * 60FPS) - 'success_threshold': 0.3 # 进入下一阶段成功率 - }, - # 阶段1: 初级 - { - 'pedestrian_cross': 6, - 'pedestrian_normal': 3, - 'pedestrian_speed_min': 0.7, - 'pedestrian_speed_max': 1.3, - 'max_episode_steps': 1800, # 30秒 - 'success_threshold': 0.5 - }, - # 阶段2: 中级 - { - 'pedestrian_cross': 8, - 'pedestrian_normal': 4, - 'pedestrian_speed_min': 0.8, - 'pedestrian_speed_max': 1.5, - 'max_episode_steps': 2400, # 40秒 - 'success_threshold': 0.7 - }, - # 阶段3: 高级 (正常难度) - { - 'pedestrian_cross': 10, - 'pedestrian_normal': 5, - 'pedestrian_speed_min': 1.0, - 'pedestrian_speed_max': 2.0, - 'max_episode_steps': 3600, # 60秒 - 'success_threshold': 0.85 - }, - # 阶段4: 专家 (挑战) - { - 'pedestrian_cross': 12, - 'pedestrian_normal': 6, - 'pedestrian_speed_min': 1.2, - 'pedestrian_speed_max': 2.5, - 'max_episode_steps': 3600, - 'success_threshold': 0.9 - } - ] - - # 训练历史 - self.success_history = deque(maxlen=20) # 记录最近20轮的成功情况 - self.reward_history = deque(maxlen=50) # 记录最近50轮的奖励 - - def update_stage(self, success, reward): - """更新训练阶段""" - # 记录历史 - self.success_history.append(1 if success else 0) - self.reward_history.append(reward) - - # 计算最近成功率 - if len(self.success_history) >= 10: - success_rate = sum(self.success_history) / len(self.success_history) - avg_reward = np.mean(self.reward_history) if self.reward_history else 0 - - print(f"课程学习 - 当前阶段: {self.current_stage}, 成功率: {success_rate:.2f}, 平均奖励: {avg_reward:.2f}") - - # 检查是否可以进入下一阶段 - if self.current_stage < len(self.stage_configs) - 1: - next_stage_threshold = self.stage_configs[self.current_stage]['success_threshold'] - if success_rate >= next_stage_threshold and avg_reward > 5: - self.current_stage += 1 - print(f"🎉 课程学习: 进阶到阶段 {self.current_stage}!") - return True - - # 如果表现太差,退回上一阶段 - if self.current_stage > 0 and success_rate < 0.2: - self.current_stage -= 1 - print(f"⚠️ 课程学习: 退回阶段 {self.current_stage}") - return True - - return False - - def get_current_config(self): - """获取当前阶段的配置""" - return self.stage_configs[min(self.current_stage, len(self.stage_configs) - 1)] - - def apply_to_environment(self): - """将当前阶段配置应用到环境""" - config = self.get_current_config() - # 注意:这里需要修改Environment.py中的行人生成逻辑来支持这些参数 - # 暂时返回配置,由外部调用者处理 - return config - - -# 多目标优化器 -class MultiObjectiveOptimizer: - def __init__(self): - # 定义优化目标及其权重(可动态调整) - self.objectives = { - 'safety': { - 'weight': 0.4, - 'description': '安全避障和避免碰撞', - 'metrics': ['collision_avoidance', 'pedestrian_distance'] - }, - 'efficiency': { - 'weight': 0.25, - 'description': '快速到达目的地', - 'metrics': ['progress_speed', 'total_time'] - }, - 'comfort': { - 'weight': 0.2, - 'description': '平稳驾驶体验', - 'metrics': ['smoothness', 'steering_changes'] - }, - 'rule_following': { - 'weight': 0.15, - 'description': '遵守交通规则', - 'metrics': ['lane_keeping', 'speed_limit'] - } - } - - # 指标跟踪 - self.metrics_history = { - 'safety': [], - 'efficiency': [], - 'comfort': [], - 'rule_following': [] - } - - def compute_composite_reward(self, metrics): - """计算综合奖励值""" - composite = 0 - - for obj_name, obj_info in self.objectives.items(): - if obj_name in metrics: - # 归一化处理每个目标的贡献 - normalized_value = self._normalize_metric(metrics[obj_name], obj_name) - composite += normalized_value * obj_info['weight'] - - # 记录指标历史 - self.metrics_history[obj_name].append(normalized_value) - - # 特殊惩罚项 - if metrics.get('collision', False): - composite -= 10 - if metrics.get('off_road', False): - composite -= 5 - if metrics.get('dangerous_action', False): - composite -= 3 - - return composite - - def _normalize_metric(self, value, metric_name): - """归一化指标值到[0, 1]范围""" - # 不同指标的归一化方式不同 - normalization_rules = { - 'safety': lambda x: min(max(x / 10, 0), 1), # 假设安全分满分10 - 'efficiency': lambda x: min(max(x / 100, 0), 1), # 效率分满分100 - 'comfort': lambda x: min(max((x + 5) / 10, 0), 1), # 舒适度[-5, 5] -> [0, 1] - 'rule_following': lambda x: min(max(x, 0), 1) # 规则遵循度[0, 1] - } - - if metric_name in normalization_rules: - return normalization_rules[metric_name](value) - return min(max(value, 0), 1) # 默认截断到[0, 1] - - def adjust_weights(self, performance_feedback): - """根据性能反馈动态调整权重""" - # 如果某个目标表现持续较差,增加其权重 - recent_performance = {} - for obj in self.objectives: - if len(self.metrics_history[obj]) >= 10: - recent_avg = np.mean(self.metrics_history[obj][-10:]) - recent_performance[obj] = recent_avg - - if recent_performance: - # 找到表现最差的目标 - worst_obj = min(recent_performance, key=recent_performance.get) - best_obj = max(recent_performance, key=recent_performance.get) - - # 如果最差目标表现低于阈值,增加其权重 - if recent_performance[worst_obj] < 0.3: - adjustment = 0.05 - self.objectives[worst_obj]['weight'] += adjustment - self.objectives[best_obj]['weight'] -= adjustment - - # 确保权重总和为1 - total = sum(obj['weight'] for obj in self.objectives.values()) - for obj in self.objectives: - self.objectives[obj]['weight'] /= total - - print(f"动态权重调整: {worst_obj}权重↑ {adjustment:.3f}, {best_obj}权重↓ {adjustment:.3f}") - - def get_performance_report(self): - """生成性能报告""" - report = "多目标优化性能报告:\n" - report += "=" * 50 + "\n" - - for obj_name, obj_info in self.objectives.items(): - history = self.metrics_history[obj_name] - if history: - avg = np.mean(history[-20:]) if len(history) >= 20 else np.mean(history) - report += f"{obj_name}(权重:{obj_info['weight']:.2f}): 平均得分={avg:.3f}\n" - report += f" 描述: {obj_info['description']}\n" - - return report - - -# 模仿学习管理器 -class ImitationLearningManager: - def __init__(self, expert_data_path=None): - self.expert_data_path = expert_data_path - self.expert_data = [] - self.is_pretrained = False - - def load_expert_data(self, path): - """加载专家示范数据""" - try: - if os.path.exists(path): - with open(path, 'rb') as f: - self.expert_data = pickle.load(f) - print(f"已加载 {len(self.expert_data)} 条专家示范数据") - return True - else: - print(f"专家数据文件不存在: {path}") - return False - except Exception as e: - print(f"加载专家数据失败: {e}") - return False - - def collect_expert_demonstration(self, env, num_episodes=10): - """收集专家示范数据(可以手动控制或使用规则控制器)""" - print(f"开始收集专家示范数据 ({num_episodes}个episodes)...") - - demonstrations = [] - - for episode in range(num_episodes): - print(f"收集专家示范 Episode {episode + 1}/{num_episodes}") - - state = env.reset(episode) - done = False - episode_data = [] - - while not done: - # 这里可以使用规则控制器或手动控制 - # 示例:简单的规则控制器 - action = self._rule_based_controller(env) - - new_state, reward, done, _ = env.step(action) - - # 保存示范数据 - episode_data.append({ - 'state': state.copy(), - 'action': action, - 'reward': reward, - 'next_state': new_state.copy(), - 'done': done - }) - - state = new_state - - demonstrations.extend(episode_data) - env.cleanup_actors() - - # 保存专家数据 - self.expert_data = demonstrations - timestamp = datetime.now().strftime("%Y%m%d_%H%M%S") - save_path = f"expert_data_{timestamp}.pkl" - - with open(save_path, 'wb') as f: - pickle.dump(demonstrations, f) - - print(f"专家示范数据已保存到: {save_path}, 共 {len(demonstrations)} 条记录") - return True - - def _rule_based_controller(self, env): - """基于规则的控制器(作为专家示范)""" - # 获取车辆状态 - vehicle_location = env.vehicle.get_location() - velocity = env.vehicle.get_velocity() - speed_kmh = 3.6 * math.sqrt(velocity.x**2 + velocity.y**2) - - # 简单规则:保持速度在20-40 km/h,避免障碍物 - if speed_kmh < 20: - return 2 # 加速 - elif speed_kmh > 40: - return 0 # 减速 - else: - # 检查前方障碍物 - has_obstacle_ahead = self._check_obstacle_ahead(env) - if has_obstacle_ahead: - return 0 # 减速 - else: - return 1 # 保持 - - return 1 # 默认保持 - - def _check_obstacle_ahead(self, env): - """检查前方是否有障碍物(简化版本)""" - # 这里可以添加更复杂的障碍物检测逻辑 - # 暂时返回False - return False - - def pretrain_with_behavioral_cloning(self, model, epochs=20): - """使用行为克隆进行预训练""" - if not self.expert_data: - print("没有专家数据可用,跳过预训练") - return model - - print(f"开始行为克隆预训练 ({epochs}个epochs)...") - - # 准备训练数据 - states = [] - actions = [] - - for demo in self.expert_data: - states.append(demo['state']) - actions.append(demo['action']) - - # 将状态归一化 - states = np.array(states) / 255.0 - - # 将动作转换为one-hot编码 - actions_onehot = tf.keras.utils.to_categorical(actions, num_classes=5) - - # 备份原始编译设置 - original_loss = model.loss - original_optimizer = model.optimizer - original_metrics = model.metrics_names - - # 重新编译模型用于分类任务 - model.compile( - optimizer=Adam(learning_rate=0.0001), - loss='categorical_crossentropy', - metrics=['accuracy'] - ) - - # 训练模型模仿专家行为 - history = model.fit( - states, actions_onehot, - batch_size=32, - epochs=epochs, - validation_split=0.2, - verbose=1 - ) - - print(f"预训练完成 - 最终准确率: {history.history['accuracy'][-1]:.3f}") - - # 恢复原始编译设置 - model.compile( - optimizer=original_optimizer, - loss=original_loss, - metrics=original_metrics - ) - - self.is_pretrained = True - return model - - def train_with_dagger(self, model, env, iterations=5, episodes_per_iter=5): - """使用DAgger算法进行训练""" - print(f"开始DAgger训练 ({iterations}次迭代,每次{episodes_per_iter}个episodes)...") - - aggregated_data = self.expert_data.copy() - - for iteration in range(iterations): - print(f"\nDAgger 迭代 {iteration + 1}/{iterations}") - - # 使用当前策略收集数据 - new_demos = [] - - for episode in range(episodes_per_iter): - print(f" 收集数据 Episode {episode + 1}/{episodes_per_iter}") - - state = env.reset(episode) - done = False - - while not done: - # 使用当前策略选择动作 - qs = model.predict(np.array(state).reshape(-1, *state.shape) / 255)[0] - action = np.argmax(qs) - - # 执行动作 - new_state, reward, done, _ = env.step(action) - - # 专家纠正(这里可以添加专家纠正逻辑) - # 如果策略动作与专家建议不同,使用专家动作 - expert_action = self._rule_based_controller(env) - - # 保存数据(使用专家纠正后的动作) - new_demos.append({ - 'state': state.copy(), - 'action': expert_action, # 使用专家动作 - 'reward': reward, - 'next_state': new_state.copy(), - 'done': done - }) - - state = new_state - - env.cleanup_actors() - - # 合并数据 - aggregated_data.extend(new_demos) - - # 在合并数据上重新训练 - states = [d['state'] for d in aggregated_data] - actions = [d['action'] for d in aggregated_data] - - states = np.array(states) / 255.0 - actions_onehot = tf.keras.utils.to_categorical(actions, num_classes=5) - - # 训练模型 - history = model.fit( - states, actions_onehot, - batch_size=32, - epochs=10, - validation_split=0.1, - verbose=0 - ) - - print(f" 训练完成 - 准确率: {history.history['accuracy'][-1]:.3f}") - - print("DAgger训练完成!") - return model - - # DQN智能体类 - 升级版(整合训练策略) class DQNAgent: def __init__(self, use_dueling=True, use_per=True, use_curriculum=True, use_multi_objective=True): diff --git a/src/Unmanned_vehicle_AD_DQN/Test.py b/src/Unmanned_vehicle_AD_DQN/Test.py index d284b8245e..ccd53b468c 100644 --- a/src/Unmanned_vehicle_AD_DQN/Test.py +++ b/src/Unmanned_vehicle_AD_DQN/Test.py @@ -9,6 +9,7 @@ from tensorflow.keras.models import load_model from Environment import CarEnv, MEMORY_FRACTION from Hyperparameters import * +from TrainingStrategies import * import os import json import glob diff --git a/src/Unmanned_vehicle_AD_DQN/TrainingStrategies.py b/src/Unmanned_vehicle_AD_DQN/TrainingStrategies.py new file mode 100644 index 0000000000..b09f48a73e --- /dev/null +++ b/src/Unmanned_vehicle_AD_DQN/TrainingStrategies.py @@ -0,0 +1,501 @@ +# TrainingStrategies.py +import os +import math +import pickle +import numpy as np +from datetime import datetime +from collections import deque +import tensorflow as tf +from tensorflow.keras.optimizers import Adam + + +# 课程学习管理器 +class CurriculumManager: + def __init__(self, env): + self.env = env + self.current_stage = 0 + self.stage_thresholds = [0.3, 0.5, 0.7, 0.85] # 成功率阈值 + self.stage_configs = [ + # 阶段0: 入门 + { + 'pedestrian_cross': 4, # 十字路口行人数量 + 'pedestrian_normal': 2, # 普通路段行人数量 + 'pedestrian_speed_min': 0.5, # 行人最低速度 + 'pedestrian_speed_max': 1.0, # 行人最高速度 + 'max_episode_steps': 1200, # 最大步数 (20秒 * 60FPS) + 'success_threshold': 0.3 # 进入下一阶段成功率 + }, + # 阶段1: 初级 + { + 'pedestrian_cross': 6, + 'pedestrian_normal': 3, + 'pedestrian_speed_min': 0.7, + 'pedestrian_speed_max': 1.3, + 'max_episode_steps': 1800, # 30秒 + 'success_threshold': 0.5 + }, + # 阶段2: 中级 + { + 'pedestrian_cross': 8, + 'pedestrian_normal': 4, + 'pedestrian_speed_min': 0.8, + 'pedestrian_speed_max': 1.5, + 'max_episode_steps': 2400, # 40秒 + 'success_threshold': 0.7 + }, + # 阶段3: 高级 (正常难度) + { + 'pedestrian_cross': 10, + 'pedestrian_normal': 5, + 'pedestrian_speed_min': 1.0, + 'pedestrian_speed_max': 2.0, + 'max_episode_steps': 3600, # 60秒 + 'success_threshold': 0.85 + }, + # 阶段4: 专家 (挑战) + { + 'pedestrian_cross': 12, + 'pedestrian_normal': 6, + 'pedestrian_speed_min': 1.2, + 'pedestrian_speed_max': 2.5, + 'max_episode_steps': 3600, + 'success_threshold': 0.9 + } + ] + + # 训练历史 + self.success_history = deque(maxlen=20) # 记录最近20轮的成功情况 + self.reward_history = deque(maxlen=50) # 记录最近50轮的奖励 + + def update_stage(self, success, reward): + """更新训练阶段""" + # 记录历史 + self.success_history.append(1 if success else 0) + self.reward_history.append(reward) + + # 计算最近成功率 + if len(self.success_history) >= 10: + success_rate = sum(self.success_history) / len(self.success_history) + avg_reward = np.mean(self.reward_history) if self.reward_history else 0 + + print(f"课程学习 - 当前阶段: {self.current_stage}, 成功率: {success_rate:.2f}, 平均奖励: {avg_reward:.2f}") + + # 检查是否可以进入下一阶段 + if self.current_stage < len(self.stage_configs) - 1: + next_stage_threshold = self.stage_configs[self.current_stage]['success_threshold'] + if success_rate >= next_stage_threshold and avg_reward > 5: + self.current_stage += 1 + print(f"🎉 课程学习: 进阶到阶段 {self.current_stage}!") + return True + + # 如果表现太差,退回上一阶段 + if self.current_stage > 0 and success_rate < 0.2: + self.current_stage -= 1 + print(f"⚠️ 课程学习: 退回阶段 {self.current_stage}") + return True + + return False + + def get_current_config(self): + """获取当前阶段的配置""" + return self.stage_configs[min(self.current_stage, len(self.stage_configs) - 1)] + + def apply_to_environment(self): + """将当前阶段配置应用到环境""" + config = self.get_current_config() + return config + + +# 多目标优化器 +class MultiObjectiveOptimizer: + def __init__(self): + # 定义优化目标及其权重(可动态调整) + self.objectives = { + 'safety': { + 'weight': 0.4, + 'description': '安全避障和避免碰撞', + 'metrics': ['collision_avoidance', 'pedestrian_distance'] + }, + 'efficiency': { + 'weight': 0.25, + 'description': '快速到达目的地', + 'metrics': ['progress_speed', 'total_time'] + }, + 'comfort': { + 'weight': 0.2, + 'description': '平稳驾驶体验', + 'metrics': ['smoothness', 'steering_changes'] + }, + 'rule_following': { + 'weight': 0.15, + 'description': '遵守交通规则', + 'metrics': ['lane_keeping', 'speed_limit'] + } + } + + # 指标跟踪 + self.metrics_history = { + 'safety': [], + 'efficiency': [], + 'comfort': [], + 'rule_following': [] + } + + def compute_composite_reward(self, metrics): + """计算综合奖励值""" + composite = 0 + + for obj_name, obj_info in self.objectives.items(): + if obj_name in metrics: + # 归一化处理每个目标的贡献 + normalized_value = self._normalize_metric(metrics[obj_name], obj_name) + composite += normalized_value * obj_info['weight'] + + # 记录指标历史 + self.metrics_history[obj_name].append(normalized_value) + + # 特殊惩罚项 + if metrics.get('collision', False): + composite -= 10 + if metrics.get('off_road', False): + composite -= 5 + if metrics.get('dangerous_action', False): + composite -= 3 + + return composite + + def _normalize_metric(self, value, metric_name): + """归一化指标值到[0, 1]范围""" + # 不同指标的归一化方式不同 + normalization_rules = { + 'safety': lambda x: min(max(x / 10, 0), 1), # 假设安全分满分10 + 'efficiency': lambda x: min(max(x / 100, 0), 1), # 效率分满分100 + 'comfort': lambda x: min(max((x + 5) / 10, 0), 1), # 舒适度[-5, 5] -> [0, 1] + 'rule_following': lambda x: min(max(x, 0), 1) # 规则遵循度[0, 1] + } + + if metric_name in normalization_rules: + return normalization_rules[metric_name](value) + return min(max(value, 0), 1) # 默认截断到[0, 1] + + def adjust_weights(self, performance_feedback): + """根据性能反馈动态调整权重""" + # 如果某个目标表现持续较差,增加其权重 + recent_performance = {} + for obj in self.objectives: + if len(self.metrics_history[obj]) >= 10: + recent_avg = np.mean(self.metrics_history[obj][-10:]) + recent_performance[obj] = recent_avg + + if recent_performance: + # 找到表现最差的目标 + worst_obj = min(recent_performance, key=recent_performance.get) + best_obj = max(recent_performance, key=recent_performance.get) + + # 如果最差目标表现低于阈值,增加其权重 + if recent_performance[worst_obj] < 0.3: + adjustment = 0.05 + self.objectives[worst_obj]['weight'] += adjustment + self.objectives[best_obj]['weight'] -= adjustment + + # 确保权重总和为1 + total = sum(obj['weight'] for obj in self.objectives.values()) + for obj in self.objectives: + self.objectives[obj]['weight'] /= total + + print(f"动态权重调整: {worst_obj}权重↑ {adjustment:.3f}, {best_obj}权重↓ {adjustment:.3f}") + + def get_performance_report(self): + """生成性能报告""" + report = "多目标优化性能报告:\n" + report += "=" * 50 + "\n" + + for obj_name, obj_info in self.objectives.items(): + history = self.metrics_history[obj_name] + if history: + avg = np.mean(history[-20:]) if len(history) >= 20 else np.mean(history) + report += f"{obj_name}(权重:{obj_info['weight']:.2f}): 平均得分={avg:.3f}\n" + report += f" 描述: {obj_info['description']}\n" + + return report + + +# 模仿学习管理器 +class ImitationLearningManager: + def __init__(self, expert_data_path=None): + self.expert_data_path = expert_data_path + self.expert_data = [] + self.is_pretrained = False + + def load_expert_data(self, path): + """加载专家示范数据""" + try: + if os.path.exists(path): + with open(path, 'rb') as f: + self.expert_data = pickle.load(f) + print(f"已加载 {len(self.expert_data)} 条专家示范数据") + return True + else: + print(f"专家数据文件不存在: {path}") + return False + except Exception as e: + print(f"加载专家数据失败: {e}") + return False + + def collect_expert_demonstration(self, env, num_episodes=10): + """收集专家示范数据(可以手动控制或使用规则控制器)""" + print(f"开始收集专家示范数据 ({num_episodes}个episodes)...") + + demonstrations = [] + + for episode in range(num_episodes): + print(f"收集专家示范 Episode {episode + 1}/{num_episodes}") + + state = env.reset(episode) + done = False + episode_data = [] + + while not done: + # 这里可以使用规则控制器或手动控制 + # 示例:简单的规则控制器 + action = self._rule_based_controller(env) + + new_state, reward, done, _ = env.step(action) + + # 保存示范数据 + episode_data.append({ + 'state': state.copy(), + 'action': action, + 'reward': reward, + 'next_state': new_state.copy(), + 'done': done + }) + + state = new_state + + demonstrations.extend(episode_data) + env.cleanup_actors() + + # 保存专家数据 + self.expert_data = demonstrations + timestamp = datetime.now().strftime("%Y%m%d_%H%M%S") + save_path = f"expert_data_{timestamp}.pkl" + + with open(save_path, 'wb') as f: + pickle.dump(demonstrations, f) + + print(f"专家示范数据已保存到: {save_path}, 共 {len(demonstrations)} 条记录") + return True + + def _rule_based_controller(self, env): + """基于规则的控制器(作为专家示范)""" + # 获取车辆状态 + vehicle_location = env.vehicle.get_location() + velocity = env.vehicle.get_velocity() + speed_kmh = 3.6 * math.sqrt(velocity.x**2 + velocity.y**2) + + # 简单规则:保持速度在20-40 km/h,避免障碍物 + if speed_kmh < 20: + return 2 # 加速 + elif speed_kmh > 40: + return 0 # 减速 + else: + # 检查前方障碍物 + has_obstacle_ahead = self._check_obstacle_ahead(env) + if has_obstacle_ahead: + return 0 # 减速 + else: + return 1 # 保持 + + return 1 # 默认保持 + + def _check_obstacle_ahead(self, env): + """检查前方是否有障碍物(简化版本)""" + # 这里可以添加更复杂的障碍物检测逻辑 + # 暂时返回False + return False + + def pretrain_with_behavioral_cloning(self, model, epochs=20): + """使用行为克隆进行预训练""" + if not self.expert_data: + print("没有专家数据可用,跳过预训练") + return model + + print(f"开始行为克隆预训练 ({epochs}个epochs)...") + + # 准备训练数据 + states = [] + actions = [] + + for demo in self.expert_data: + states.append(demo['state']) + actions.append(demo['action']) + + # 将状态归一化 + states = np.array(states) / 255.0 + + # 将动作转换为one-hot编码 + actions_onehot = tf.keras.utils.to_categorical(actions, num_classes=5) + + # 备份原始编译设置 + original_loss = model.loss + original_optimizer = model.optimizer + original_metrics = model.metrics_names + + # 重新编译模型用于分类任务 + model.compile( + optimizer=Adam(learning_rate=0.0001), + loss='categorical_crossentropy', + metrics=['accuracy'] + ) + + # 训练模型模仿专家行为 + history = model.fit( + states, actions_onehot, + batch_size=32, + epochs=epochs, + validation_split=0.2, + verbose=1 + ) + + print(f"预训练完成 - 最终准确率: {history.history['accuracy'][-1]:.3f}") + + # 恢复原始编译设置 + model.compile( + optimizer=original_optimizer, + loss=original_loss, + metrics=original_metrics + ) + + self.is_pretrained = True + return model + + def train_with_dagger(self, model, env, iterations=5, episodes_per_iter=5): + """使用DAgger算法进行训练""" + print(f"开始DAgger训练 ({iterations}次迭代,每次{episodes_per_iter}个episodes)...") + + aggregated_data = self.expert_data.copy() + + for iteration in range(iterations): + print(f"\nDAgger 迭代 {iteration + 1}/{iterations}") + + # 使用当前策略收集数据 + new_demos = [] + + for episode in range(episodes_per_iter): + print(f" 收集数据 Episode {episode + 1}/{episodes_per_iter}") + + state = env.reset(episode) + done = False + + while not done: + # 使用当前策略选择动作 + qs = model.predict(np.array(state).reshape(-1, *state.shape) / 255)[0] + action = np.argmax(qs) + + # 执行动作 + new_state, reward, done, _ = env.step(action) + + # 专家纠正(这里可以添加专家纠正逻辑) + # 如果策略动作与专家建议不同,使用专家动作 + expert_action = self._rule_based_controller(env) + + # 保存数据(使用专家纠正后的动作) + new_demos.append({ + 'state': state.copy(), + 'action': expert_action, # 使用专家动作 + 'reward': reward, + 'next_state': new_state.copy(), + 'done': done + }) + + state = new_state + + env.cleanup_actors() + + # 合并数据 + aggregated_data.extend(new_demos) + + # 在合并数据上重新训练 + states = [d['state'] for d in aggregated_data] + actions = [d['action'] for d in aggregated_data] + + states = np.array(states) / 255.0 + actions_onehot = tf.keras.utils.to_categorical(actions, num_classes=5) + + # 训练模型 + history = model.fit( + states, actions_onehot, + batch_size=32, + epochs=10, + validation_split=0.1, + verbose=0 + ) + + print(f" 训练完成 - 准确率: {history.history['accuracy'][-1]:.3f}") + + print("DAgger训练完成!") + return model + + +# 优先经验回放缓冲区 +class PrioritizedReplayBuffer: + def __init__(self, max_size=10000, alpha=0.6, beta_start=0.4, beta_frames=100000): + self.max_size = max_size + self.alpha = alpha # 优先级程度 (0 = 均匀采样, 1 = 完全优先级) + self.beta_start = beta_start # 重要性采样权重起始值 + self.beta_frames = beta_frames # beta线性增长的帧数 + self.frame = 1 + + # 使用循环缓冲区 + self.buffer = deque(maxlen=max_size) + self.priorities = deque(maxlen=max_size) + + def __len__(self): + return len(self.buffer) + + def beta(self): + """线性递增的beta值,用于重要性采样权重""" + return min(1.0, self.beta_start + self.frame * (1.0 - self.beta_start) / self.beta_frames) + + def add(self, experience, error=None): + """添加经验到缓冲区""" + if error is None: + priority = max(self.priorities) if self.priorities else 1.0 + else: + priority = (abs(error) + 1e-5) ** self.alpha + + self.buffer.append(experience) + self.priorities.append(priority) + + def sample(self, batch_size): + """从缓冲区中采样一批经验""" + if len(self.buffer) == 0: + return [], [], [], [] + + # 计算采样概率 + priorities = np.array(self.priorities, dtype=np.float32) + probs = priorities ** self.alpha + probs /= probs.sum() + + # 采样索引 + indices = np.random.choice(len(self.buffer), batch_size, p=probs) + + # 获取样本 + samples = [self.buffer[i] for i in indices] + + # 计算重要性采样权重 + total = len(self.buffer) + weights = (total * probs[indices]) ** (-self.beta()) + weights /= weights.max() # 归一化 + + # 更新帧计数器 + self.frame += 1 + + return indices, samples, weights + + def update_priorities(self, indices, errors): + """更新采样经验的优先级""" + for idx, error in zip(indices, errors): + if 0 <= idx < len(self.priorities): + self.priorities[idx] = (abs(error) + 1e-5) ** self.alpha \ No newline at end of file diff --git a/src/Unmanned_vehicle_AD_DQN/main.py b/src/Unmanned_vehicle_AD_DQN/main.py index de06a3dddc..9c0084895b 100644 --- a/src/Unmanned_vehicle_AD_DQN/main.py +++ b/src/Unmanned_vehicle_AD_DQN/main.py @@ -15,11 +15,13 @@ from threading import Thread from tqdm import tqdm +import pickle import Hyperparameters from Environment import * from Model import * from Hyperparameters import * +from TrainingStrategies import * def extended_reward_calculation(env, action, reward, done, step_info): """ @@ -90,11 +92,6 @@ def extended_reward_calculation(env, action, reward, done, step_info): FPS = 60 # 帧率 ep_rewards = [-200] # 存储每轮奖励 - # 为了结果可重复性(注释掉) - # random.seed(1) - # np.random.seed(1) - # tf.compat.v1.set_random_seed(1) - # GPU内存配置,主要用于多智能体训练 gpu_options = tf.compat.v1.GPUOptions(per_process_gpu_memory_fraction=MEMORY_FRACTION) tf.compat.v1.keras.backend.set_session( @@ -186,8 +183,6 @@ def extended_reward_calculation(env, action, reward, done, step_info): # 应用课程学习配置 if agent.curriculum_manager: config = agent.curriculum_manager.get_current_config() - # 这里可以根据配置调整环境难度 - # 例如:调整行人数量、速度等 print(f"课程学习 - 阶段 {agent.curriculum_manager.current_stage}: " f"行人(十字路口={config['pedestrian_cross']}, 普通={config['pedestrian_normal']})") curriculum_stages.append(agent.curriculum_manager.current_stage) diff --git a/src/box/moblARMslndex.py b/src/box/moblARMslndex.py new file mode 100644 index 0000000000..950f646641 --- /dev/null +++ b/src/box/moblARMslndex.py @@ -0,0 +1,270 @@ +""" +MoblArmsIndex 生物力学模型 - 修改版 +这是基于 MoBL ARMS 模型的 MuJoCo 实现,专门用于手指指向任务 +""" + +import sys +import os + +# ================ 动态导入 BaseBMModel ================ +try: + # 尝试1: 从原始相对位置导入 + from ..base import BaseBMModel + print("✓ 使用相对导入: from ..base import BaseBMModel") +except ImportError: + try: + # 尝试2: 从绝对路径导入(假设在 uitb 包中) + from uitb.base import BaseBMModel + print("✓ 使用绝对导入: from uitb.base import BaseBMModel") + except ImportError: + try: + # 尝试3: 从常见路径导入 + import sys + import os + + # 将项目根目录添加到路径 + current_dir = os.path.dirname(os.path.abspath(__file__)) + project_root = os.path.dirname(os.path.dirname(current_dir)) + sys.path.insert(0, project_root) + + from base import BaseBMModel + print(f"✓ 使用项目根目录导入: from base import BaseBMModel (路径: {project_root})") + except ImportError as e: + # 尝试4: 搜索并动态导入 + print("⚠ 无法自动导入 BaseBMModel,尝试动态查找...") + + # 搜索 base.py 文件 + base_path = None + search_dirs = [ + os.path.dirname(os.path.dirname(os.path.abspath(__file__))), # 上一级目录 + os.path.dirname(os.path.dirname(os.path.dirname(os.path.abspath(__file__)))), # 上上级目录 + os.path.join(os.path.dirname(__file__), '..'), # 相对路径 + ] + + for search_dir in search_dirs: + possible_path = os.path.join(search_dir, 'base.py') + if os.path.exists(possible_path): + base_path = possible_path + break + + if base_path: + # 动态导入 + import importlib.util + spec = importlib.util.spec_from_file_location("base_module", base_path) + base_module = importlib.util.module_from_spec(spec) + sys.modules[spec.name] = base_module + spec.loader.exec_module(base_module) + BaseBMModel = base_module.BaseBMModel + print(f"✓ 动态导入成功: {base_path}") + else: + # 如果都失败,创建简化的 BaseBMModel + print("⚠ 无法找到 BaseBMModel,使用简化版本") + + class BaseBMModel: + """简化的 BaseBMModel 基类""" + def __init__(self, model, data, **kwargs): + self.model = model + self.data = data + + def _update(self, model, data): + """更新方法 - 子类应重写""" + pass + + @classmethod + def _get_floor(cls): + """获取地板 - 子类应重写""" + return None + + BaseBMModel = BaseBMModel + print("✓ 使用简化版本的 BaseBMModel") + +# ================ 继续原来的代码 ================ +import numpy as np +import mujoco + + +class MoblArmsIndex(BaseBMModel): + """ + 基于 MoBL ARMS 模型的生物力学模型 + + 来源: + - 原始 OpenSim 模型: https://simtk.org/frs/?group_id=657 + - MuJoCo 转换工具: https://github.com/aikkala/O2MConverter + + 说明: + 此模型与 uitb/bm_models/mobl_arms 中的模型相同, + 但食指处于弯曲状态并包含力传感器。 + """ + + def __init__(self, model, data, **kwargs): + """ + 初始化 MoblArmsIndex 模型 + + 参数: + model: MuJoCo 模型实例 + data: MuJoCo 数据实例 + **kwargs: 额外参数 + - shoulder_variant: 肩部变体,可选 "none" (默认) 或 "patch-v1" + """ + super().__init__(model, data, **kwargs) + + # 设置肩部变体 + # 使用 "none" 作为默认值 + # 使用 "patch-v1" 可以获得更合理的运动外观(未经全面测试) + self.shoulder_variant = kwargs.get("shoulder_variant", "none") + + print(f"✓ MoblArmsIndex 初始化完成,肩部变体: {self.shoulder_variant}") + + def _update(self, model, data): + """ + 更新模型状态 + + 此方法在每个时间步被调用,用于更新肩部约束。 + + 参数: + model: MuJoCo 模型实例 + data: MuJoCo 数据实例 + """ + # 更新肩部等式约束 + if self.shoulder_variant.startswith("patch"): + # 约束1: shoulder1_r2_con 约束的数据更新 + model.equality("shoulder1_r2_con").data[1] = \ + -((np.pi - 2 * data.joint('shoulder_elv').qpos) / np.pi) + + # patch-v2 变体有额外的约束 + if self.shoulder_variant == "patch-v2": + # 动态调整肩部旋转范围 + data.joint('shoulder_rot').range[:] = \ + np.array([-np.pi / 2, np.pi / 9]) - \ + 2 * np.min((data.joint('shoulder_elv').qpos, + np.pi - data.joint('shoulder_elv').qpos)) / np.pi \ + * data.joint('elv_angle').qpos + + # 执行前向计算,更新物理状态 + mujoco.mj_forward(model, data) + + # 打印调试信息(可选) + if hasattr(self, 'debug') and self.debug: + print(f"更新肩部约束: shoulder_elv={data.joint('shoulder_elv').qpos:.3f}, " + f"约束值={model.equality('shoulder1_r2_con').data[1]:.3f}") + + @classmethod + def _get_floor(cls): + """ + 获取地板配置 + + 此模型不包含地板,返回 None。 + + 返回: + None: 表示此模型没有地板 + """ + return None + + def get_force_sensor_data(self, data, sensor_name="index_force_sensor"): + """ + 获取力传感器数据 + + 参数: + data: MuJoCo 数据实例 + sensor_name: 传感器名称,默认为 "index_force_sensor" + + 返回: + numpy.ndarray: 传感器数据,如果没有找到传感器则返回 None + """ + try: + sensor_id = self.model.sensor(sensor_name).id + return data.sensordata[sensor_id] + except Exception as e: + if hasattr(self, 'debug') and self.debug: + print(f"⚠ 无法获取力传感器数据: {e}") + return None + + def set_debug_mode(self, debug=True): + """ + 设置调试模式 + + 参数: + debug: 是否启用调试模式 + """ + self.debug = debug + + def __str__(self): + """ + 返回模型的字符串表示 + + 返回: + str: 模型描述 + """ + return (f"MoblArmsIndex 模型 (肩部变体: {self.shoulder_variant})\n" + f"描述: 基于 MoBL ARMS 的上肢模型,食指弯曲并包含力传感器") + + +# ================ 测试代码 ================ +if __name__ == "__main__": + """ + 直接运行此文件的测试代码 + """ + print("=" * 60) + print("MoblArmsIndex 模型测试") + print("=" * 60) + + # 测试导入 + print("1. 测试导入和类定义:") + print(f" 类名: {MoblArmsIndex.__name__}") + print(f" 基类: {MoblArmsIndex.__bases__[0].__name__}") + print(f" 文档: {MoblArmsIndex.__doc__.strip().split('\n')[0]}") + + # 测试创建实例(需要 MuJoCo 模型) + try: + import mujoco + import numpy as np + + print("\n2. 测试创建实例:") + + # 创建一个简单的 MuJoCo 模型用于测试 + xml_string = """ + + + + + + + + + """ + + model = mujoco.MjModel.from_xml_string(xml_string) + data = mujoco.MjData(model) + + # 创建模型实例 + bm_model = MoblArmsIndex(model, data, shoulder_variant="none") + + print(f" ✓ 成功创建 MoblArmsIndex 实例") + print(f" 肩部变体: {bm_model.shoulder_variant}") + + # 测试更新方法 + print("\n3. 测试更新方法:") + bm_model._update(model, data) + print(" ✓ _update 方法执行成功") + + # 测试获取地板 + print("\n4. 测试获取地板:") + floor = bm_model._get_floor() + print(f" 地板配置: {floor}") + + # 测试字符串表示 + print("\n5. 测试字符串表示:") + print(f" {bm_model}") + + print("\n" + "=" * 60) + print("所有测试通过!") + print("=" * 60) + + except Exception as e: + print(f"\n✗ 测试过程中出现错误: {type(e).__name__}: {e}") + import traceback + traceback.print_exc() + + print("\n" + "=" * 60) + print("注意: 缺少 MuJoCo 模型文件,但导入测试成功") + print("=" * 60) diff --git a/src/drone_perception/dron.py b/src/drone_perception/dron.py deleted file mode 100644 index c873064205..0000000000 --- a/src/drone_perception/dron.py +++ /dev/null @@ -1,561 +0,0 @@ -""" -drone_perception.py -无人机感知模块 - 用于目标检测、深度估计和障碍物感知 -""" - -import cv2 -import numpy as np -from typing import Tuple, Optional, List -import time -import os - -class DronePerception: - """ - 无人机感知模块主类 - 提供目标检测、深度估计和障碍物感知功能 - """ - - def __init__(self, camera_params: Optional[dict] = None): - """ - 初始化感知模块 - - Args: - camera_params: 相机内参矩阵和畸变系数 - """ - self.camera_params = camera_params or { - 'fx': 920.0, 'fy': 920.0, # 焦距 - 'cx': 640.0, 'cy': 360.0, # 主点坐标 - 'width': 1280, 'height': 720 - } - - # 初始化目标检测器 - self.target_detector = TargetDetector() - - # 初始化深度估计器 - self.depth_estimator = DepthEstimator() - - # 状态变量 - self.obstacles = [] - self.targets = [] - self.last_update_time = time.time() - - print("DronePerception模块初始化完成") - - def process_frame(self, frame: np.ndarray) -> dict: - """ - 处理单帧图像 - - Args: - frame: 输入图像帧 (BGR格式) - - Returns: - 包含感知结果的字典 - """ - if frame is None: - return {"error": "空帧输入"} - - results = { - 'timestamp': time.time(), - 'frame_shape': frame.shape, - 'targets': [], - 'obstacles': [], - 'depth_map': None - } - - try: - # 1. 目标检测 - self.targets = self.target_detector.detect(frame) - results['targets'] = self.targets - - # 2. 深度估计 - depth_map = self.depth_estimator.estimate(frame) - results['depth_map'] = depth_map - - # 3. 障碍物检测 - self.obstacles = self.detect_obstacles(frame, depth_map) - results['obstacles'] = self.obstacles - - # 4. 安全区域分析 - results['safety_score'] = self.analyze_safety(self.obstacles) - - self.last_update_time = time.time() - - except Exception as e: - results['error'] = str(e) - print(f"处理帧时出错: {e}") - - return results - - def detect_obstacles(self, frame: np.ndarray, depth_map: Optional[np.ndarray]) -> List[dict]: - """ - 检测障碍物 - - Args: - frame: 输入图像 - depth_map: 深度图 - - Returns: - 障碍物列表 - """ - obstacles = [] - - # 只检测非目标区域的障碍物(避免重复检测目标) - mask = np.ones(frame.shape[:2], dtype=np.uint8) * 255 - - # 在目标区域创建掩码,避免将目标误检测为障碍物 - for target in self.targets: - if 'bbox' in target: - x1, y1, x2, y2 = map(int, target['bbox']) - cv2.rectangle(mask, (x1, y1), (x2, y2), 0, -1) - - # 使用掩码后的图像进行障碍物检测 - gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY) - gray_masked = cv2.bitwise_and(gray, gray, mask=mask) - - # 使用自适应阈值而不是Canny,减少噪声 - blurred = cv2.GaussianBlur(gray_masked, (5, 5), 0) - edges = cv2.adaptiveThreshold(blurred, 255, - cv2.ADAPTIVE_THRESH_GAUSSIAN_C, - cv2.THRESH_BINARY, 11, 2) - - # 形态学操作去除小噪声 - kernel = np.ones((3,3), np.uint8) - edges = cv2.morphologyEx(edges, cv2.MORPH_CLOSE, kernel) - - # 查找轮廓 - contours, _ = cv2.findContours(edges, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE) - - for contour in contours[:10]: # 只处理前10个最大轮廓 - area = cv2.contourArea(contour) - if 100 < area < 50000: # 调整面积阈值范围 - x, y, w, h = cv2.boundingRect(contour) - - # 计算轮廓的中心 - M = cv2.moments(contour) - if M["m00"] != 0: - cX = int(M["m10"] / M["m00"]) - cY = int(M["m01"] / M["m00"]) - else: - cX, cY = x + w//2, y + h//2 - - obstacle = { - 'bbox': [x, y, x+w, y+h], - 'area': area, - 'center': (cX, cY), - 'type': 'unknown', - 'contour_points': len(contour) - } - - # 如果有深度信息,估算距离 - if depth_map is not None and y < depth_map.shape[0] and x < depth_map.shape[1]: - # 获取ROI区域内的深度值 - y_end = min(y+h, depth_map.shape[0]) - x_end = min(x+w, depth_map.shape[1]) - roi_depth = depth_map[y:y_end, x:x_end] - - if roi_depth.size > 0: - # 使用中值而不是均值,减少异常值影响 - median_depth = np.median(roi_depth) - obstacle['estimated_distance'] = float(median_depth) - - obstacles.append(obstacle) - - return obstacles - - def analyze_safety(self, obstacles: List[dict]) -> float: - """ - 分析当前帧的安全性 - - Args: - obstacles: 障碍物列表 - - Returns: - 安全分数 (0-1) - """ - if not obstacles: - return 1.0 - - danger_score = 0 - max_danger_per_obstacle = 0.3 # 每个障碍物最大危险分数 - - for obs in obstacles: - # 障碍物大小越大,危险分数越高(对数缩放) - area_factor = min(np.log10(obs['area'] / 100 + 1), 1.0) - - # 距离因素 - if 'estimated_distance' in obs: - # 距离越近危险越大,指数衰减 - dist = obs['estimated_distance'] - if dist <= 0.1: # 非常近 - dist_factor = 1.0 - else: - dist_factor = min(2.0 / (dist + 0.1), 1.0) - else: - dist_factor = 0.3 # 默认中等危险 - - # 位置因素:靠近图像中心更危险 - h, w = self.camera_params['height'], self.camera_params['width'] - center_x, center_y = w // 2, h // 2 - obs_center_x, obs_center_y = obs['center'] - - # 计算到图像中心的距离(归一化) - dist_to_center = np.sqrt(((obs_center_x - center_x) / w)**2 + - ((obs_center_y - center_y) / h)**2) - center_factor = max(0, 1.0 - dist_to_center * 2) # 越靠近中心值越大 - - # 综合危险分数 - obstacle_danger = area_factor * dist_factor * center_factor - danger_score += min(obstacle_danger, max_danger_per_obstacle) - - # 确保危险分数在0-1之间 - danger_score = min(danger_score, 1.0) - safety_score = 1.0 - danger_score - - return round(safety_score, 2) - - def visualize(self, frame: np.ndarray, results: dict) -> np.ndarray: - """ - 可视化感知结果 - - Args: - frame: 原始图像帧 - results: 感知结果 - - Returns: - 可视化后的图像 - """ - vis_frame = frame.copy() - - # 绘制目标边界框 - for target in results.get('targets', []): - if 'bbox' in target: - x1, y1, x2, y2 = map(int, target['bbox']) - cv2.rectangle(vis_frame, (x1, y1), (x2, y2), (0, 255, 0), 2) - if 'class_name' in target: - cv2.putText(vis_frame, f"Target: {target['class_name']}", - (x1, y1-10), cv2.FONT_HERSHEY_SIMPLEX, - 0.5, (0, 255, 0), 2) - - # 绘制障碍物边界框 - for obstacle in results.get('obstacles', []): - if 'bbox' in obstacle: - x1, y1, x2, y2 = map(int, obstacle['bbox']) - cv2.rectangle(vis_frame, (x1, y1), (x2, y2), (0, 0, 255), 1) - - # 显示估计距离 - if 'estimated_distance' in obstacle: - dist_text = f"{obstacle['estimated_distance']:.2f}" - cv2.putText(vis_frame, dist_text, - (x1, y2+15), cv2.FONT_HERSHEY_SIMPLEX, - 0.4, (0, 0, 255), 1) - - # 显示安全分数 - safety_score = results.get('safety_score', 1.0) - if safety_score > 0.7: - safety_color = (0, 255, 0) # 绿色 - elif safety_score > 0.4: - safety_color = (0, 255, 255) # 黄色 - else: - safety_color = (0, 0, 255) # 红色 - - # 绘制安全分数背景框 - cv2.rectangle(vis_frame, (5, 5), (200, 85), (40, 40, 40), -1) - cv2.rectangle(vis_frame, (5, 5), (200, 85), safety_color, 2) - - cv2.putText(vis_frame, f"Safety Score: {safety_score}", - (10, 30), cv2.FONT_HERSHEY_SIMPLEX, - 0.7, safety_color, 2) - - # 显示帧信息 - cv2.putText(vis_frame, f"Targets: {len(results.get('targets', []))}", - (10, 55), cv2.FONT_HERSHEY_SIMPLEX, - 0.6, (255, 255, 255), 1) - cv2.putText(vis_frame, f"Obstacles: {len(results.get('obstacles', []))}", - (10, 75), cv2.FONT_HERSHEY_SIMPLEX, - 0.6, (255, 255, 255), 1) - - return vis_frame - - def get_status(self) -> dict: - """ - 获取模块状态 - - Returns: - 状态字典 - """ - return { - 'active': True, - 'last_update': self.last_update_time, - 'camera_params': self.camera_params, - 'target_count': len(self.targets), - 'obstacle_count': len(self.obstacles) - } - - def save_visualization(self, frame: np.ndarray, results: dict, filename: str = "perception_output.jpg"): - """ - 保存可视化结果到文件 - - Args: - frame: 原始图像帧 - results: 感知结果 - filename: 输出文件名 - """ - vis_frame = self.visualize(frame, results) - cv2.imwrite(filename, vis_frame) - print(f"可视化结果已保存到: {filename}") - return filename - - -class TargetDetector: - """目标检测器""" - - def __init__(self): - # 这里可以加载YOLO等模型 - # 简化版本使用预定义的HSV颜色范围 - self.color_ranges = { - 'red': [ - ([0, 100, 100], [10, 255, 255]), # 红色范围1 - ([160, 100, 100], [180, 255, 255]) # 红色范围2 - ], - 'green': ([40, 50, 50], [80, 255, 255]), - 'blue': ([100, 50, 50], [130, 255, 255]), - 'yellow': ([20, 100, 100], [30, 255, 255]) - } - - def detect(self, frame: np.ndarray) -> List[dict]: - """ - 检测目标 - - Args: - frame: 输入图像 - - Returns: - 检测到的目标列表 - """ - targets = [] - hsv = cv2.cvtColor(frame, cv2.COLOR_BGR2HSV) - - for color_name, color_range in self.color_ranges.items(): - if color_name == 'red': - # 红色有两个范围 - lower1 = np.array(color_range[0][0], dtype=np.uint8) - upper1 = np.array(color_range[0][1], dtype=np.uint8) - lower2 = np.array(color_range[1][0], dtype=np.uint8) - upper2 = np.array(color_range[1][1], dtype=np.uint8) - - mask1 = cv2.inRange(hsv, lower1, upper1) - mask2 = cv2.inRange(hsv, lower2, upper2) - mask = cv2.bitwise_or(mask1, mask2) - else: - lower = np.array(color_range[0], dtype=np.uint8) - upper = np.array(color_range[1], dtype=np.uint8) - mask = cv2.inRange(hsv, lower, upper) - - # 形态学操作 - kernel = np.ones((5,5), np.uint8) - mask = cv2.morphologyEx(mask, cv2.MORPH_CLOSE, kernel) - mask = cv2.morphologyEx(mask, cv2.MORPH_OPEN, kernel) - - contours, _ = cv2.findContours(mask, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE) - - for contour in contours: - area = cv2.contourArea(contour) - if 100 < area < 50000: # 合理的面积范围 - x, y, w, h = cv2.boundingRect(contour) - - # 计算轮廓的圆度 - perimeter = cv2.arcLength(contour, True) - if perimeter > 0: - circularity = 4 * np.pi * area / (perimeter * perimeter) - else: - circularity = 0 - - target = { - 'bbox': [x, y, x+w, y+h], - 'class_name': color_name, - 'confidence': min(0.7 + circularity * 0.3, 0.95), # 圆度影响置信度 - 'area': area, - 'circularity': round(circularity, 2) - } - targets.append(target) - - return targets - - -class DepthEstimator: - """深度估计器""" - - def __init__(self): - # 这里可以加载MiDaS等深度估计模型 - pass - - def estimate(self, frame: np.ndarray) -> Optional[np.ndarray]: - """ - 估计深度图 - - Args: - frame: 输入图像 - - Returns: - 深度图 (归一化到0-1) - """ - # 简化版本:使用图像强度作为深度代理 - gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY) - - # 使用双边滤波保留边缘 - blurred = cv2.bilateralFilter(gray, 9, 75, 75) - - # 计算局部对比度 - laplacian = cv2.Laplacian(blurred, cv2.CV_64F) - contrast_map = np.abs(laplacian) - - # 归一化对比度图 - if contrast_map.max() > contrast_map.min(): - contrast_map = (contrast_map - contrast_map.min()) / (contrast_map.max() - contrast_map.min()) - - # 计算强度图 - intensity_map = blurred.astype(float) / 255.0 - - # 模拟距离:假设图像中心区域更近 - h, w = gray.shape - y_coords, x_coords = np.ogrid[:h, :w] - center_x, center_y = w // 2, h // 2 - - # 创建中心距离权重图 - dist_from_center = np.sqrt(((x_coords - center_x) / (w//2))**2 + - ((y_coords - center_y) / (h//2))**2) - center_weight = np.clip(1.0 - dist_from_center, 0, 1) - - # 创建深度图:高强度(亮)+ 高对比度(边缘)+ 靠近中心 表示更近 - depth_map = 0.4 * intensity_map + 0.3 * (1 - contrast_map) + 0.3 * center_weight - - # 归一化到0-1 - depth_map = (depth_map - depth_map.min()) / (depth_map.max() - depth_map.min() + 1e-6) - - # 应用高斯平滑 - depth_map = cv2.GaussianBlur(depth_map, (7, 7), 0) - - return depth_map - - -def test_perception(): - """测试函数""" - print("=== 测试DronePerception模块 ===") - - # 初始化感知模块 - perception = DronePerception() - - # 创建测试图像 - test_frame = np.zeros((480, 640, 3), dtype=np.uint8) - - # 添加一些彩色方块模拟目标 - cv2.rectangle(test_frame, (100, 100), (200, 200), (0, 255, 0), -1) # 绿色 - cv2.rectangle(test_frame, (400, 150), (500, 250), (0, 0, 255), -1) # 红色 - cv2.rectangle(test_frame, (300, 300), (400, 400), (255, 0, 0), -1) # 蓝色 - - # 添加一些障碍物(灰色方块) - cv2.rectangle(test_frame, (50, 50), (80, 200), (100, 100, 100), -1) # 左边障碍物 - cv2.rectangle(test_frame, (550, 300), (600, 400), (150, 150, 150), -1) # 右边障碍物 - - # 添加一些随机噪声(模拟纹理) - noise = np.random.randint(0, 30, test_frame.shape[:2], dtype=np.uint8) - for i in range(3): - test_frame[:,:,i] = cv2.add(test_frame[:,:,i], noise[:,:,np.newaxis]) - - # 处理帧 - results = perception.process_frame(test_frame) - - # 打印详细结果 - print(f"\n检测到目标数量: {len(results['targets'])}") - for i, target in enumerate(results['targets']): - print(f" 目标{i+1}: {target.get('class_name', 'unknown')}, " - f"位置: {target.get('bbox', [])}, " - f"置信度: {target.get('confidence', 0):.2f}") - - print(f"\n检测到障碍物数量: {len(results['obstacles'])}") - for i, obstacle in enumerate(results['obstacles'][:3]): # 只显示前3个 - if 'estimated_distance' in obstacle: - print(f" 障碍物{i+1}: 位置{obstacle.get('bbox', [])}, " - f"距离: {obstacle['estimated_distance']:.2f}") - else: - print(f" 障碍物{i+1}: 位置{obstacle.get('bbox', [])}") - - print(f"\n安全分数: {results['safety_score']}") - - if results.get('error'): - print(f"错误信息: {results['error']}") - - # 保存可视化结果 - output_dir = "output" - if not os.path.exists(output_dir): - os.makedirs(output_dir) - - output_path = os.path.join(output_dir, "perception_test_output.jpg") - perception.save_visualization(test_frame, results, output_path) - - # 显示模块状态 - print("\n模块状态:") - for key, value in perception.get_status().items(): - print(f" {key}: {value}") - - # 保存深度图 - if results['depth_map'] is not None: - depth_map = results['depth_map'] - depth_map_display = (depth_map * 255).astype(np.uint8) - depth_map_colored = cv2.applyColorMap(depth_map_display, cv2.COLORMAP_JET) - depth_path = os.path.join(output_dir, "depth_map.jpg") - cv2.imwrite(depth_path, depth_map_colored) - print(f"深度图已保存到: {depth_path}") - - print(f"\n测试完成!结果已保存到 {output_dir} 目录") - - -def test_with_real_image(image_path: str = None): - """使用真实图像测试""" - print("=== 使用真实图像测试 ===") - - # 如果没有提供图像路径,创建一个简单的测试图像 - if image_path is None or not os.path.exists(image_path): - print("未提供有效图像路径,创建测试图像...") - test_frame = np.zeros((480, 640, 3), dtype=np.uint8) - cv2.circle(test_frame, (320, 240), 100, (0, 255, 0), -1) # 绿色圆形 - cv2.rectangle(test_frame, (100, 100), (200, 150), (255, 0, 0), -1) # 蓝色矩形 - cv2.line(test_frame, (500, 100), (600, 400), (0, 0, 255), 5) # 红色线 - else: - test_frame = cv2.imread(image_path) - if test_frame is None: - print(f"无法读取图像: {image_path}") - return - - # 初始化感知模块 - perception = DronePerception() - - # 处理帧 - results = perception.process_frame(test_frame) - - print(f"图像尺寸: {test_frame.shape}") - print(f"检测到目标: {len(results['targets'])}") - print(f"检测到障碍物: {len(results['obstacles'])}") - print(f"安全分数: {results['safety_score']}") - - # 保存结果 - output_dir = "output" - if not os.path.exists(output_dir): - os.makedirs(output_dir) - - output_path = os.path.join(output_dir, "real_image_test.jpg") - perception.save_visualization(test_frame, results, output_path) - - print(f"结果已保存到: {output_path}") - - -if __name__ == "__main__": - # 创建输出目录 - if not os.path.exists("output"): - os.makedirs("output") - - # 运行测试 - test_perception() - - # 如果需要测试真实图像,取消下面一行的注释并提供图像路径 - # test_with_real_image("your_image.jpg") \ No newline at end of file diff --git a/src/drone_perception/environment_3d.py b/src/drone_perception/environment_3d.py new file mode 100644 index 0000000000..ba994263c5 --- /dev/null +++ b/src/drone_perception/environment_3d.py @@ -0,0 +1,1007 @@ +""" +3D环境建设模块 +实现地形生成、环境建模、可视化等功能 +""" +import numpy as np +import matplotlib.pyplot as plt +from mpl_toolkits.mplot3d.art3d import Poly3DCollection +import plotly.graph_objects as go +from plotly.subplots import make_subplots +import json +import random +import time +from scipy import ndimage +from scipy.interpolate import CubicSpline + +class TerrainConfig: + """地形配置参数""" + + def __init__(self, + width: float = 100.0, + length: float = 100.0, + height_range: float = 20.0, + resolution: float = 2.0, + seed: int = 42, + has_hills: bool = True, + has_valleys: bool = True, + has_river: bool = False, + vegetation_density: float = 0.1, + tree_density: float = 0.02, + water_level: float = 5.0): + self.width = width + self.length = length + self.height_range = height_range + self.resolution = resolution + self.seed = seed + self.has_hills = has_hills + self.has_valleys = has_valleys + self.has_river = has_river + self.vegetation_density = vegetation_density + self.tree_density = tree_density + self.water_level = water_level + +class TerrainGenerator: + """地形生成器""" + + def __init__(self, config: TerrainConfig): + """初始化地形生成器""" + self.config = config + self.heightmap = None + self.terrain_mesh = None + self.obstacles_mesh = [] + self.water_mesh = None + + # 设置随机种子 + np.random.seed(config.seed) + random.seed(config.seed) + + def generate_terrain(self) -> np.ndarray: + """生成地形高度图""" + # 计算网格尺寸 + nx = int(self.config.width / self.config.resolution) + ny = int(self.config.length / self.config.resolution) + + # 初始化高度图 + heightmap = np.zeros((nx, ny)) + + # 添加基本噪声 + for i in range(nx): + for j in range(ny): + noise = np.random.randn() * 0.5 + heightmap[i, j] = noise + + # 使用高斯滤波平滑 + heightmap = ndimage.gaussian_filter(heightmap, sigma=1.0) + + # 归一化到 [0, 1] + heightmap_min = heightmap.min() + heightmap_max = heightmap.max() + if heightmap_max - heightmap_min > 0: + heightmap = (heightmap - heightmap_min) / (heightmap_max - heightmap_min) + + # 应用高度范围 + heightmap = heightmap * self.config.height_range + + # 添加丘陵 + if self.config.has_hills: + num_hills = random.randint(3, 8) + for _ in range(num_hills): + center_x = random.uniform(0.2, 0.8) * nx + center_y = random.uniform(0.2, 0.8) * ny + radius = random.uniform(5, 15) + height = random.uniform(3, 8) + + for i in range(nx): + for j in range(ny): + distance = np.sqrt((i - center_x)**2 + (j - center_y)**2) + if distance < radius: + # 高斯形状的山丘 + hill_height = height * np.exp(-(distance**2) / (2 * (radius/3)**2)) + heightmap[i, j] += hill_height + + # 添加山谷/河流 + if self.config.has_valleys or self.config.has_river: + if self.config.has_river: + # 创建弯曲的河流 + river_path = self._generate_river_path(nx, ny) + for point in river_path: + x, y = point + radius = 3 + for i in range(max(0, int(x)-radius), min(nx, int(x)+radius+1)): + for j in range(max(0, int(y)-radius), min(ny, int(y)+radius+1)): + distance = np.sqrt((i - x)**2 + (j - y)**2) + if distance < radius: + heightmap[i, j] -= (1 - distance/radius) * 3 + else: + # 添加随机山谷 + num_valleys = random.randint(2, 5) + for _ in range(num_valleys): + start_x = random.uniform(0.1, 0.9) * nx + start_y = random.uniform(0.1, 0.9) * ny + length = random.uniform(20, 40) + angle = random.uniform(0, 2*np.pi) + width = random.uniform(3, 8) + + for i in range(nx): + for j in range(ny): + # 计算点到线段的距离 + distance = self._distance_to_line(i, j, start_x, start_y, length, angle) + if distance < width: + valley_depth = 2 * (1 - distance/width) + heightmap[i, j] -= valley_depth + + # 确保最小高度为0 + heightmap = heightmap - heightmap.min() + + self.heightmap = heightmap + self._create_mesh() + + return heightmap + + def _generate_river_path(self, nx: int, ny: int) -> list: + """生成河流路径""" + path = [] + + # 随机起点和终点 + start_x = random.uniform(0.1 * nx, 0.4 * nx) + start_y = 0 if random.random() < 0.5 else ny-1 + + end_x = random.uniform(0.6 * nx, 0.9 * nx) + end_y = ny-1 if start_y == 0 else 0 + + # 生成控制点进行插值 + num_points = 10 + t = np.linspace(0, 1, num_points) + + # 添加一些随机偏移创建弯曲效果 + control_x = np.linspace(start_x, end_x, num_points) + control_y = np.linspace(start_y, end_y, num_points) + + for i in range(1, num_points-1): + control_x[i] += random.uniform(-0.1 * nx, 0.1 * nx) + control_y[i] += random.uniform(-0.1 * ny, 0.1 * ny) + + # 三次样条插值 + cs_x = CubicSpline(t, control_x) + cs_y = CubicSpline(t, control_y) + + # 生成更密集的点 + t_dense = np.linspace(0, 1, 100) + for t_val in t_dense: + x = cs_x(t_val) + y = cs_y(t_val) + if 0 <= x < nx and 0 <= y < ny: + path.append((x, y)) + + return path + + def _distance_to_line(self, x: float, y: float, start_x: float, start_y: float, + length: float, angle: float) -> float: + """计算点到线段的距离""" + end_x = start_x + length * np.cos(angle) + end_y = start_y + length * np.sin(angle) + + # 计算点到直线的最短距离 + numerator = abs((end_y - start_y)*x - (end_x - start_x)*y + end_x*start_y - end_y*start_x) + denominator = np.sqrt((end_y - start_y)**2 + (end_x - start_x)**2) + + return numerator / denominator if denominator > 0 else float('inf') + + def _create_mesh(self): + """从高度图创建网格""" + if self.heightmap is None: + return + + nx, ny = self.heightmap.shape + + # 创建顶点 + vertices = [] + for i in range(nx): + for j in range(ny): + x = i * self.config.resolution + y = j * self.config.resolution + z = self.heightmap[i, j] + vertices.append([x, y, z]) + + vertices = np.array(vertices) + + # 创建三角形面片 + faces = [] + for i in range(nx-1): + for j in range(ny-1): + idx1 = i * ny + j + idx2 = i * ny + (j+1) + idx3 = (i+1) * ny + j + idx4 = (i+1) * ny + (j+1) + + # 两个三角形组成一个网格单元 + faces.append([idx1, idx2, idx3]) + faces.append([idx2, idx4, idx3]) + + self.terrain_mesh = { + 'vertices': vertices, + 'faces': np.array(faces) + } + + # 创建水面 + if self.config.water_level > 0: + self._create_water_mesh() + + def _create_water_mesh(self): + """创建水面网格""" + nx, ny = self.heightmap.shape + + vertices = [] + faces = [] + + # 创建平面水面 + water_height = self.config.water_level + + # 四个顶点 + vertices.append([0, 0, water_height]) + vertices.append([0, ny * self.config.resolution, water_height]) + vertices.append([nx * self.config.resolution, 0, water_height]) + vertices.append([nx * self.config.resolution, ny * self.config.resolution, water_height]) + + # 两个三角形 + faces.append([0, 1, 2]) + faces.append([1, 3, 2]) + + self.water_mesh = { + 'vertices': np.array(vertices), + 'faces': np.array(faces) + } + + def add_building(self, position: list, dimensions: list): + """添加建筑物障碍物""" + x, y, z_base = position + width, depth, height = dimensions + + # 建筑物底部顶点 + vertices = [ + [x, y, z_base], + [x + width, y, z_base], + [x + width, y + depth, z_base], + [x, y + depth, z_base], + [x, y, z_base + height], + [x + width, y, z_base + height], + [x + width, y + depth, z_base + height], + [x, y + depth, z_base + height] + ] + + # 立方体的6个面(12个三角形) + faces = [ + [0, 1, 2], [0, 2, 3], # 底面 + [4, 5, 6], [4, 6, 7], # 顶面 + [0, 1, 5], [0, 5, 4], # 前面 + [2, 3, 7], [2, 7, 6], # 后面 + [1, 2, 6], [1, 6, 5], # 右面 + [0, 3, 7], [0, 7, 4] # 左面 + ] + + building_mesh = { + 'vertices': np.array(vertices), + 'faces': np.array(faces), + 'centroid': [x + width/2, y + depth/2, z_base + height/2], + 'bounds': [[x, y, z_base], [x+width, y+depth, z_base+height]] + } + + self.obstacles_mesh.append(building_mesh) + + # 调整地形高度(建筑物区域需要平整) + if self.heightmap is not None: + nx, ny = self.heightmap.shape + building_grid_x = int(x / self.config.resolution) + building_grid_y = int(y / self.config.resolution) + building_grid_w = int(width / self.config.resolution) + building_grid_d = int(depth / self.config.resolution) + + for i in range(building_grid_x, min(building_grid_x + building_grid_w, nx)): + for j in range(building_grid_y, min(building_grid_y + building_grid_d, ny)): + if 0 <= i < nx and 0 <= j < ny: + # 将建筑物区域的地形高度设置为建筑物基础高度 + self.heightmap[i, j] = z_base + + def add_tree_obstacle(self, position: tuple, height: float = 10.0): + """添加树木障碍物""" + x, y = position + + # 获取地形高度 + if self.heightmap is not None: + nx, ny = self.heightmap.shape + grid_x = min(int(x / self.config.resolution), nx-1) + grid_y = min(int(y / self.config.resolution), ny-1) + z_base = self.heightmap[grid_x, grid_y] + else: + z_base = 0 + + # 树冠(圆锥体) + crown_radius = 2.0 + crown_height = height * 0.7 + crown_base = z_base + height * 0.3 + + # 创建树冠网格(圆锥体) + crown_vertices = [] + crown_faces = [] + + # 圆锥顶点 + crown_vertices.append([x, y, crown_base + crown_height]) + + # 圆锥底部圆周上的点 + num_segments = 8 + for i in range(num_segments): + angle = 2 * np.pi * i / num_segments + vx = x + crown_radius * np.cos(angle) + vy = y + crown_radius * np.sin(angle) + crown_vertices.append([vx, vy, crown_base]) + + # 圆锥侧面三角形 + for i in range(num_segments): + crown_faces.append([0, i+1, (i+1)%num_segments + 1]) + + # 圆锥底面三角形 + center_idx = num_segments + 1 + crown_vertices.append([x, y, crown_base]) # 底面中心点 + for i in range(num_segments): + crown_faces.append([center_idx, i+1, (i+1)%num_segments + 1]) + + tree_mesh = { + 'vertices': np.array(crown_vertices), + 'faces': np.array(crown_faces), + 'centroid': [x, y, crown_base + crown_height/2], + 'bounds': [[x-crown_radius, y-crown_radius, z_base], + [x+crown_radius, y+crown_radius, z_base+height]], + 'type': 'tree' + } + + self.obstacles_mesh.append(tree_mesh) + + def add_vegetation(self, density: float = None): + """添加植被""" + if density is None: + density = self.config.vegetation_density + + if self.heightmap is None: + return + + nx, ny = self.heightmap.shape + num_trees = int(nx * ny * density) + + for _ in range(num_trees): + x = random.uniform(0, nx * self.config.resolution) + y = random.uniform(0, ny * self.config.resolution) + + # 获取地形高度 + grid_x = min(int(x / self.config.resolution), nx-1) + grid_y = min(int(y / self.config.resolution), ny-1) + z_base = self.heightmap[grid_x, grid_y] + + # 避免在水面或建筑物上种植 + if (self.config.water_level > 0 and z_base < self.config.water_level + 1): + continue + + # 随机树木高度 + tree_height = random.uniform(5, 15) + self.add_tree_obstacle((x, y), tree_height) + + def export_environment(self, filepath: str): + """导出环境数据""" + export_data = { + 'config': { + 'width': self.config.width, + 'length': self.config.length, + 'height_range': self.config.height_range, + 'resolution': self.config.resolution, + 'water_level': self.config.water_level + }, + 'terrain': { + 'heightmap': self.heightmap.tolist() if self.heightmap is not None else [], + 'mesh': { + 'vertices': self.terrain_mesh['vertices'].tolist() if self.terrain_mesh else [], + 'faces': self.terrain_mesh['faces'].tolist() if self.terrain_mesh else [] + } + }, + 'obstacles': [ + { + 'type': obs.get('type', 'building'), + 'vertices': obs['vertices'].tolist(), + 'faces': obs['faces'].tolist(), + 'centroid': obs.get('centroid', []), + 'bounds': obs.get('bounds', []) + } for obs in self.obstacles_mesh + ], + 'water': { + 'vertices': self.water_mesh['vertices'].tolist() if self.water_mesh else [], + 'faces': self.water_mesh['faces'].tolist() if self.water_mesh else [] + } if self.water_mesh else None + } + + with open(filepath, 'w', encoding='utf-8') as f: + json.dump(export_data, f, indent=2, ensure_ascii=False) + + print(f"环境数据已导出到: {filepath}") + +class Drone3DModel: + """无人机3D模型""" + + def __init__(self, model_type: str = "quadcopter", scale: float = 1.0): + """ + 初始化无人机模型 + + Args: + model_type: 模型类型 ('quadcopter', 'fixed_wing') + scale: 模型缩放比例 + """ + self.model_type = model_type + self.scale = scale + self.position = np.array([0.0, 0.0, 0.0]) + self.orientation = np.array([0.0, 0.0, 0.0]) # 欧拉角 (roll, pitch, yaw) + + # 创建模型网格 + self.mesh = self._create_model_mesh() + + def _create_model_mesh(self): + """创建无人机模型网格""" + if self.model_type == "quadcopter": + return self._create_quadcopter_mesh() + elif self.model_type == "fixed_wing": + return self._create_fixed_wing_mesh() + else: + return self._create_basic_drone_mesh() + + def _create_quadcopter_mesh(self): + """创建四旋翼无人机模型""" + vertices = [] + faces = [] + + # 机身(中心立方体) + body_size = 0.3 * self.scale + half_size = body_size / 2 + + # 机身顶点 + body_vertices = [ + [-half_size, -half_size, -half_size], + [half_size, -half_size, -half_size], + [half_size, half_size, -half_size], + [-half_size, half_size, -half_size], + [-half_size, -half_size, half_size], + [half_size, -half_size, half_size], + [half_size, half_size, half_size], + [-half_size, half_size, half_size] + ] + + # 机身面 + body_faces = [ + [0, 1, 2], [0, 2, 3], # 底面 + [4, 5, 6], [4, 6, 7], # 顶面 + [0, 1, 5], [0, 5, 4], # 前面 + [2, 3, 7], [2, 7, 6], # 后面 + [1, 2, 6], [1, 6, 5], # 右面 + [0, 3, 7], [0, 7, 4] # 左面 + ] + + vertices.extend(body_vertices) + face_offset = len(vertices) - 8 + faces.extend([[v + face_offset for v in face] for face in body_faces]) + + # 四个旋翼臂 + arm_length = 0.8 * self.scale + arm_radius = 0.05 * self.scale + + arm_positions = [ + [arm_length, 0, 0], # 右 + [-arm_length, 0, 0], # 左 + [0, arm_length, 0], # 前 + [0, -arm_length, 0] # 后 + ] + + for i, (dx, dy, dz) in enumerate(arm_positions): + # 旋翼臂(圆柱体简化) + arm_vertices = [ + [dx - arm_radius, dy - arm_radius, dz], + [dx + arm_radius, dy - arm_radius, dz], + [dx + arm_radius, dy + arm_radius, dz], + [dx - arm_radius, dy + arm_radius, dz], + [0 - arm_radius, 0 - arm_radius, 0], + [0 + arm_radius, 0 - arm_radius, 0], + [0 + arm_radius, 0 + arm_radius, 0], + [0 - arm_radius, 0 + arm_radius, 0] + ] + + arm_faces = [ + [0, 1, 5], [0, 5, 4], + [1, 2, 6], [1, 6, 5], + [2, 3, 7], [2, 7, 6], + [3, 0, 4], [3, 4, 7] + ] + + vertices.extend(arm_vertices) + face_offset = len(vertices) - 8 + faces.extend([[v + face_offset for v in face] for face in arm_faces]) + + # 旋翼(圆形) + rotor_radius = 0.2 * self.scale + num_segments = 8 + + rotor_center = [dx, dy, dz + 0.1 * self.scale] + rotor_vertices = [rotor_center] + + for j in range(num_segments): + angle = 2 * np.pi * j / num_segments + vx = dx + rotor_radius * np.cos(angle) + vy = dy + rotor_radius * np.sin(angle) + rotor_vertices.append([vx, vy, dz + 0.1 * self.scale]) + + vertices.extend(rotor_vertices) + + # 旋翼面 + center_idx = len(vertices) - num_segments - 1 + for j in range(num_segments): + vertex_idx = center_idx + j + 1 + next_vertex_idx = center_idx + ((j + 1) % num_segments) + 1 + faces.append([center_idx, vertex_idx, next_vertex_idx]) + + return { + 'vertices': np.array(vertices), + 'faces': np.array(faces), + 'color': '#3498db' # 蓝色 + } + + def _create_fixed_wing_mesh(self): + """创建固定翼无人机模型""" + vertices = [] + faces = [] + + # 机身(流线型) + body_length = 1.5 * self.scale + body_radius = 0.1 * self.scale + + # 创建机身顶点(简化) + num_sections = 8 + for i in range(num_sections): + t = i / (num_sections - 1) + z = (t - 0.5) * body_length + radius = body_radius * (1 - abs(t-0.5)*1.5) # 中间粗,两头细 + + for j in range(8): + angle = 2 * np.pi * j / 8 + x = radius * np.cos(angle) + y = radius * np.sin(angle) + vertices.append([x, y, z]) + + # 创建机翼 + wing_span = 2.0 * self.scale + wing_chord = 0.4 * self.scale + + wing_vertices = [ + [-wing_chord/2, -wing_span/2, 0], + [wing_chord/2, -wing_span/2, 0], + [wing_chord/2, wing_span/2, 0], + [-wing_chord/2, wing_span/2, 0] + ] + + wing_faces = [[0, 1, 2], [0, 2, 3]] + + vertices.extend(wing_vertices) + face_offset = len(vertices) - 4 + faces.extend([[v + face_offset for v in face] for face in wing_faces]) + + # 尾翼 + tail_span = 0.6 * self.scale + tail_chord = 0.2 * self.scale + + tail_vertices = [ + [-tail_chord/2, -tail_span/2, -body_length/2], + [tail_chord/2, -tail_span/2, -body_length/2], + [tail_chord/2, tail_span/2, -body_length/2], + [-tail_chord/2, tail_span/2, -body_length/2] + ] + + tail_faces = [[0, 1, 2], [0, 2, 3]] + + vertices.extend(tail_vertices) + face_offset = len(vertices) - 4 + faces.extend([[v + face_offset for v in face] for face in tail_faces]) + + return { + 'vertices': np.array(vertices), + 'faces': np.array(faces), + 'color': '#e74c3c' # 红色 + } + + def _create_basic_drone_mesh(self): + """创建基本无人机模型""" + vertices = [ + [0, 0, 0], + [0.5 * self.scale, 0, 0], + [0, 0.5 * self.scale, 0], + [-0.5 * self.scale, 0, 0], + [0, -0.5 * self.scale, 0], + [0, 0, 0.5 * self.scale] + ] + + faces = [ + [0, 1, 5], + [0, 2, 5], + [0, 3, 5], + [0, 4, 5], + [1, 2, 0], + [2, 3, 0], + [3, 4, 0], + [4, 1, 0] + ] + + return { + 'vertices': np.array(vertices), + 'faces': np.array(faces), + 'color': '#2ecc71' # 绿色 + } + + def update_pose(self, position: np.ndarray, orientation: np.ndarray = None): + """ + 更新无人机位姿 + + Args: + position: 位置 [x, y, z] + orientation: 欧拉角 [roll, pitch, yaw](弧度) + """ + self.position = position + + if orientation is not None: + self.orientation = orientation + + def get_transformed_mesh(self): + """获取变换后的模型网格""" + if self.mesh is None: + return None + + vertices = self.mesh['vertices'].copy() + + # 应用旋转 + roll, pitch, yaw = self.orientation + + # 绕Z轴旋转(偏航) + if yaw != 0: + rotation_z = np.array([ + [np.cos(yaw), -np.sin(yaw), 0], + [np.sin(yaw), np.cos(yaw), 0], + [0, 0, 1] + ]) + vertices = vertices @ rotation_z.T + + # 绕Y轴旋转(俯仰) + if pitch != 0: + rotation_y = np.array([ + [np.cos(pitch), 0, np.sin(pitch)], + [0, 1, 0], + [-np.sin(pitch), 0, np.cos(pitch)] + ]) + vertices = vertices @ rotation_y.T + + # 绕X轴旋转(滚转) + if roll != 0: + rotation_x = np.array([ + [1, 0, 0], + [0, np.cos(roll), -np.sin(roll)], + [0, np.sin(roll), np.cos(roll)] + ]) + vertices = vertices @ rotation_x.T + + # 应用平移 + vertices += self.position + + return { + 'vertices': vertices, + 'faces': self.mesh['faces'].copy(), + 'color': self.mesh['color'] + } + +class Environment3DVisualizer: + """3D环境可视化器""" + + def __init__(self, render_engine: str = "matplotlib"): + """ + 初始化可视化器 + + Args: + render_engine: 渲染引擎 ('matplotlib' 或 'plotly') + """ + self.render_engine = render_engine + + def visualize_terrain(self, terrain_gen: TerrainGenerator, + drones: list = None, + paths: list = None): + """ + 可视化地形环境 + + Args: + terrain_gen: 地形生成器 + drones: 无人机列表 + paths: 路径列表 + """ + if self.render_engine == "matplotlib": + self._visualize_with_matplotlib(terrain_gen, drones, paths) + elif self.render_engine == "plotly": + self._visualize_with_plotly(terrain_gen, drones, paths) + else: + print(f"未知的渲染引擎: {self.render_engine},使用matplotlib") + self._visualize_with_matplotlib(terrain_gen, drones, paths) + + def _visualize_with_matplotlib(self, terrain_gen: TerrainGenerator, + drones: list, + paths: list): + """使用matplotlib进行可视化""" + fig = plt.figure(figsize=(14, 10)) + ax = fig.add_subplot(111, projection='3d') + + # 绘制地形 + if terrain_gen.terrain_mesh: + vertices = terrain_gen.terrain_mesh['vertices'] + faces = terrain_gen.terrain_mesh['faces'] + + # 创建Poly3DCollection + poly3d = [[vertices[face[0]], vertices[face[1]], vertices[face[2]]] + for face in faces[::10]] # 每10个面绘制一个以加速 + + terrain_collection = Poly3DCollection(poly3d, + facecolors='#8B7355', + edgecolors='#654321', + alpha=0.8, + linewidths=0.3) + ax.add_collection3d(terrain_collection) + + # 绘制水面 + if terrain_gen.water_mesh: + vertices = terrain_gen.water_mesh['vertices'] + faces = terrain_gen.water_mesh['faces'] + + poly3d = [[vertices[face[0]], vertices[face[1]], vertices[face[2]]] + for face in faces] + + water_collection = Poly3DCollection(poly3d, + facecolors='#1E90FF', + edgecolors='#1C86EE', + alpha=0.3, + linewidths=0.5) + ax.add_collection3d(water_collection) + + # 绘制障碍物 + for obstacle in terrain_gen.obstacles_mesh: + vertices = obstacle['vertices'] + faces = obstacle['faces'] + + if obstacle.get('type') == 'tree': + color = '#228B22' # 森林绿 + alpha = 0.7 + else: + color = '#A0522D' # 棕色 + alpha = 0.8 + + poly3d = [[vertices[face[0]], vertices[face[1]], vertices[face[2]]] + for face in faces] + + obstacle_collection = Poly3DCollection(poly3d, + facecolors=color, + edgecolors='#654321', + alpha=alpha, + linewidths=0.5) + ax.add_collection3d(obstacle_collection) + + # 绘制无人机 + if drones: + for i, drone in enumerate(drones): + drone_mesh = drone.get_transformed_mesh() + if drone_mesh: + vertices = drone_mesh['vertices'] + faces = drone_mesh['faces'] + color = drone_mesh['color'] + + # 绘制无人机 + for face in faces: + poly = [vertices[face[0]], vertices[face[1]], vertices[face[2]]] + ax.add_collection3d(Poly3DCollection([poly], + facecolors=color, + edgecolors='black', + alpha=1.0, + linewidths=1)) + + # 标记无人机位置 + ax.scatter(drone.position[0], drone.position[1], drone.position[2], + color=color, s=100, marker='^', label=f'无人机 {i+1}') + + # 绘制路径 + if paths: + colors = ['#FF0000', '#00FF00', '#0000FF', '#FF00FF', '#00FFFF'] + for i, path in enumerate(paths): + if len(path) > 1: + color = colors[i % len(colors)] + xs = path[:, 0] + ys = path[:, 1] + zs = path[:, 2] + + ax.plot(xs, ys, zs, color=color, linewidth=2, alpha=0.8, + label=f'路径 {i+1}') + ax.scatter(xs, ys, zs, color=color, s=20, alpha=0.6) + + # 设置坐标轴 + config = terrain_gen.config + ax.set_xlim(0, config.width) + ax.set_ylim(0, config.length) + ax.set_zlim(0, config.height_range + 5) + + ax.set_xlabel('X (米)', fontsize=12) + ax.set_ylabel('Y (米)', fontsize=12) + ax.set_zlabel('Z (米)', fontsize=12) + + ax.set_title('3D环境可视化', fontsize=14, fontweight='bold') + + # 添加图例 + if drones or paths: + ax.legend() + + # 设置视角 + ax.view_init(elev=30, azim=45) + + plt.tight_layout() + plt.show() + + def _visualize_with_plotly(self, terrain_gen: TerrainGenerator, + drones: list, + paths: list): + """使用plotly进行交互式可视化""" + fig = make_subplots(rows=1, cols=1, + specs=[[{'type': 'scene'}]]) + + # 绘制地形 + if terrain_gen.terrain_mesh: + vertices = terrain_gen.terrain_mesh['vertices'] + faces = terrain_gen.terrain_mesh['faces'] + + # 创建三角网格 + x = vertices[:, 0] + y = vertices[:, 1] + z = vertices[:, 2] + + i = faces[:, 0] + j = faces[:, 1] + k = faces[:, 2] + + fig.add_trace(go.Mesh3d( + x=x, y=y, z=z, + i=i, j=j, k=k, + color='#8B7355', + opacity=0.8, + name='地形', + showlegend=True + )) + + # 绘制水面 + if terrain_gen.water_mesh: + vertices = terrain_gen.water_mesh['vertices'] + faces = terrain_gen.water_mesh['faces'] + + x = vertices[:, 0] + y = vertices[:, 1] + z = vertices[:, 2] + + i = faces[:, 0] + j = faces[:, 1] + k = faces[:, 2] + + fig.add_trace(go.Mesh3d( + x=x, y=y, z=z, + i=i, j=j, k=k, + color='#1E90FF', + opacity=0.3, + name='水面', + showlegend=True + )) + + # 绘制障碍物 + for idx, obstacle in enumerate(terrain_gen.obstacles_mesh): + vertices = obstacle['vertices'] + faces = obstacle['faces'] + + if obstacle.get('type') == 'tree': + color = '#228B22' + name = f'树木 {idx+1}' + else: + color = '#A0522D' + name = f'建筑物 {idx+1}' + + x = vertices[:, 0] + y = vertices[:, 1] + z = vertices[:, 2] + + i = faces[:, 0] + j = faces[:, 1] + k = faces[:, 2] + + fig.add_trace(go.Mesh3d( + x=x, y=y, z=z, + i=i, j=j, k=k, + color=color, + opacity=0.7, + name=name, + showlegend=True + )) + + # 绘制无人机 + if drones: + for i, drone in enumerate(drones): + drone_mesh = drone.get_transformed_mesh() + if drone_mesh: + vertices = drone_mesh['vertices'] + faces = drone_mesh['faces'] + color = drone_mesh['color'] + + x = vertices[:, 0] + y = vertices[:, 1] + z = vertices[:, 2] + + i_idx = faces[:, 0] + j_idx = faces[:, 1] + k_idx = faces[:, 2] + + fig.add_trace(go.Mesh3d( + x=x, y=y, z=z, + i=i_idx, j=j_idx, k=k_idx, + color=color, + opacity=1.0, + name=f'无人机 {i+1}', + showlegend=True + )) + + # 绘制路径 + if paths: + colors = ['#FF0000', '#00FF00', '#0000FF', '#FF00FF', '#00FFFF'] + for i, path in enumerate(paths): + if len(path) > 1: + color = colors[i % len(colors)] + + fig.add_trace(go.Scatter3d( + x=path[:, 0], + y=path[:, 1], + z=path[:, 2], + mode='lines+markers', + line=dict(color=color, width=4), + marker=dict(color=color, size=3), + name=f'路径 {i+1}', + showlegend=True + )) + + # 更新布局 + config = terrain_gen.config + fig.update_layout( + scene=dict( + xaxis=dict(title='X (米)', range=[0, config.width]), + yaxis=dict(title='Y (米)', range=[0, config.length]), + zaxis=dict(title='Z (米)', range=[0, config.height_range + 5]), + aspectmode='manual', + aspectratio=dict(x=1, y=config.length/config.width, z=0.5), + camera=dict( + eye=dict(x=1.5, y=1.5, z=1.0) + ) + ), + title=dict( + text='3D环境交互式可视化', + font=dict(size=20, family='Arial', color='black') + ), + showlegend=True, + legend=dict( + x=0.02, + y=0.98, + bgcolor='rgba(255, 255, 255, 0.8)', + bordercolor='black', + borderwidth=1 + ), + width=1000, + height=800 + ) + + fig.show() + + def save_visualization(self, filepath: str): + """保存可视化结果""" + print(f"可视化结果可保存为: {filepath}") + # 在实际使用中,可以根据render_engine保存为不同格式 \ No newline at end of file diff --git a/src/drone_perception/forecast.py b/src/drone_perception/forecast.py index f9b10b7c0b..85a73278d0 100644 --- a/src/drone_perception/forecast.py +++ b/src/drone_perception/forecast.py @@ -79,24 +79,10 @@ def predict_image(model_path, img_path, train_dir, img_size=(128, 128)): transforms.Normalize(mean=[0.485, 0.456, 0.406], std=[0.229, 0.224, 0.225]) ]) - # 检查img_path是文件还是目录 - if os.path.isdir(img_path): - print(f"检测到目录路径: {img_path}") - # 如果是目录,找到目录中的第一个图像文件 - image_files = [f for f in os.listdir(img_path) - if f.lower().endswith(('.jpg', '.jpeg', '.png', '.bmp'))] - if not image_files: - print("错误: 目录中没有找到图像文件!") - return None - # 使用第一个图像文件 - img_path = os.path.join(img_path, image_files[0]) - print(f"使用目录中的第一个图像: {image_files[0]}") - # 加载和预处理图像 try: image = Image.open(img_path).convert('RGB') print(f"成功加载图像: {img_path}") - print(f"图像尺寸: {image.size}") except Exception as e: print(f"加载图像失败: {e}") return None @@ -140,47 +126,31 @@ def main(): train_dir = os.path.join(base_dir, "train") # 要预测的图像路径 - 可以修改为你的测试图像路径 - # 可以选择使用目录或具体图像文件 - test_dir = os.path.join(base_dir, "test", "Fire") # 目录路径 - # 或者直接指定具体图像文件: - # test_image_path = os.path.join(base_dir, "test", "Fire", "具体的图像文件名.jpg") + img_path = os.path.join(base_dir, "test", "Fire", "fi10.jpg") # 示例路径 # 检查路径是否存在 print("=" * 50) print("路径检查:") print(f"模型路径: {model_path}, 存在: {os.path.exists(model_path)}") print(f"训练目录: {train_dir}, 存在: {os.path.exists(train_dir)}") - print(f"测试目录: {test_dir}, 存在: {os.path.exists(test_dir)}") - - # 如果指定的是目录,检查其中是否有图像文件 - if os.path.exists(test_dir) and os.path.isdir(test_dir): - image_files = [f for f in os.listdir(test_dir) - if f.lower().endswith(('.jpg', '.jpeg', '.png', '.bmp'))] - print(f"测试目录中的图像文件: {len(image_files)} 个") - if image_files: - print(f"前几个文件: {image_files[:3]}") # 显示前3个文件 - + print(f"图像路径: {img_path}, 存在: {os.path.exists(img_path)}") print("=" * 50) - if not all([os.path.exists(model_path), os.path.exists(train_dir)]): - print("错误: 模型或训练目录不存在!") - return - - if not os.path.exists(test_dir): - print("错误: 测试路径不存在!") + if not all([os.path.exists(model_path), os.path.exists(train_dir), os.path.exists(img_path)]): + print("错误: 必要的文件或目录不存在!") return # 执行预测 - result = predict_image(model_path, test_dir, train_dir) + result = predict_image(model_path, img_path, train_dir) if result: predicted_class, confidence = result print(f"\n🎯 最终预测: {predicted_class} (置信度: {confidence*100:.2f}%)") -# 批量预测单个目录中的所有图像(不要求子目录结构) -def predict_directory(model_path, directory_path, train_dir, img_size=(128, 128)): +# 批量预测函数 +def batch_predict(model_path, test_dir, train_dir, img_size=(128, 128)): """ - 预测指定目录中的所有图像文件 + 批量预测测试目录中的所有图像 """ device = torch.device("cuda" if torch.cuda.is_available() else "cpu") @@ -204,55 +174,51 @@ def predict_directory(model_path, directory_path, train_dir, img_size=(128, 128) results = [] - # 获取目录中的所有图像文件 - image_files = [f for f in os.listdir(directory_path) - if f.lower().endswith(('.jpg', '.jpeg', '.png', '.bmp'))] - - if not image_files: - print(f"在目录 {directory_path} 中没有找到图像文件!") - return results - - print(f"\n开始批量预测 {len(image_files)} 个图像...") - - for img_name in image_files: - img_path = os.path.join(directory_path, img_name) - - try: - image = Image.open(img_path).convert('RGB') - input_tensor = transform(image).unsqueeze(0).to(device) - - with torch.no_grad(): - outputs = model(input_tensor) - probabilities = torch.nn.functional.softmax(outputs[0], dim=0) - predicted_class_idx = torch.argmax(probabilities).item() - confidence = probabilities[predicted_class_idx].item() - - predicted_class = class_labels[predicted_class_idx] - - results.append({ - 'image_name': img_name, - 'predicted_class': predicted_class, - 'confidence': confidence - }) + # 遍历测试目录 + for class_name in class_labels: + class_dir = os.path.join(test_dir, class_name) + if not os.path.exists(class_dir): + continue - print(f"📸 {img_name}: {predicted_class} (置信度: {confidence*100:.2f}%)") - - except Exception as e: - print(f"处理图像 {img_path} 时出错: {e}") - - # 统计预测结果 + for img_name in os.listdir(class_dir): + if img_name.lower().endswith(('.jpg', '.jpeg', '.png', '.bmp')): + img_path = os.path.join(class_dir, img_name) + + try: + image = Image.open(img_path).convert('RGB') + input_tensor = transform(image).unsqueeze(0).to(device) + + with torch.no_grad(): + outputs = model(input_tensor) + predicted_class_idx = torch.argmax(outputs[0]).item() + confidence = torch.nn.functional.softmax(outputs[0], dim=0)[predicted_class_idx].item() + + predicted_class = class_labels[predicted_class_idx] + is_correct = (predicted_class == class_name) + + results.append({ + 'image_path': img_path, + 'true_class': class_name, + 'predicted_class': predicted_class, + 'confidence': confidence, + 'correct': is_correct + }) + + status = "✅" if is_correct else "❌" + print(f"{status} {img_name}: 真实={class_name}, 预测={predicted_class}, 置信度={confidence:.4f}") + + except Exception as e: + print(f"处理图像 {img_path} 时出错: {e}") + + # 计算准确率 if results: - print(f"\n📊 批量预测完成!") - class_counts = {} - for result in results: - cls = result['predicted_class'] - class_counts[cls] = class_counts.get(cls, 0) + 1 - - print("预测结果统计:") - for cls, count in class_counts.items(): - print(f" {cls}: {count} 个图像") + correct_predictions = sum(1 for r in results if r['correct']) + accuracy = correct_predictions / len(results) + print(f"\n📊 批量预测准确率: {accuracy:.4f} ({correct_predictions}/{len(results)})") return results if __name__ == "__main__": - main() \ No newline at end of file + main() + + diff --git a/src/drone_perception/main.py b/src/drone_perception/main.py index 86e32bc20f..134b9adf35 100644 --- a/src/drone_perception/main.py +++ b/src/drone_perception/main.py @@ -1,242 +1,742 @@ -import os -import sys - -# 获取当前脚本所在的目录 -if hasattr(sys, '_MEIPASS'): - # 如果是打包后的exe,使用临时解压目录 - current_dir = sys._MEIPASS -else: - # 否则使用脚本所在目录 - current_dir = os.path.dirname(os.path.abspath(__file__)) - -# 设置工作目录为脚本所在目录 -os.chdir(current_dir) -import os -import numpy as np -from PIL import Image -import torch -import torch.nn as nn -from torch.utils.data import Dataset, DataLoader -from torchvision import transforms, models -from sklearn.metrics import accuracy_score -import matplotlib.pyplot as plt - -# 导入其他模块的功能 -from Data_classfication import split_dataset -from image_classification import ImageDataset, ImageClassifier -from visual_navigation import main as run_visual_navigation -from forecast import predict_image, batch_predict - -# 路径设置 -base_dir = "data" -train_dir = os.path.join(base_dir, "train") -test_dir = os.path.join(base_dir, "test") -dataset_dir = os.path.join(base_dir, "dataset") - -def setup_directories(): - """设置数据目录""" - print("=" * 50) - print("设置数据目录...") - - # 检查并创建目录 - os.makedirs(base_dir, exist_ok=True) - os.makedirs(train_dir, exist_ok=True) - os.makedirs(test_dir, exist_ok=True) - - # 检查是否需要分割数据集 - if not os.path.exists(train_dir) or not os.listdir(train_dir): - print("训练集不存在或为空,开始自动分割数据集...") - if os.path.exists(dataset_dir): - success = split_dataset(dataset_dir, train_dir, test_dir, split_ratio=0.8) - if not success: - print("❌ 数据集分割失败,请检查原始数据集路径") - return False - else: - print(f"❌ 原始数据集路径不存在: {dataset_dir}") - print("请将数据集放入 ./data/dataset/ 目录") - print("数据集结构应为:") - print("data/dataset/") - print("├── 类别1/") - print("│ ├── image1.jpg") - print("│ └── image2.jpg") - print("├── 类别2/") - print("│ ├── image1.jpg") - print("│ └── image2.jpg") - print("└── ...") - return False - else: - print("✅ 训练集已存在,跳过数据集分割步骤") - - return True - -def train_pytorch_model(): - """使用PyTorch训练模型""" - print("\n" + "=" * 50) - print("开始PyTorch模型训练...") - - # 参数配置 - img_size = (128, 128) - batch_size = 32 - epochs = 70 - - # 设置设备 - device = torch.device("cuda" if torch.cuda.is_available() else "cpu") - print(f"使用设备: {device}") - - # 数据预处理 - train_transform = transforms.Compose([ - transforms.Resize(img_size), - transforms.RandomRotation(30), - transforms.RandomHorizontalFlip(p=0.5), - transforms.RandomAffine(degrees=0, translate=(0.1, 0.1), shear=0.2), - transforms.ColorJitter(brightness=0.2, contrast=0.2), - transforms.ToTensor(), - transforms.Normalize(mean=[0.485, 0.456, 0.406], std=[0.229, 0.224, 0.225]) - ]) - - test_transform = transforms.Compose([ - transforms.Resize(img_size), - transforms.ToTensor(), - transforms.Normalize(mean=[0.485, 0.456, 0.406], std=[0.229, 0.224, 0.225]) - ]) - - # 创建数据集 - train_dataset = ImageDataset(train_dir, transform=train_transform) - test_dataset = ImageDataset(test_dir, transform=test_transform) - - if len(train_dataset) == 0: - print("❌ 训练集为空,无法训练模型") - return None, [], [] - - num_classes = len(train_dataset.class_to_idx) - print(f"检测到 {num_classes} 个类别: {train_dataset.class_to_idx}") - - # 创建数据加载器 - train_loader = DataLoader(train_dataset, batch_size=batch_size, shuffle=True) - test_loader = DataLoader(test_dataset, batch_size=batch_size, shuffle=False) - - # 初始化模型 - model = ImageClassifier(num_classes=num_classes).to(device) - - # 定义损失函数和优化器 - criterion = nn.CrossEntropyLoss() - optimizer = optim.Adam(model.parameters(), lr=0.001) - scheduler = optim.lr_scheduler.StepLR(optimizer, step_size=10, gamma=0.1) - - # 训练模型 - best_accuracy = 0.0 - train_losses = [] - val_accuracies = [] - - for epoch in range(epochs): - model.train() - running_loss = 0.0 - - for images, labels in train_loader: - images, labels = images.to(device), labels.to(device) - - outputs = model(images) - loss = criterion(outputs, labels) - - optimizer.zero_grad() - loss.backward() - optimizer.step() - - running_loss += loss.item() * images.size(0) - - epoch_loss = running_loss / len(train_loader.dataset) - train_losses.append(epoch_loss) - - # 验证 - model.eval() - all_preds = [] - all_labels = [] - - with torch.no_grad(): - for images, labels in test_loader: - images, labels = images.to(device), labels.to(device) - outputs = model(images) - _, preds = torch.max(outputs, 1) - all_preds.extend(preds.cpu().numpy()) - all_labels.extend(labels.cpu().numpy()) - - accuracy = accuracy_score(all_labels, all_preds) - val_accuracies.append(accuracy) - - print(f'Epoch [{epoch+1}/{epochs}], Loss: {epoch_loss:.4f}, Accuracy: {accuracy:.4f}') - - if accuracy > best_accuracy: - best_accuracy = accuracy - torch.save(model.state_dict(), os.path.join(base_dir, "best_model.pth")) - print(f"✅ 保存最佳模型,准确率: {accuracy:.4f}") - - scheduler.step() - - # 保存最终模型 - torch.save(model.state_dict(), os.path.join(base_dir, "final_model.pth")) - print("✅ 最终模型已保存") - - # 绘制训练曲线 - plt.figure(figsize=(12, 4)) - plt.subplot(1, 2, 1) - plt.plot(train_losses) - plt.title('Training Loss') - plt.xlabel('Epoch') - plt.ylabel('Loss') - - plt.subplot(1, 2, 2) - plt.plot(val_accuracies) - plt.title('Validation Accuracy') - plt.xlabel('Epoch') - plt.ylabel('Accuracy') - - plt.tight_layout() - plt.savefig(os.path.join(base_dir, "training_plot.png")) - plt.show() - - return model, train_losses, val_accuracies - -def main(): - """主函数""" - print("🚀 开始图像分类系统...") - # 1. 设置数据目录 - if not setup_directories(): - return - - # 2. 训练PyTorch模型 - model, train_losses, val_accuracies = train_pytorch_model() - - if model is None: - print("❌ 模型训练失败") - return - - # 3. 提供预测功能 - print("\n" + "=" * 50) - choice = input("是否进行图像预测?(y/n): ") - if choice.lower() == 'y': - test_image_path = input("请输入测试图像路径: ") - if os.path.exists(test_image_path): - result = predict_image( - os.path.join(base_dir, "best_model.pth"), - test_image_path, - train_dir - ) - else: - print("❌ 指定的路径不存在") - - # 4. 启动视觉导航(可选) - print("\n" + "=" * 50) - choice = input("是否启动视觉导航系统?(y/n): ") - if choice.lower() == 'y': - try: - from visual_navigation import main as nav_main - nav_main() - except ImportError as e: - print(f"❌ 无法启动视觉导航: {e}") - - print("\n🎉 所有任务完成!") - -if __name__ == "__main__": - # 添加必要的导入 - import torch.optim as optim - main() \ No newline at end of file +import os +import sys +import io + +# 设置UTF-8编码 +sys.stdout = io.TextIOWrapper(sys.stdout.buffer, encoding='utf-8') +sys.stderr = io.TextIOWrapper(sys.stderr.buffer, encoding='utf-8') + +# 将当前目录添加到系统路径 +sys.path.insert(0, os.path.dirname(os.path.abspath(__file__))) + +import numpy as np +import time +import torch +import torch.nn as nn +from torch.utils.data import DataLoader +from torchvision import transforms +import matplotlib.pyplot as plt +from datetime import datetime + +# 导入自定义模块 +try: + from Data_classfication import split_dataset + from image_classification import ImageDataset, ImageClassifier + from visual_navigation import main as run_visual_navigation + from forecast import predict_image, batch_predict + from path_planning import DynamicPathPlanner, PathFollower, Node, Obstacle + from environment_3d import (TerrainGenerator, TerrainConfig, + Environment3DVisualizer, Drone3DModel) +except ImportError as e: + print(f"导入模块时出错: {e}") + print("请确保所有模块文件都在同一目录下") + sys.exit(1) + +# 路径设置 +base_dir = "data" +train_dir = os.path.join(base_dir, "train") +test_dir = os.path.join(base_dir, "test") +dataset_dir = os.path.join(base_dir, "dataset") + +# 获取当前目录 +current_dir = os.getcwd() +# 日志系统 +class Logger: + """日志记录器""" + + def __init__(self, log_file="drone_system.log"): + self.log_file = log_file + self.logs = [] + + def log(self, message, level="INFO"): + """记录日志""" + timestamp = datetime.now().strftime("%Y-%m-%d %H:%M:%S") + log_entry = f"[{timestamp}] [{level}] {message}" + + print(log_entry) + self.logs.append(log_entry) + + # 写入文件 - 修复编码问题 + try: + with open(self.log_file, 'a', encoding='utf-8', errors='ignore') as f: + f.write(log_entry + '\n') + except Exception as e: + print(f"写入日志文件失败: {e}") + + def info(self, message): + """信息级别日志""" + self.log(message, "INFO") + + def warning(self, message): + """警告级别日志""" + self.log(message, "WARNING") + + def error(self, message): + """错误级别日志""" + self.log(message, "ERROR") + + def success(self, message): + """成功级别日志""" + self.log(message, "SUCCESS") + +# ==================== 初始化日志 ==================== +logger = Logger() + +def setup_directories(): + """设置数据目录""" + logger.info("=" * 50) + logger.info("设置数据目录...") + + # 检查并创建目录 + os.makedirs(base_dir, exist_ok=True) + os.makedirs(train_dir, exist_ok=True) + os.makedirs(test_dir, exist_ok=True) + + # 创建新增功能目录 + os.makedirs(os.path.join(base_dir, "paths"), exist_ok=True) + os.makedirs(os.path.join(base_dir, "environments"), exist_ok=True) + os.makedirs(os.path.join(base_dir, "logs"), exist_ok=True) + + # 检查是否需要分割数据集 + if not os.path.exists(train_dir) or not os.listdir(train_dir): + logger.info("训练集不存在或为空,开始自动分割数据集...") + if os.path.exists(dataset_dir): + success = split_dataset(dataset_dir, train_dir, test_dir, split_ratio=0.8) + if not success: + logger.error("数据集分割失败,请检查原始数据集路径") + return False + else: + logger.error(f"原始数据集路径不存在: {dataset_dir}") + logger.info("请将数据集放入 ./data/dataset/ 目录") + logger.info("数据集结构应为:") + logger.info("data/dataset/") + logger.info("├── 类别1/") + logger.info("│ ├── image1.jpg") + logger.info("│ └── image2.jpg") + logger.info("├── 类别2/") + logger.info("│ ├── image1.jpg") + logger.info("│ └── image2.jpg") + logger.info("└── ...") + return False + else: + logger.success("训练集已存在,跳过数据集分割步骤") + + return True + +def train_pytorch_model(): + """使用PyTorch训练模型""" + logger.info("\n" + "=" * 50) + logger.info("开始PyTorch模型训练...") + + # 参数配置 + img_size = (128, 128) + batch_size = 32 + epochs = 70 + + # 设置设备 + device = torch.device("cuda" if torch.cuda.is_available() else "cpu") + logger.info(f"使用设备: {device}") + + # 数据预处理 + train_transform = transforms.Compose([ + transforms.Resize(img_size), + transforms.RandomRotation(30), + transforms.RandomHorizontalFlip(p=0.5), + transforms.RandomAffine(degrees=0, translate=(0.1, 0.1), shear=0.2), + transforms.ColorJitter(brightness=0.2, contrast=0.2), + transforms.ToTensor(), + transforms.Normalize(mean=[0.485, 0.456, 0.406], std=[0.229, 0.224, 0.225]) + ]) + + test_transform = transforms.Compose([ + transforms.Resize(img_size), + transforms.ToTensor(), + transforms.Normalize(mean=[0.485, 0.456, 0.406], std=[0.229, 0.224, 0.225]) + ]) + + # 创建数据集 + train_dataset = ImageDataset(train_dir, transform=train_transform) + test_dataset = ImageDataset(test_dir, transform=test_transform) + + if len(train_dataset) == 0: + logger.error("训练集为空,无法训练模型") + return None, [], [] + + num_classes = len(train_dataset.class_to_idx) + logger.info(f"检测到 {num_classes} 个类别: {train_dataset.class_to_idx}") + + # 创建数据加载器 + train_loader = DataLoader(train_dataset, batch_size=batch_size, shuffle=True) + test_loader = DataLoader(test_dataset, batch_size=batch_size, shuffle=False) + + # 初始化模型 + model = ImageClassifier(num_classes=num_classes).to(device) + + # 定义损失函数和优化器 + criterion = nn.CrossEntropyLoss() + import torch.optim as optim + optimizer = optim.Adam(model.parameters(), lr=0.001) + scheduler = optim.lr_scheduler.StepLR(optimizer, step_size=10, gamma=0.1) + + # 训练模型 + best_accuracy = 0.0 + train_losses = [] + val_accuracies = [] + + for epoch in range(epochs): + model.train() + running_loss = 0.0 + + for images, labels in train_loader: + images, labels = images.to(device), labels.to(device) + + outputs = model(images) + loss = criterion(outputs, labels) + + optimizer.zero_grad() + loss.backward() + optimizer.step() + + running_loss += loss.item() * images.size(0) + + epoch_loss = running_loss / len(train_loader.dataset) + train_losses.append(epoch_loss) + + # 验证 + model.eval() + all_preds = [] + all_labels = [] + + with torch.no_grad(): + for images, labels in test_loader: + images, labels = images.to(device), labels.to(device) + outputs = model(images) + _, preds = torch.max(outputs, 1) + all_preds.extend(preds.cpu().numpy()) + all_labels.extend(labels.cpu().numpy()) + + from sklearn.metrics import accuracy_score + accuracy = accuracy_score(all_labels, all_preds) + val_accuracies.append(accuracy) + + logger.info(f'Epoch [{epoch+1}/{epochs}], Loss: {epoch_loss:.4f}, Accuracy: {accuracy:.4f}') + + if accuracy > best_accuracy: + best_accuracy = accuracy + torch.save(model.state_dict(), os.path.join(base_dir, "best_model.pth")) + logger.success(f"保存最佳模型,准确率: {accuracy:.4f}") + + scheduler.step() + + # 保存最终模型 + torch.save(model.state_dict(), os.path.join(base_dir, "final_model.pth")) + logger.success("最终模型已保存") + + # 绘制训练曲线 + plt.figure(figsize=(12, 4)) + plt.subplot(1, 2, 1) + plt.plot(train_losses) + plt.title('Training Loss') + plt.xlabel('Epoch') + plt.ylabel('Loss') + + plt.subplot(1, 2, 2) + plt.plot(val_accuracies) + plt.title('Validation Accuracy') + plt.xlabel('Epoch') + plt.ylabel('Accuracy') + + plt.tight_layout() + plt.savefig(os.path.join(base_dir, "training_plot.png")) + logger.success("训练曲线图已保存") + plt.show() + + return model, train_losses, val_accuracies + +def run_path_planning_demo(): + """运行路径规划演示""" + logger.info("\n" + "=" * 50) + logger.info("开始路径规划演示...") + + try: + # 创建路径规划器 + planner = DynamicPathPlanner(grid_size=0.5, safety_margin=0.5) + + # 设置环境边界 (x, y, z) + planner.set_environment_bounds([0, 0, 0], [50, 50, 20]) + + # 添加障碍物 + obstacles = [ + Obstacle([10, 10, 5], radius=2.0, height=10), + Obstacle([25, 20, 3], radius=3.0, height=6), + Obstacle([40, 35, 4], radius=2.5, height=8), + Obstacle([15, 40, 2], radius=1.5, height=4) + ] + + for obs in obstacles: + planner.add_obstacle(obs) + logger.info(f"添加障碍物: 位置({obs.position[0]}, {obs.position[1]}, {obs.position[2]}), " + f"半径{obs.radius}米, 高度{obs.height}米") + + # 定义起点和终点 + start = Node(5, 5, 3) + goal = Node(45, 45, 5) + + logger.info(f"起点: ({start.x}, {start.y}, {start.z})") + logger.info(f"终点: ({goal.x}, {goal.y}, {goal.z})") + + # 执行路径规划 + logger.info("正在规划路径...") + path = planner.hybrid_plan(start, goal) + + if path: + logger.success("路径规划成功!") + + # 计算路径长度 + path_length = 0 + for i in range(len(path) - 1): + path_length += path[i].distance_to(path[i+1]) + logger.info(f"路径节点数: {len(path)}") + logger.info(f"路径长度: {path_length:.2f} 米") + + # 可视化结果 + planner.visualize_path(path, "无人机路径规划结果") + + # 导出路径 + path_file = os.path.join(base_dir, "paths", f"path_{int(time.time())}.json") + planner.export_path_to_json(path, path_file) + + # 创建路径跟随器并模拟 + follower = PathFollower(lookahead_distance=1.5, max_velocity=3.0) + follower.set_path(path) + + logger.info("模拟路径跟随...") + current_pos = np.array([start.x, start.y, start.z]) + current_vel = np.array([0, 0, 0]) + + for step in range(50): + target_pos, completed = follower.get_next_target(current_pos) + + if completed: + logger.success("路径跟随完成!") + break + + desired_vel, desired_acc = follower.compute_control_command( + current_pos, current_vel, target_pos + ) + + # 更新位置(简单积分) + current_vel = desired_vel + current_pos += current_vel * 0.1 + + if step % 10 == 0: + logger.info(f"步骤 {step+1}: 位置 {current_pos.round(2)}, 目标 {target_pos.round(2)}") + + return path, planner + + else: + logger.error("路径规划失败!") + return None, None + + except Exception as e: + logger.error(f"路径规划演示出错: {e}") + import traceback + logger.error(traceback.format_exc()) + return None, None + +def run_3d_environment_demo(): + """运行3D环境创建演示""" + logger.info("\n" + "=" * 50) + logger.info("开始3D环境创建演示...") + + try: + # 创建地形配置 + config = TerrainConfig( + width=80.0, + length=80.0, + height_range=15.0, + resolution=2.0, + seed=int(time.time()), + has_hills=True, + has_valleys=True, + has_river=True, + vegetation_density=0.15, + tree_density=0.03, + water_level=3.0 + ) + + # 创建地形生成器 + terrain_gen = TerrainGenerator(config) + + # 生成地形 + logger.info("正在生成地形...") + heightmap = terrain_gen.generate_terrain() + + # 添加建筑物障碍物 + logger.info("添加障碍物...") + buildings = [ + ([20, 20, 0], [8, 6, 12]), + ([50, 30, 0], [10, 8, 15]), + ([35, 60, 0], [6, 6, 10]), + ] + + for pos, dim in buildings: + terrain_gen.add_building(pos, dim) + logger.info(f" 添加建筑物: 位置{pos}, 尺寸{dim}") + + # 添加树木障碍物 + for i in range(15): + x = np.random.uniform(10, 70) + y = np.random.uniform(10, 70) + terrain_gen.add_tree_obstacle((x, y)) + + logger.info(f"总障碍物数量: {len(terrain_gen.obstacles_mesh)}") + + # 创建无人机模型 + logger.info("创建无人机模型...") + drone1 = Drone3DModel(model_type="quadcopter", scale=1.5) + drone1.update_pose(np.array([10, 10, 5]), np.array([0, 0, 0])) + + drone2 = Drone3DModel(model_type="quadcopter", scale=1.5) + drone2.update_pose(np.array([70, 70, 5]), np.array([0, 0, np.pi])) + + drones = [drone1, drone2] + logger.info(f"创建了 {len(drones)} 架无人机") + + # 创建示例路径 + logger.info("创建示例路径...") + path1 = np.array([ + [10, 10, 8], + [25, 20, 12], + [40, 35, 15], + [55, 50, 12], + [70, 70, 8] + ]) + + path2 = np.array([ + [70, 70, 10], + [55, 55, 14], + [40, 40, 16], + [25, 25, 14], + [10, 10, 10] + ]) + + paths = [path1, path2] + logger.info(f"创建了 {len(paths)} 条路径") + + # 可视化环境 + logger.info("开始3D可视化...") + visualizer = Environment3DVisualizer(render_engine="plotly") + visualizer.visualize_terrain(terrain_gen, drones, paths) + + # 保存可视化结果 + env_file = os.path.join(base_dir, "environments", f"environment_{int(time.time())}.html") + visualizer.save_visualization(env_file) + + # 导出环境数据 + env_data_file = os.path.join(base_dir, "environments", f"environment_data_{int(time.time())}.json") + terrain_gen.export_environment(env_data_file) + + # 显示地形统计信息 + if terrain_gen.terrain_mesh: + vertices = terrain_gen.terrain_mesh.vertices + min_height = vertices[:, 2].min() + max_height = vertices[:, 2].max() + avg_height = vertices[:, 2].mean() + + logger.info("\n地形统计信息:") + logger.info(f" 最低点: {min_height:.2f} 米") + logger.info(f" 最高点: {max_height:.2f} 米") + logger.info(f" 平均高度: {avg_height:.2f} 米") + logger.info(f" 地形范围: {config.width} × {config.length} 米") + logger.info(f" 顶点数量: {len(vertices)}") + + logger.success("3D环境创建完成!") + return terrain_gen, drones, paths, visualizer + + except Exception as e: + logger.error(f"3D环境创建演示出错: {e}") + import traceback + logger.error(traceback.format_exc()) + return None, None, None, None + +def integrated_navigation_demo(): + """集成导航演示:路径规划 + 3D环境 + 图像分类""" + logger.info("\n" + "=" * 50) + logger.info("开始集成导航演示...") + + try: + # 1. 创建3D环境 + logger.info("阶段1: 创建3D导航环境") + terrain_gen, drones, paths, visualizer = run_3d_environment_demo() + + if terrain_gen is None: + logger.error("3D环境创建失败,中止集成演示") + return + + # 2. 在环境中进行路径规划 + logger.info("\n阶段2: 在3D环境中进行路径规划") + + # 从3D环境中提取边界 + config = terrain_gen.config + min_bound = [0, 0, 0] + max_bound = [config.width, config.length, config.height_range] + + # 创建路径规划器 + planner = DynamicPathPlanner(grid_size=2.0, safety_margin=1.0) + planner.set_environment_bounds(min_bound, max_bound) + + # 将3D环境中的障碍物添加到路径规划器 + for obstacle in terrain_gen.obstacles_mesh: + center = obstacle.centroid + bounds = obstacle.bounds + radius = max(bounds[1][0] - bounds[0][0], + bounds[1][1] - bounds[0][1]) / 2 + height = bounds[1][2] - bounds[0][2] + + planner_obstacle = Obstacle(center, radius=radius, height=height) + planner.add_obstacle(planner_obstacle) + + # 规划路径 + start = Node(10, 10, 5) + goal = Node(70, 70, 10) + + logger.info(f"规划从 {start} 到 {goal} 的路径") + path = planner.hybrid_plan(start, goal) + + if path: + logger.success(f"路径规划成功,找到 {len(path)} 个路径点") + + # 将路径转换为numpy数组用于3D可视化 + path_array = np.array([[node.x, node.y, node.z] for node in path]) + + # 更新3D可视化 + logger.info("更新3D可视化显示规划路径...") + visualizer.visualize_terrain(terrain_gen, drones, [path_array]) + + # 3. 模拟无人机沿路径飞行并进行图像分类 + logger.info("\n阶段3: 模拟无人机飞行与实时图像分类") + + # 检查是否有训练好的模型 + model_path = os.path.join(base_dir, "best_model.pth") + if os.path.exists(model_path): + logger.info("检测到训练好的模型,加载模型...") + + # 这里可以集成图像分类功能 + # 在实际系统中,这里会从无人机摄像头获取图像并进行分类 + logger.info("模拟图像分类过程...") + logger.info("飞行过程中检测到: 森林、建筑物、道路等场景") + logger.info("根据场景调整飞行策略...") + + # 模拟根据图像分类结果调整路径 + logger.info("检测到前方有建筑物,调整飞行高度...") + logger.info("检测到森林区域,启用避障模式...") + + else: + logger.warning("未找到训练好的模型,跳过图像分类模拟") + + # 4. 路径跟随模拟 + logger.info("\n阶段4: 路径跟随控制模拟") + follower = PathFollower(lookahead_distance=2.0, max_velocity=2.5) + follower.set_path(path) + + # 模拟飞行过程 + current_pos = np.array([start.x, start.y, start.z]) + logger.info(f"开始飞行,当前位置: {current_pos}") + + for step in range(30): + target_pos, completed = follower.get_next_target(current_pos) + + if completed: + logger.success("到达目的地!") + break + + # 模拟控制指令 + desired_vel = np.array([1.0, 1.0, 0.2]) # 简化控制 + current_pos += desired_vel * 0.1 + + if step % 5 == 0: + distance_to_target = np.linalg.norm(target_pos - current_pos) + logger.info(f"步骤 {step+1}: 位置 {current_pos.round(2)}, " + f"距离目标: {distance_to_target:.2f}米") + + logger.success("集成导航演示完成!") + + else: + logger.error("路径规划失败") + + except Exception as e: + logger.error(f"集成导航演示出错: {e}") + import traceback + logger.error(traceback.format_exc()) + +def display_menu(): + """显示主菜单""" + print("\n" + "=" * 60) + print("🚀 无人机智能导航系统 v2.0") + print("=" * 60) + print("1. 训练图像分类模型") + print("2. 单张图像预测") + print("3. 批量图像预测") + print("4. 启动视觉导航(摄像头实时分类)") + print("5. 路径规划演示") + print("6. 3D环境创建演示") + print("7. 集成导航演示(路径规划 + 3D环境 + 图像分类)") + print("8. 查看系统日志") + print("9. 退出系统") + print("=" * 60) + +def main(): + """主函数""" + logger.info(" 启动无人机智能导航系统...") + logger.info(f"工作目录: {current_dir}") + logger.info(f"数据目录: {base_dir}") + + # 检查CUDA可用性 + if torch.cuda.is_available(): + logger.success(f"检测到CUDA设备: {torch.cuda.get_device_name(0)}") + else: + logger.info("使用CPU进行计算") + + # 1. 设置数据目录 + if not setup_directories(): + logger.error("目录设置失败,程序退出") + return + + # 主循环 + while True: + display_menu() + + try: + choice = input("\n请输入选项编号 (1-9): ").strip() + + if choice == '1': + # 训练图像分类模型 + logger.info("开始训练图像分类模型...") + model, train_losses, val_accuracies = train_pytorch_model() + + if model is None: + logger.error("模型训练失败") + else: + logger.success("模型训练完成!") + + elif choice == '2': + # 单张图像预测 + logger.info("单张图像预测模式") + test_image_path = input("请输入测试图像路径 (或按回车使用默认路径): ").strip() + + if not test_image_path: + # 使用默认测试图像 + default_test_dir = os.path.join(test_dir) + if os.path.exists(default_test_dir): + # 查找第一个图像文件 + for root, dirs, files in os.walk(default_test_dir): + for file in files: + if file.lower().endswith(('.jpg', '.jpeg', '.png')): + test_image_path = os.path.join(root, file) + break + if test_image_path: + break + + if test_image_path and os.path.exists(test_image_path): + model_path = os.path.join(base_dir, "best_model.pth") + + if os.path.exists(model_path): + result = predict_image(model_path, test_image_path, train_dir) + if result: + predicted_class, confidence = result + logger.success(f"预测结果: {predicted_class} (置信度: {confidence*100:.2f}%)") + else: + logger.warning("未找到训练好的模型,请先训练模型 (选项1)") + else: + logger.error(f"图像路径不存在: {test_image_path}") + + elif choice == '3': + # 批量图像预测 + logger.info("批量图像预测模式") + model_path = os.path.join(base_dir, "best_model.pth") + + if os.path.exists(model_path): + results = batch_predict(model_path, test_dir, train_dir) + if results: + logger.success(f"批量预测完成,处理了 {len(results)} 张图像") + else: + logger.warning("未找到训练好的模型,请先训练模型 (选项1)") + + elif choice == '4': + # 启动视觉导航 + logger.info("启动视觉导航系统...") + model_path = os.path.join(base_dir, "best_model.pth") + + if os.path.exists(model_path): + try: + run_visual_navigation() + logger.success("视觉导航完成") + except Exception as e: + logger.error(f"视觉导航出错: {e}") + else: + logger.warning("未找到训练好的模型,请先训练模型 (选项1)") + + elif choice == '5': + # 路径规划演示 + path, planner = run_path_planning_demo() + if path: + logger.success("路径规划演示完成") + + elif choice == '6': + # 3D环境创建演示 + terrain_gen, drones, paths, visualizer = run_3d_environment_demo() + if terrain_gen: + logger.success("3D环境创建演示完成") + + elif choice == '7': + # 集成导航演示 + integrated_navigation_demo() + + elif choice == '8': + # 查看系统日志 + logger.info("系统日志:") + print("\n" + "=" * 60) + print("最近日志记录:") + print("=" * 60) + + # 显示最近10条日志 + recent_logs = logger.logs[-10:] if len(logger.logs) > 10 else logger.logs + for log in recent_logs: + print(log) + + print("=" * 60) + input("\n按回车键继续...") + + elif choice == '9': + # 退出系统 + logger.info("感谢使用无人机智能导航系统!") + print("\n🎉 程序正常退出") + break + + else: + logger.warning(f"无效选项: {choice}") + + # 每次操作后暂停一下 + input("\n按回车键返回主菜单...") + + except KeyboardInterrupt: + logger.warning("用户中断操作") + continue + except Exception as e: + logger.error(f"操作出错: {e}") + import traceback + logger.error(traceback.format_exc()) + input("\n按回车键继续...") + +if __name__ == "__main__": + # 添加必要的导入 + import torch.optim as optim + + try: + main() + except Exception as e: + print(f"程序发生错误: {e}") + import traceback + print(traceback.format_exc()) + input("\n按回车键退出...") \ No newline at end of file diff --git a/src/drone_perception/path_planning.py b/src/drone_perception/path_planning.py new file mode 100644 index 0000000000..9bbe24462c --- /dev/null +++ b/src/drone_perception/path_planning.py @@ -0,0 +1,579 @@ +""" +路径规划模块 +实现动态路径规划、A*算法、RRT算法等 +""" +import numpy as np +import matplotlib.pyplot as plt +import json +import time +import math +import random +from queue import PriorityQueue +from typing import List, Tuple, Optional + +class Node: + """路径节点""" + + def __init__(self, x: float, y: float, z: float, parent=None, cost: float = 0.0, heuristic: float = 0.0): + self.x = x + self.y = y + self.z = z + self.parent = parent + self.cost = cost + self.heuristic = heuristic + + def __eq__(self, other): + if not isinstance(other, Node): + return False + return (abs(self.x - other.x) < 1e-6 and + abs(self.y - other.y) < 1e-6 and + abs(self.z - other.z) < 1e-6) + + def __hash__(self): + return hash((self.x, self.y, self.z)) + + def __lt__(self, other): + return (self.cost + self.heuristic) < (other.cost + other.heuristic) + + def distance_to(self, other: 'Node') -> float: + """计算到另一个节点的欧几里得距离""" + return np.sqrt((self.x - other.x)**2 + + (self.y - other.y)**2 + + (self.z - other.z)**2) + + def to_dict(self): + """转换为字典""" + return { + 'x': self.x, + 'y': self.y, + 'z': self.z, + 'cost': self.cost + } + + def __repr__(self): + return f"Node({self.x:.2f}, {self.y:.2f}, {self.z:.2f})" + +class Obstacle: + """障碍物""" + + def __init__(self, position: List[float], radius: float, height: float): + self.position = position + self.radius = radius + self.height = height + + def contains(self, node: Node) -> bool: + """检查节点是否在障碍物内""" + dx = node.x - self.position[0] + dy = node.y - self.position[1] + dz = node.z - self.position[2] + horizontal_dist = np.sqrt(dx**2 + dy**2) + + if horizontal_dist > self.radius: + return False + + # 检查高度 + if dz < 0 or dz > self.height: + return False + + return True + + def distance_to(self, node: Node) -> float: + """计算到障碍物的最短距离""" + dx = node.x - self.position[0] + dy = node.y - self.position[1] + horizontal_dist = np.sqrt(dx**2 + dy**2) - self.radius + vertical_dist = min(abs(node.z - self.position[2]), + abs(node.z - (self.position[2] + self.height))) + + if horizontal_dist > 0: + return horizontal_dist if vertical_dist <= 0 else np.sqrt(horizontal_dist**2 + vertical_dist**2) + else: + return abs(vertical_dist) + +class DynamicPathPlanner: + """动态路径规划器""" + + def __init__(self, grid_size: float = 0.5, safety_margin: float = 1.0): + """ + 初始化路径规划器 + + Args: + grid_size: 网格大小(米) + safety_margin: 安全边界(米) + """ + self.grid_size = grid_size + self.safety_margin = safety_margin + self.obstacles: List[Obstacle] = [] + self.bounds_min = [0, 0, 0] + self.bounds_max = [100, 100, 50] + + def set_environment_bounds(self, min_bound: List[float], max_bound: List[float]): + """设置环境边界""" + self.bounds_min = min_bound + self.bounds_max = max_bound + + def add_obstacle(self, obstacle: Obstacle): + """添加障碍物""" + self.obstacles.append(obstacle) + + def remove_obstacle(self, index: int): + """移除障碍物""" + if 0 <= index < len(self.obstacles): + self.obstacles.pop(index) + + def is_collision_free(self, node: Node) -> bool: + """检查节点是否无碰撞""" + # 检查边界 + if (node.x < self.bounds_min[0] or node.x > self.bounds_max[0] or + node.y < self.bounds_min[1] or node.y > self.bounds_max[1] or + node.z < self.bounds_min[2] or node.z > self.bounds_max[2]): + return False + + # 检查障碍物 + for obstacle in self.obstacles: + if obstacle.contains(node): + return False + + # 检查安全边界 + if obstacle.distance_to(node) < self.safety_margin: + return False + + return True + + def get_neighbors(self, node: Node) -> List[Node]: + """获取邻居节点""" + neighbors = [] + + # 26方向连接(包括对角线) + for dx in [-1, 0, 1]: + for dy in [-1, 0, 1]: + for dz in [-1, 0, 1]: + if dx == 0 and dy == 0 and dz == 0: + continue + + new_x = node.x + dx * self.grid_size + new_y = node.y + dy * self.grid_size + new_z = node.z + dz * self.grid_size + + neighbor = Node(new_x, new_y, new_z, parent=node) + + # 对角线距离更远 + move_cost = np.sqrt(dx**2 + dy**2 + dz**2) * self.grid_size + neighbor.cost = node.cost + move_cost + + if self.is_collision_free(neighbor): + neighbors.append(neighbor) + + return neighbors + + def heuristic(self, node: Node, goal: Node) -> float: + """启发式函数(欧几里得距离)""" + return node.distance_to(goal) + + def a_star_search(self, start: Node, goal: Node, max_iterations: int = 10000) -> Optional[List[Node]]: + """ + A*搜索算法 + + Args: + start: 起点 + goal: 终点 + max_iterations: 最大迭代次数 + + Returns: + 路径节点列表(从起点到终点) + """ + # 初始化开放集和关闭集 + open_set = PriorityQueue() + closed_set = set() + + # 设置起点的启发值 + start.heuristic = self.heuristic(start, goal) + open_set.put((start.cost + start.heuristic, start)) + + iterations = 0 + + while not open_set.empty() and iterations < max_iterations: + iterations += 1 + + # 获取当前节点 + _, current = open_set.get() + + # 检查是否到达目标 + if current.distance_to(goal) < self.grid_size: + # 重建路径 + path = [] + while current is not None: + path.append(current) + current = current.parent + return list(reversed(path)) + + # 添加到关闭集 + closed_set.add((current.x, current.y, current.z)) + + # 获取邻居 + neighbors = self.get_neighbors(current) + + for neighbor in neighbors: + neighbor_key = (neighbor.x, neighbor.y, neighbor.z) + + if neighbor_key in closed_set: + continue + + # 计算启发值 + neighbor.heuristic = self.heuristic(neighbor, goal) + + # 添加到开放集 + open_set.put((neighbor.cost + neighbor.heuristic, neighbor)) + + return None + + def rrt_plan(self, start: Node, goal: Node, max_iterations: int = 5000, + goal_bias: float = 0.1, step_size: float = 2.0) -> Optional[List[Node]]: + """ + RRT(快速探索随机树)算法 + + Args: + start: 起点 + goal: 终点 + max_iterations: 最大迭代次数 + goal_bias: 目标偏置概率 + step_size: 步长 + + Returns: + 路径节点列表 + """ + # 初始化树 + tree = [start] + + for i in range(max_iterations): + # 随机采样(有一定概率采样到目标点) + if random.random() < goal_bias: + sample = goal + else: + sample = Node( + random.uniform(self.bounds_min[0], self.bounds_max[0]), + random.uniform(self.bounds_min[1], self.bounds_max[1]), + random.uniform(self.bounds_min[2], self.bounds_max[2]) + ) + + # 找到树上最近的节点 + nearest = min(tree, key=lambda n: n.distance_to(sample)) + + # 向采样点移动 + direction = np.array([sample.x - nearest.x, + sample.y - nearest.y, + sample.z - nearest.z]) + direction_norm = np.linalg.norm(direction) + + if direction_norm > 0: + direction = direction / direction_norm + + new_node = Node( + nearest.x + direction[0] * step_size, + nearest.y + direction[1] * step_size, + nearest.z + direction[2] * step_size, + parent=nearest + ) + + # 检查是否无碰撞 + if self.is_collision_free(new_node): + # 检查是否可以直接连接到目标 + if new_node.distance_to(goal) < step_size and self.is_collision_free(goal): + goal.parent = new_node + tree.append(new_node) + tree.append(goal) + + # 重建路径 + path = [] + current = goal + while current is not None: + path.append(current) + current = current.parent + return list(reversed(path)) + + tree.append(new_node) + + return None + + def hybrid_plan(self, start: Node, goal: Node, primary_method: str = 'astar') -> Optional[List[Node]]: + """ + 混合规划算法 + + Args: + start: 起点 + goal: 终点 + primary_method: 主要方法 ('astar' 或 'rrt') + + Returns: + 路径节点列表 + """ + # 首先尝试A*算法 + if primary_method == 'astar': + path = self.a_star_search(start, goal) + if path: + return path + + # 如果A*失败,尝试RRT + print("A*搜索失败,尝试RRT算法...") + return self.rrt_plan(start, goal) + else: + # 首先尝试RRT算法 + path = self.rrt_plan(start, goal) + if path: + return path + + # 如果RRT失败,尝试A* + print("RRT搜索失败,尝试A*算法...") + return self.a_star_search(start, goal) + + def smooth_path(self, path: List[Node], iterations: int = 50) -> List[Node]: + """ + 路径平滑处理 + + Args: + path: 原始路径 + iterations: 平滑迭代次数 + + Returns: + 平滑后的路径 + """ + if len(path) < 3: + return path + + smoothed_path = path.copy() + + for _ in range(iterations): + for i in range(1, len(smoothed_path) - 1): + # 尝试将当前点向相邻两点中点移动 + prev_node = smoothed_path[i-1] + next_node = smoothed_path[i+1] + current_node = smoothed_path[i] + + # 计算中点 + mid_x = (prev_node.x + next_node.x) / 2 + mid_y = (prev_node.y + next_node.y) / 2 + mid_z = (prev_node.z + next_node.z) / 2 + + # 创建新节点 + new_node = Node(mid_x, mid_y, mid_z, parent=current_node.parent) + + # 检查新节点是否无碰撞 + if self.is_collision_free(new_node): + # 检查新路径是否更短且无碰撞 + if (new_node.distance_to(prev_node) + new_node.distance_to(next_node) < + current_node.distance_to(prev_node) + current_node.distance_to(next_node)): + smoothed_path[i] = new_node + + # 更新父节点关系 + for i in range(1, len(smoothed_path)): + smoothed_path[i].parent = smoothed_path[i-1] + + return smoothed_path + + def visualize_path(self, path: List[Node], title: str = "路径规划结果"): + """可视化路径""" + fig = plt.figure(figsize=(12, 10)) + + # 创建3D坐标系 + ax = fig.add_subplot(111, projection='3d') + + # 绘制障碍物 + for obstacle in self.obstacles: + # 绘制圆柱体 + u = np.linspace(0, 2 * np.pi, 50) + v = np.linspace(0, obstacle.height, 10) + u_grid, v_grid = np.meshgrid(u, v) + + X = obstacle.radius * np.cos(u_grid) + obstacle.position[0] + Y = obstacle.radius * np.sin(u_grid) + obstacle.position[1] + Z = v_grid + obstacle.position[2] + + ax.plot_surface(X, Y, Z, alpha=0.3, color='red') + + # 绘制安全边界 + X_safe = (obstacle.radius + self.safety_margin) * np.cos(u) + obstacle.position[0] + Y_safe = (obstacle.radius + self.safety_margin) * np.sin(u) + obstacle.position[1] + + ax.plot(X_safe, Y_safe, obstacle.position[2], 'r--', alpha=0.5, linewidth=0.5) + ax.plot(X_safe, Y_safe, obstacle.position[2] + obstacle.height, 'r--', alpha=0.5, linewidth=0.5) + + # 绘制路径 + if path: + xs = [node.x for node in path] + ys = [node.y for node in path] + zs = [node.z for node in path] + + ax.plot(xs, ys, zs, 'b-', linewidth=2, label='路径') + ax.scatter(xs, ys, zs, c='blue', s=20) + + # 标记起点和终点 + ax.scatter([xs[0]], [ys[0]], [zs[0]], c='green', s=100, marker='o', label='起点') + ax.scatter([xs[-1]], [ys[-1]], [zs[-1]], c='red', s=100, marker='x', label='终点') + + # 设置坐标轴标签 + ax.set_xlabel('X (米)') + ax.set_ylabel('Y (米)') + ax.set_zlabel('Z (米)') + + # 设置标题 + ax.set_title(title) + + # 添加图例 + ax.legend() + + # 设置坐标轴范围 + ax.set_xlim(self.bounds_min[0], self.bounds_max[0]) + ax.set_ylim(self.bounds_min[1], self.bounds_max[1]) + ax.set_zlim(self.bounds_min[2], self.bounds_max[2]) + + # 显示图形 + plt.tight_layout() + plt.show() + + def export_path_to_json(self, path: List[Node], filepath: str): + """将路径导出为JSON文件""" + path_data = { + 'metadata': { + 'grid_size': self.grid_size, + 'safety_margin': self.safety_margin, + 'obstacle_count': len(self.obstacles), + 'generation_time': time.strftime('%Y-%m-%d %H:%M:%S') + }, + 'bounds': { + 'min': self.bounds_min, + 'max': self.bounds_max + }, + 'obstacles': [ + { + 'position': obs.position, + 'radius': obs.radius, + 'height': obs.height + } for obs in self.obstacles + ], + 'path': [node.to_dict() for node in path] + } + + with open(filepath, 'w', encoding='utf-8') as f: + json.dump(path_data, f, indent=2, ensure_ascii=False) + + print(f"路径已导出到: {filepath}") + +class PathFollower: + """路径跟随器""" + + def __init__(self, lookahead_distance: float = 1.0, max_velocity: float = 2.0): + """ + 初始化路径跟随器 + + Args: + lookahead_distance: 前瞻距离(米) + max_velocity: 最大速度(米/秒) + """ + self.lookahead_distance = lookahead_distance + self.max_velocity = max_velocity + self.path = [] + self.current_waypoint_index = 0 + + def set_path(self, path: List[Node]): + """设置路径""" + self.path = path + self.current_waypoint_index = 0 + + def get_next_target(self, current_position: np.ndarray) -> Tuple[np.ndarray, bool]: + """ + 获取下一个目标点 + + Args: + current_position: 当前位置 [x, y, z] + + Returns: + (target_position, completed) - 目标位置和是否完成 + """ + if not self.path or self.current_waypoint_index >= len(self.path): + return current_position, True + + # 找到最近的路点 + min_dist = float('inf') + nearest_index = self.current_waypoint_index + + for i in range(self.current_waypoint_index, len(self.path)): + node = self.path[i] + dist = np.linalg.norm(current_position - np.array([node.x, node.y, node.z])) + + if dist < min_dist: + min_dist = dist + nearest_index = i + + self.current_waypoint_index = nearest_index + + # 寻找前瞻点 + lookahead_index = nearest_index + accumulated_distance = 0.0 + + for i in range(nearest_index, len(self.path) - 1): + node1 = self.path[i] + node2 = self.path[i + 1] + segment_length = node1.distance_to(node2) + + if accumulated_distance + segment_length >= self.lookahead_distance: + # 在这个线段上找到前瞻点 + ratio = (self.lookahead_distance - accumulated_distance) / segment_length + target = Node( + node1.x + ratio * (node2.x - node1.x), + node1.y + ratio * (node2.y - node1.y), + node1.z + ratio * (node2.z - node1.z) + ) + return np.array([target.x, target.y, target.z]), False + + accumulated_distance += segment_length + lookahead_index = i + 1 + + # 如果到达终点 + last_node = self.path[-1] + return np.array([last_node.x, last_node.y, last_node.z]), True + + def compute_control_command(self, current_position: np.ndarray, + current_velocity: np.ndarray, + target_position: np.ndarray) -> Tuple[np.ndarray, np.ndarray]: + """ + 计算控制命令 + + Args: + current_position: 当前位置 + current_velocity: 当前速度 + target_position: 目标位置 + + Returns: + (desired_velocity, desired_acceleration) + """ + # 计算位置误差 + position_error = target_position - current_position + distance = np.linalg.norm(position_error) + + # 计算期望速度(PD控制器) + kp = 2.0 # 比例增益 + kd = 0.5 # 微分增益 + + desired_direction = position_error / distance if distance > 0 else np.zeros(3) + + # 速度限制 + desired_speed = min(self.max_velocity, kp * distance) + desired_velocity = desired_direction * desired_speed + + # 计算期望加速度 + velocity_error = desired_velocity - current_velocity + desired_acceleration = kp * position_error + kd * velocity_error + + # 加速度限制 + max_acceleration = 2.0 + if np.linalg.norm(desired_acceleration) > max_acceleration: + desired_acceleration = desired_acceleration / np.linalg.norm(desired_acceleration) * max_acceleration + + return desired_velocity, desired_acceleration + + def get_progress(self) -> float: + """获取进度百分比""" + if not self.path or len(self.path) < 2: + return 0.0 + + return min(100.0, (self.current_waypoint_index / len(self.path)) * 100) \ No newline at end of file diff --git a/src/enhance_pedestrian_safety/main.py b/src/enhance_pedestrian_safety/main.py index 8d754b0c0d..97d1356ab2 100644 --- a/src/enhance_pedestrian_safety/main.py +++ b/src/enhance_pedestrian_safety/main.py @@ -1,237 +1,698 @@ -# cvips_smart_final.py -""" -CVIPS 智能版本 - 不再关闭运行的CARLA,并修复所有问题 -""" - import sys import os import time -import random # 添加这行 +import random +import argparse import traceback from datetime import datetime +import glob + +print("=" * 80) +print("CVIPS v3.0 - 行人安全数据生成器") +print("=" * 80) + +def find_carla_egg(): + """自动查找CARLA的egg文件""" + common_paths = [ + os.path.expanduser("~/carla/*"), + os.path.expanduser("~/Desktop/carla/*"), + "/opt/carla/*", + os.path.dirname(os.path.abspath(__file__)), + os.path.join(os.path.dirname(os.path.abspath(__file__)), "carla"), + ] -print("=" * 70) -print("CVIPS 数据生成器 - 智能版本") -print("=" * 70) - -# ============================================================ -# 1. 设置CARLA路径 -# ============================================================ -print("\n[1/5] 设置CARLA路径...") -CARLA_EGG = r"D:\carla\carla0914\CARLA_0.9.14\WindowsNoEditor\PythonAPI\carla\dist\carla-0.9.14-py3.7-win-amd64.egg" - -if os.path.exists(CARLA_EGG): - sys.path.append(CARLA_EGG) - print(f"✓ CARLA路径: {os.path.basename(CARLA_EGG)}") + for path in common_paths: + egg_pattern = os.path.join(path, "PythonAPI", "carla", "dist", "carla-*.egg") + egg_files = glob.glob(egg_pattern, recursive=True) + if egg_files: + return egg_files[0] + egg_pattern = os.path.join(path, "dist", "carla-*.egg") + egg_files = glob.glob(egg_pattern, recursive=True) + if egg_files: + return egg_files[0] + return None + +print("\n[1/5] 初始化CARLA环境...") +path_parser = argparse.ArgumentParser(add_help=False) +path_parser.add_argument('--carla-path', type=str, help='CARLA的egg文件路径或dist目录路径') +args_path, remaining_argv = path_parser.parse_known_args() + +carla_egg_path = None +if args_path.carla_path: + if os.path.isfile(args_path.carla_path) and args_path.carla_path.endswith('.egg'): + carla_egg_path = args_path.carla_path + elif os.path.isdir(args_path.carla_path): + egg_files = glob.glob(os.path.join(args_path.carla_path, "carla-*.egg")) + if egg_files: + carla_egg_path = egg_files[0] + if carla_egg_path: + sys.path.append(os.path.dirname(carla_egg_path)) + print(f"✓ 通过命令行参数加载CARLA egg文件: {carla_egg_path}") + else: + print(f"✗ 命令行参数指定的路径中未找到CARLA egg文件: {args_path.carla_path}") + sys.exit(1) +elif os.getenv("CARLA_PYTHON_PATH"): + env_carla_path = os.getenv("CARLA_PYTHON_PATH") + if os.path.isfile(env_carla_path) and env_carla_path.endswith('.egg'): + carla_egg_path = env_carla_path + elif os.path.isdir(env_carla_path): + egg_files = glob.glob(os.path.join(env_carla_path, "carla-*.egg")) + if egg_files: + carla_egg_path = egg_files[0] + if carla_egg_path: + sys.path.append(os.path.dirname(carla_egg_path)) + print(f"✓ 通过环境变量加载CARLA egg文件: {carla_egg_path}") + else: + print(f"✗ 环境变量CARLA_PYTHON_PATH中未找到CARLA egg文件: {env_carla_path}") + sys.exit(1) else: - print(f"✗ 找不到egg文件: {CARLA_EGG}") - sys.exit(1) + carla_egg_path = find_carla_egg() + if carla_egg_path: + sys.path.append(os.path.dirname(carla_egg_path)) + print(f"✓ 自动找到CARLA egg文件: {carla_egg_path}") + else: + print("✗ 未找到CARLA egg文件!") + print("提示:请通过以下方式之一配置CARLA路径:") + print(" 1. 命令行参数:--carla-path ") + print(" 2. 环境变量:设置CARLA_PYTHON_PATH=") + print(" 3. 将CARLA放在用户目录carla/、桌面carla/或/opt/carla/(自动查找)") + sys.exit(1) -# ============================================================ -# 2. 导入CARLA -# ============================================================ print("\n[2/5] 导入CARLA模块...") try: import carla - print("✓ CARLA模块导入成功") except ImportError as e: print(f"✗ 导入失败: {e}") sys.exit(1) +class PedestrianSafetyGenerator: + def __init__(self, args): + self.args = args + self.client = None + self.world = None + self.actors = [] + self.sensors = [] + self.frame_count = 0 + self.camera_last_save = {} + self.setup_output_directory() + + def setup_output_directory(self): + timestamp = datetime.now().strftime("%Y%m%d_%H%M%S") + scenario_name = f"{self.args.scenario}_ped_safety_{timestamp}" + self.output_dir = os.path.join("pedestrian_safety_data", scenario_name) + os.makedirs(self.output_dir, exist_ok=True) + print(f"输出目录: {self.output_dir}") + + def connect_to_server(self): + print("\n[3/5] 连接到CARLA服务器...") + + for attempt in range(1, 6): + try: + print(f" 尝试 {attempt}/5...") + self.client = carla.Client('localhost', 2000) + self.client.set_timeout(15.0) + + if self.args.town: + self.world = self.client.load_world(self.args.town) + else: + self.world = self.client.get_world() + + print(f"✓ 连接成功! 地图: {self.world.get_map().name}") + settings = self.world.get_settings() + settings.synchronous_mode = False + self.world.apply_settings(settings) + + return True + + except Exception as e: + error_msg = str(e) + print(f" 尝试 {attempt} 失败: {error_msg[:80]}...") + if attempt < 5: + print(" 等待3秒后重试...") + time.sleep(3) + + print("✗ 连接失败") + return False + + def setup_pedestrian_safety_scene(self): + print("\n[4/5] 设置行人安全场景...") -# ============================================================ -# 3. 智能连接CARLA服务器 -# ============================================================ -def smart_connect_to_carla(max_retries=10, retry_delay=3): - """智能连接CARLA服务器,不关闭已有服务器""" - print(f"\n[3/5] 连接到CARLA服务器 (最多尝试{max_retries}次)...") - - for attempt in range(1, max_retries + 1): try: - print(f" 尝试 {attempt}/{max_retries}...") + self.set_high_quality_settings() + self.set_pedestrian_friendly_environment() + time.sleep(2.0) - # 创建客户端 - client = carla.Client('localhost', 2000) - client.set_timeout(15.0) + ego_vehicle = self.spawn_ego_vehicle_near_crosswalk() + if not ego_vehicle: + print("⚠ 无法生成主车辆") + return None - # 获取服务器版本 - server_version = client.get_server_version() - print(f" ✓ 连接成功! 服务器版本: {server_version}") + crossing_pedestrians = self.spawn_crossing_pedestrians() + self.spawn_background_traffic() - # 获取世界 - world = client.get_world() - print(f" ✓ 地图: {world.get_map().name}") + print("等待场景稳定...") + time.sleep(5.0) - return client, world + return ego_vehicle except Exception as e: - error_msg = str(e) - print(f" 尝试 {attempt} 失败: {error_msg[:80]}...") - - # 给出具体建议 - if "time-out" in error_msg: - if attempt == 1: - print(f" ℹ 请确保CARLA服务器正在运行") - print(f" ℹ 如果CARLA正在启动中,请等待几秒钟") - elif attempt == 3: - print(f" ℹ 如果CARLA窗口无响应,请尝试在窗口中点击一下") - - if attempt < max_retries: - print(f" 等待 {retry_delay} 秒后重试...") - time.sleep(retry_delay) - else: - print(f"\n✗ 所有连接尝试失败") - print(f"\n请检查:") - print(f"1. CARLA服务器是否正在运行 (应该能看到3D窗口)") - print(f"2. CARLA窗口是否在前台 (尝试点击一下CARLA窗口)") - print(f"3. 如果CARLA刚启动,可能需要更多时间加载") - return None, None - - return None, None - - -# 智能连接 -client, world = smart_connect_to_carla() - -if not client or not world: - print("\n" + "=" * 70) - print("连接失败!") - print("=" * 70) - sys.exit(1) - -# ============================================================ -# 4. 创建简单场景 -# ============================================================ -print("\n[4/5] 创建数据收集场景...") + print(f"设置场景失败: {e}") + traceback.print_exc() + return None -try: - # 创建输出目录 - timestamp = datetime.now().strftime("%Y%m%d_%H%M%S") - output_dir = f"cvips_data/success_{timestamp}" - os.makedirs(output_dir, exist_ok=True) - - print(f"输出目录: {output_dir}") - - # 保存配置 - with open(f"{output_dir}/config.txt", "w") as f: - f.write(f"生成时间: {datetime.now()}\n") - f.write(f"地图: {world.get_map().name}\n") - - # 设置异步模式(更稳定) - settings = world.get_settings() - settings.synchronous_mode = False - world.apply_settings(settings) - - # 设置简单天气 - weather = carla.WeatherParameters( - sun_altitude_angle=90, - cloudiness=0, - precipitation=0, - fog_density=0 - ) - world.set_weather(weather) - print("✓ 天气设置完成") - - # 生成车辆 - blueprint_lib = world.get_blueprint_library() - - # 选择简单车辆 - vehicle_bp = None - vehicle_types = [ - 'vehicle.tesla.model3', - 'vehicle.audi.tt', - 'vehicle.nissan.micra', - 'vehicle.mini.cooperst' - ] + def set_high_quality_settings(self): + try: + print("✓ 应用行人检测优化的渲染设置(部分参数需在CARLA UE4端配置)") + settings = self.world.get_settings() + settings.no_rendering_mode = False + self.world.apply_settings(settings) - for vtype in vehicle_types: - if blueprint_lib.filter(vtype): - vehicle_bp = random.choice(blueprint_lib.filter(vtype)) - break + except Exception as e: + print(f"设置渲染质量失败: {e}") + traceback.print_exc() + + def set_pedestrian_friendly_environment(self): + weather = carla.WeatherParameters() + + if self.args.weather == 'clear': + weather.sun_altitude_angle = 75 + weather.sun_azimuth_angle = 0 + weather.cloudiness = 5.0 + weather.precipitation = 0.0 + weather.precipitation_deposits = 0.0 + weather.wind_intensity = 5.0 + weather.fog_density = 0.0 + weather.wetness = 0.0 + weather.scattering_intensity = 1.0 + weather.mie_scattering_scale = 0.8 + weather.rayleigh_scattering_scale = 1.0 + + elif self.args.weather == 'rainy': + weather.sun_altitude_angle = 40 + weather.cloudiness = 90.0 + weather.precipitation = 60.0 + weather.precipitation_deposits = 50.0 + weather.wind_intensity = 30.0 + weather.fog_density = 15.0 + weather.wetness = 70.0 + weather.scattering_intensity = 1.5 + + elif self.args.weather == 'cloudy': + weather.sun_altitude_angle = 60 + weather.cloudiness = 70.0 + weather.precipitation = 0.0 + weather.wind_intensity = 10.0 + weather.fog_density = 5.0 + weather.wetness = 10.0 + weather.scattering_intensity = 1.2 + + if self.args.time_of_day == 'night': + weather.sun_altitude_angle = -10 + weather.fog_density = 5.0 + weather.wetness = 10.0 + weather.scattering_intensity = 1.8 + elif self.args.time_of_day == 'sunset': + weather.sun_altitude_angle = 5 + weather.cloudiness = 50.0 + weather.fog_density = 10.0 + + self.world.set_weather(weather) + print(f"✓ 行人友好环境设置: {self.args.weather}, {self.args.time_of_day}") + + def find_crosswalk_locations(self): + waypoints = self.world.get_map().generate_waypoints(10.0) + crosswalk_points = [] + + for wp in waypoints: + if wp.is_junction or wp.lane_type == carla.LaneType.Sidewalk: + crosswalk_points.append(wp.transform) + if len(crosswalk_points) >= 3: + break + + if not crosswalk_points: + spawn_points = self.world.get_map().get_spawn_points() + crosswalk_points = spawn_points[:3] + + return crosswalk_points + + def spawn_ego_vehicle_near_crosswalk(self): + blueprint_lib = self.world.get_blueprint_library() + + vehicle_types = [ + 'vehicle.tesla.model3', + 'vehicle.audi.tt', + 'vehicle.mini.cooperst', + 'vehicle.nissan.patrol' + ] + + vehicle_bp = None + for vtype in vehicle_types: + if blueprint_lib.filter(vtype): + vehicle_bp = random.choice(blueprint_lib.filter(vtype)) + break + + if not vehicle_bp: + vehicle_bp = random.choice(blueprint_lib.filter('vehicle.*')) + + crosswalk_points = self.find_crosswalk_locations() + spawn_points = self.world.get_map().get_spawn_points() + + if not spawn_points: + print("⚠ 没有生成点") + return None + + if crosswalk_points: + crosswalk_point = crosswalk_points[0] + nearest_spawn_point = None + min_distance = float('inf') + + for spawn_point in spawn_points: + distance = spawn_point.location.distance(crosswalk_point.location) + if distance < min_distance and distance > 10.0: + min_distance = distance + nearest_spawn_point = spawn_point + + spawn_point = nearest_spawn_point if nearest_spawn_point else random.choice(spawn_points) + else: + spawn_point = random.choice(spawn_points) - if not vehicle_bp: - vehicle_bp = random.choice(blueprint_lib.filter('vehicle.*')) + try: + vehicle = self.world.spawn_actor(vehicle_bp, spawn_point) + self.actors.append(vehicle) - # 获取生成点 - spawn_points = world.get_map().get_spawn_points() - if spawn_points: - spawn_point = random.choice(spawn_points) - print(f"使用生成点: ({spawn_point.location.x:.1f}, {spawn_point.location.y:.1f})") + vehicle.set_autopilot(True) + tm = self.client.get_trafficmanager(8000) + tm.set_desired_speed(vehicle, 10.0) - # 生成车辆 - vehicle = world.spawn_actor(vehicle_bp, spawn_point) - print(f"✓ 生成车辆: {vehicle.type_id}") + print(f"✓ 生成主车辆: {vehicle.type_id}") + print(f" 位置: ({spawn_point.location.x:.1f}, {spawn_point.location.y:.1f})") - # 设置自动驾驶 - vehicle.set_autopilot(True) + return vehicle - # 添加简单摄像头 - camera_bp = blueprint_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') + except Exception as e: + print(f"生成主车辆失败: {e}") + traceback.print_exc() + return None + + def spawn_crossing_pedestrians(self): + blueprint_lib = self.world.get_blueprint_library() + crosswalk_points = self.find_crosswalk_locations() + if not crosswalk_points: + print("⚠ 未找到合适的过马路区域") + return [] + + crossing_pedestrians = [] + num_crossing = min(3, self.args.num_crossing_pedestrians) + + print(f"生成 {num_crossing} 个过马路行人...") + + for i in range(num_crossing): + try: + ped_types = list(blueprint_lib.filter('walker.pedestrian.*')) + if not ped_types: + continue + + ped_bp = random.choice(ped_types) + crosswalk_point = crosswalk_points[i % len(crosswalk_points)] + + offset_x = random.uniform(-1.0, 1.0) + offset_y = random.uniform(-1.0, 1.0) + + location = carla.Location( + x=crosswalk_point.location.x + offset_x, + y=crosswalk_point.location.y + offset_y, + z=crosswalk_point.location.z + 0.1 + ) + + spawn_point = carla.Transform(location) + pedestrian = self.world.try_spawn_actor(ped_bp, spawn_point) + if not pedestrian: + print(f" 行人 {i+1} 生成位置无效,跳过") + continue + + self.actors.append(pedestrian) + + controller_bp = blueprint_lib.find('controller.ai.walker') + if controller_bp: + controller = self.world.spawn_actor( + controller_bp, + carla.Transform(), + attach_to=pedestrian + ) + self.actors.append(controller) + + controller.start() + controller.set_max_speed(1.0) + + wp = self.world.get_map().get_waypoint(pedestrian.get_location()) + if wp: + target_location = wp.transform.location + carla.Location( + x=10 * (-wp.transform.rotation.yaw / 90), + y=10 * (wp.transform.rotation.yaw / 90), + z=wp.transform.location.z + ) + else: + target_location = carla.Location( + x=location.x + random.uniform(10.0, 20.0) * (1 if random.random() > 0.5 else -1), + y=location.y + random.uniform(10.0, 20.0) * (1 if random.random() > 0.5 else -1), + z=location.z + ) + + controller.go_to_location(target_location) + crossing_pedestrians.append(pedestrian) + print(f" 过马路行人 {i + 1} 已生成") + + except Exception as e: + print(f" 生成过马路行人失败: {e}") + traceback.print_exc() + continue + + print(f"✓ 生成 {len(crossing_pedestrians)} 个过马路行人") + return crossing_pedestrians + + def spawn_background_traffic(self): + blueprint_lib = self.world.get_blueprint_library() + + vehicles_spawned = 0 + spawn_points = self.world.get_map().get_spawn_points() + for i in range(min(5, self.args.num_background_vehicles)): + try: + vehicle_bp = random.choice(blueprint_lib.filter('vehicle.*')) + if spawn_points: + spawn_point = random.choice(spawn_points) + vehicle = self.world.try_spawn_actor(vehicle_bp, spawn_point) + if vehicle: + self.actors.append(vehicle) + vehicle.set_autopilot(True) + tm = self.client.get_trafficmanager(8000) + tm.set_desired_speed(vehicle, 15.0) + vehicles_spawned += 1 + except Exception as e: + print(f" 生成背景车辆失败: {e}") + continue + + pedestrians_spawned = 0 + for i in range(min(8, self.args.num_background_pedestrians)): + try: + ped_bp = random.choice(blueprint_lib.filter('walker.pedestrian.*')) + location = self.world.get_random_location_from_navigation() + + if location: + location.z += 0.1 + pedestrian = self.world.try_spawn_actor(ped_bp, carla.Transform(location)) + if pedestrian: + self.actors.append(pedestrian) + + controller_bp = blueprint_lib.find('controller.ai.walker') + controller = self.world.spawn_actor( + controller_bp, + carla.Transform(), + attach_to=pedestrian + ) + controller.start() + controller.set_max_speed(0.8) + + target_location = self.world.get_random_location_from_navigation() + if target_location: + controller.go_to_location(target_location) + else: + controller.go_to_location(location + carla.Location(x=5, y=5)) + + pedestrians_spawned += 1 + self.actors.append(controller) + except Exception as e: + print(f" 生成背景行人失败: {e}") + continue + + print(f"✓ 生成 {vehicles_spawned} 辆背景车辆和 {pedestrians_spawned} 个背景行人") + + def setup_pedestrian_safety_cameras(self, vehicle): + if not vehicle: + return + + blueprint_lib = self.world.get_blueprint_library() + print("\n安装行人安全摄像头系统...") + + camera_configs = [ + ('front_wide', + carla.Location(x=2.0, z=1.8), + carla.Rotation(pitch=-3.0), + 100, + "前视广角摄像头 - 检测前方行人"), + + ('front_narrow', + carla.Location(x=2.0, z=1.6), + carla.Rotation(pitch=0), + 60, + "前视窄角摄像头 - 细节识别"), + + ('right_side', + carla.Location(x=0.5, y=1.0, z=1.5), + carla.Rotation(pitch=-2.0, yaw=45), + 90, + "右侧摄像头 - 检测右侧行人"), + + ('left_side', + carla.Location(x=0.5, y=-1.0, z=1.5), + carla.Rotation(pitch=-2.0, yaw=-45), + 90, + "左侧摄像头 - 检测左侧行人") + ] + + installed_cameras = 0 + + for name, location, rotation, fov, description in camera_configs: + try: + camera_bp = blueprint_lib.find('sensor.camera.rgb') + + camera_bp.set_attribute('image_size_x', '1280') + camera_bp.set_attribute('image_size_y', '720') + camera_bp.set_attribute('fov', str(fov)) + camera_bp.set_attribute('motion_blur_intensity', '0.0') + camera_bp.set_attribute('motion_blur_max_distortion', '0.0') + camera_bp.set_attribute('enable_postprocess_effects', 'False') + camera_bp.set_attribute('gamma', '2.2') + camera_bp.set_attribute('shutter_speed', '100') + camera_bp.set_attribute('iso', '200') + camera_bp.set_attribute('fstop', '2.0') + camera_bp.set_attribute('lens_k', '0.0') + camera_bp.set_attribute('lens_kcube', '0.0') + camera_bp.set_attribute('lens_x_size', '0.08') + camera_bp.set_attribute('lens_y_size', '0.08') + + transform = carla.Transform(location, rotation) + camera = self.world.spawn_actor(camera_bp, transform, attach_to=vehicle) + + camera_dir = os.path.join(self.output_dir, name) + os.makedirs(camera_dir, exist_ok=True) + + self.camera_last_save[name] = time.time() + + def save_image(image, save_dir=camera_dir, cam_name=name): + current_time = time.time() + + if current_time - self.camera_last_save[cam_name] >= self.args.capture_interval: + self.frame_count += 1 + self.camera_last_save[cam_name] = current_time + + filename = f"{save_dir}/ped_frame_{self.frame_count:04d}.png" + image.save_to_disk(filename, carla.ColorConverter.Raw) + + if self.frame_count % 10 == 0: + print(f" [{cam_name}] 保存第 {self.frame_count} 帧") + + camera.listen(save_image) + self.actors.append(camera) + self.sensors.append(camera) + + installed_cameras += 1 + print(f"✓ {description}") + + except Exception as e: + print(f" 安装{name}摄像头失败: {e}") + traceback.print_exc() + continue + + print(f"✓ 总共安装 {installed_cameras} 个行人安全摄像头") + + def collect_pedestrian_safety_data(self): + print("\n[5/5] 开始收集行人安全数据...") + print(f"数据收集模式: 间隔{self.args.capture_interval}秒捕捉") + print(f"预计总时长: {self.args.total_duration}秒") + print(f"预计帧数: {self.args.total_duration // self.args.capture_interval}") + print("\n提示: 正在模拟行人过马路场景...") + print("按 Ctrl+C 提前结束\n") - # 前摄像头 - camera_transform = carla.Transform( - carla.Location(x=1.5, z=1.4), - carla.Rotation(pitch=0, yaw=0, roll=0) - ) - camera = world.spawn_actor(camera_bp, camera_transform, attach_to=vehicle) + start_time = time.time() + self.frame_count = 0 + for cam_name in self.camera_last_save: + self.camera_last_save[cam_name] = start_time - # 图像保存回调 - frame_count = [0] + try: + update_interval = 5.0 + while time.time() - start_time < self.args.total_duration: + elapsed = time.time() - start_time + remaining = max(0, self.args.total_duration - elapsed) - def save_image(image): - frame_count[0] += 1 - if frame_count[0] <= 30: # 只保存30张 - image.save_to_disk(f"{output_dir}/frame_{frame_count[0]:03d}.png") - if frame_count[0] % 10 == 0: - print(f" 已保存 {frame_count[0]}/30 帧") + if int(elapsed) % update_interval == 0 and elapsed % update_interval < 0.1: + progress_percent = (elapsed / self.args.total_duration) * 100 + print(f" 进度: {elapsed:.0f}/{self.args.total_duration}秒 " + f"({progress_percent:.1f}%) | " + f"已保存帧数: {self.frame_count} | " + f"剩余: {remaining:.0f}秒") - camera.listen(save_image) - print("✓ 摄像头已安装") + time.sleep(0.1) - # 收集数据 - print("\n[5/5] 收集数据 (15秒)...") - print("按 Ctrl+C 可提前结束") + elapsed = time.time() - start_time - start_time = time.time() - try: - for i in range(15): - print(f" 进度: {i + 1}/15 秒") - time.sleep(1.0) + print(f"\n✓ 行人安全数据收集完成!") + print(f" 总时长: {elapsed:.1f}秒") + print(f" 保存帧数: {self.frame_count}") + print(f" 实际帧率: {self.frame_count / elapsed if elapsed > 0 else 0:.2f} FPS") - print(f"\n✓ 数据收集完成!") - print(f" 总帧数: {frame_count[0]}") - print(f" 数据保存到: {output_dir}") + self.display_data_summary() except KeyboardInterrupt: - print(f"\n数据收集中断,已保存 {frame_count[0]} 帧") + elapsed = time.time() - start_time + print(f"\n数据收集中断,已收集 {self.frame_count} 帧") + print(f"总时长: {elapsed:.1f}秒") - # 清理 + def display_data_summary(self): + print("\n" + "-" * 60) + print("数据收集摘要:") + print("-" * 60) + + camera_dirs = ['front_wide', 'front_narrow', 'right_side', 'left_side'] + + for cam_dir in camera_dirs: + cam_path = os.path.join(self.output_dir, cam_dir) + if os.path.exists(cam_path): + image_files = [f for f in os.listdir(cam_path) if f.endswith('.png')] + print(f" {cam_dir}: {len(image_files)} 张图像") + + print(f"\n数据目录: {self.output_dir}") + print("建议: 检查图像质量,确保行人清晰可见") + print("-" * 60) + + def cleanup(self): print("\n清理场景...") - camera.stop() - camera.destroy() - vehicle.destroy() - print("✓ 场景已清理") - else: - print("⚠ 没有找到生成点,跳过车辆生成") - -except Exception as e: - print(f"✗ 创建场景时出错: {e}") - traceback.print_exc() - -# ============================================================ -# 完成 -# ============================================================ -print("\n" + "=" * 70) -print("🎉 CVIPS 数据生成完成!") -print("=" * 70) -print(f"CARLA服务器仍在运行,可以继续使用") -print("=" * 70) - -input("\n按Enter键退出...") \ No newline at end of file + destroyed = 0 + for actor in self.actors: + try: + if actor and actor.is_alive: + actor.destroy() + destroyed += 1 + except Exception as e: + print(f" 销毁actor失败: {e}") + continue + + print(f"销毁 {destroyed} 个actor") + self.actors.clear() + self.sensors.clear() + +def main(): + parser = argparse.ArgumentParser( + description='CVIPS v3.0 - 行人安全协同感知数据生成器', + formatter_class=argparse.RawDescriptionHelpFormatter, + parents=[path_parser], + epilog=""" +示例: + # 基本行人安全场景(晴天中午) + python cvips_pedestrian_safety.py + + # 雨天夜晚的行人安全场景 + python cvips_pedestrian_safety.py --weather rainy --time-of-day night --total-duration 120 + + # 自定义场景 + python cvips_pedestrian_safety.py --scenario crosswalk_test --capture-interval 3 --num-crossing-pedestrians 4 + """ + ) + + parser.add_argument('--scenario', type=str, default='pedestrian_crossing', + help='场景名称') + parser.add_argument('--town', type=str, default='Town10HD', + choices=['Town03', 'Town04', 'Town05', 'Town10HD'], + help='CARLA地图(推荐Town10HD行人多)') + + parser.add_argument('--weather', type=str, default='clear', + choices=['clear', 'rainy', 'cloudy'], + help='天气条件(推荐clear)') + parser.add_argument('--time-of-day', type=str, default='noon', + choices=['noon', 'sunset', 'night'], + help='时间(推荐noon)') + + parser.add_argument('--num-crossing-pedestrians', type=int, default=3, + help='过马路行人数(重点行人)') + parser.add_argument('--num-background-pedestrians', type=int, default=6, + help='背景行人数') + parser.add_argument('--num-background-vehicles', type=int, default=4, + help='背景车辆数') + + parser.add_argument('--total-duration', type=int, default=60, + help='总收集时间(秒)') + parser.add_argument('--capture-interval', type=float, default=2.0, + help='图像捕捉间隔(秒) - 建议2.0-5.0秒') + + args = parser.parse_args() + + if args.capture_interval < 1.0: + print("⚠ 警告: 捕捉间隔太短可能导致图像变化不明显,建议使用2.0秒或更长") + args.capture_interval = 2.0 + + print(f"\n配置参数:") + print(f" 场景: {args.scenario}") + print(f" 地图: {args.town}") + print(f" 天气: {args.weather}, 时间: {args.time_of_day}") + print(f" 过马路行人: {args.num_crossing_pedestrians}") + print(f" 总时长: {args.total_duration}秒") + print(f" 捕捉间隔: {args.capture_interval}秒") + print(f" 预计帧数: {args.total_duration // args.capture_interval}") + + generator = PedestrianSafetyGenerator(args) + + try: + if not generator.connect_to_server(): + print("\n连接失败,退出") + return + + ego_vehicle = generator.setup_pedestrian_safety_scene() + + if not ego_vehicle: + print("\n场景设置失败") + generator.cleanup() + return + + generator.setup_pedestrian_safety_cameras(ego_vehicle) + + if not generator.sensors: + print("\n摄像头安装失败") + generator.cleanup() + return + + generator.collect_pedestrian_safety_data() + + except KeyboardInterrupt: + print("\n程序被用户中断") + except Exception as e: + print(f"\n运行出错: {e}") + traceback.print_exc() + finally: + generator.cleanup() + + print("\n" + "=" * 80) + print("行人安全数据收集完成!") + print(f"数据保存到: {generator.output_dir}") + print("=" * 80) + + print("\n使用提示:") + print("1. 检查输出目录中的图像质量") + print("2. 确保行人清晰可见") + print("3. 可调整capture-interval参数控制图像间隔") + print("4. 下次运行可尝试不同天气和时间条件") + +if __name__ == "__main__": + main() \ No newline at end of file diff --git a/src/image_object_detection/camera_detector.py b/src/image_object_detection/camera_detector.py index cb0ad65ca4..466e923f15 100644 --- a/src/image_object_detection/camera_detector.py +++ b/src/image_object_detection/camera_detector.py @@ -1,110 +1,73 @@ # camera_detector.py import cv2 import time -import os import traceback class CameraDetector: - def __init__(self, detection_engine, camera_index=0): + def __init__(self, detection_engine, output_interval=1.0): self.engine = detection_engine - self.camera_index = camera_index - self.window_name = "YOLO 实时检测 - 摄像头" + self.output_interval = output_interval + self.last_output_time = 0 + self.frame_count = 0 + self.window_name = "YOLO_Live_Detection" # 👈 英文窗口名 - def detect_camera(self): - """对外统一接口:启动摄像头实时检测""" - self.start_detection() - - def start_detection(self): - """实际执行摄像头检测的逻辑""" - print("正在初始化摄像头...") - cap = self._open_camera() - if cap is None: + def start_detection(self, camera_index=0): + cap = cv2.VideoCapture(camera_index) + if not cap.isOpened(): + print(f"Error: Cannot open camera {camera_index}") return - cv2.namedWindow(self.window_name, cv2.WINDOW_NORMAL) - cv2.resizeWindow(self.window_name, 800, 600) - - print("摄像头已启动。按 'q' 或 ESC 退出,按 's' 保存当前帧。") - - prev_time = time.time() - saved_frame_index = 0 + print("Starting live detection. Press 'q' to quit, 's' to save frame.") + cv2.namedWindow(self.window_name, cv2.WINDOW_NORMAL) # 创建一次即可 try: while True: ret, frame = cap.read() if not ret: - print("警告: 无法读取摄像头帧(可能设备被拔出或占用)") + print("Warning: Failed to read frame from camera") break - annotated_frame, results = self._perform_detection(frame) - if annotated_frame is None: - annotated_frame = frame + current_time = time.time() + annotated_frame, results = self.engine.detect(frame) - curr_time = time.time() - fps = 1.0 / (curr_time - prev_time) if (curr_time - prev_time) > 0 else 0 - prev_time = curr_time - - num_objects = len(results[0].boxes) if results else 0 - info_text = f"FPS: {fps:.1f} | Objects: {num_objects}" - cv2.putText( - annotated_frame, - info_text, - (10, 30), - cv2.FONT_HERSHEY_SIMPLEX, - 0.7, - (0, 255, 0), - 2 - ) + if annotated_frame is None or annotated_frame.size == 0: + print("Warning: Invalid detection result") + continue cv2.imshow(self.window_name, annotated_frame) key = cv2.waitKey(1) & 0xFF - if key == ord('q') or key == 27: - print("用户请求退出。") + if key == ord('q'): break elif key == ord('s'): - saved_frame_index = self._save_frame(annotated_frame, saved_frame_index) - + self.save_frame(annotated_frame) + + self._print_fps_if_needed(current_time) + self.frame_count += 1 + + except KeyboardInterrupt: + print("\nDetection interrupted by user.") except Exception as e: - print(f"检测过程中发生未预期错误: {e}") + print(f"Unexpected error during detection: {e}") traceback.print_exc() finally: cap.release() cv2.destroyAllWindows() - print("摄像头已关闭,窗口已清理.") - - def _open_camera(self): - cap = cv2.VideoCapture(self.camera_index) - if not cap.isOpened(): - print(f"错误: 无法打开摄像头(索引 {self.camera_index})") - print("请检查摄像头是否连接、是否被其他程序占用。") - return None - - cap.set(cv2.CAP_PROP_FRAME_WIDTH, 1280) - cap.set(cv2.CAP_PROP_FRAME_HEIGHT, 720) - return cap - - def _perform_detection(self, frame): - try: - # 抑制 YOLO 内部输出 - import sys, os - old_stdout = sys.stdout - sys.stdout = open(os.devnull, 'w') - annotated_frame, results = self.engine.detect(frame) - sys.stdout = old_stdout - return annotated_frame, results - except Exception as e: - print(f"单帧检测失败: {e}") - return None, [] - - def _save_frame(self, frame, index): - save_dir = "saved_frames" - os.makedirs(save_dir, exist_ok=True) - filename = os.path.join(save_dir, f"frame_{index:04d}.jpg") + print("Camera released and windows closed.") + + def _print_fps_if_needed(self, current_time): + if current_time - self.last_output_time >= self.output_interval: + fps = self.frame_count / (current_time - self.last_output_time) if self.last_output_time > 0 else 0 + print(f"FPS: {fps:.2f}") + self.last_output_time = current_time + self.frame_count = 0 + + def save_frame(self, frame): + import os + timestamp = int(time.time()) + filename = f"saved_frame_{timestamp}.jpg" success = cv2.imwrite(filename, frame) if success: - print(f"✅ 帧已保存: {filename}") - return index + 1 + print(f"Frame saved as {filename}") else: - print("❌ 保存帧失败!") - return index + print("Failed to save frame") diff --git a/src/image_object_detection/image_detector.py b/src/image_object_detection/image_detector.py index b161b45597..70f31256b2 100644 --- a/src/image_object_detection/image_detector.py +++ b/src/image_object_detection/image_detector.py @@ -2,111 +2,65 @@ import cv2 import numpy as np import traceback +import os class ImageDetector: def __init__(self, detection_engine): self.engine = detection_engine def detect_static_image(self, image_path): - """检测静态图像""" - print(f"正在加载图像: {image_path}") + print(f"Loading image: {image_path}") - if not self._check_image_exists(image_path): + if not os.path.exists(image_path): + print(f"Error: Image file not found - {image_path}") return - frame = self._load_image(image_path) + frame = cv2.imread(image_path) if frame is None or frame.size == 0: - print("错误: 无法读取图像文件或图像为空") + print("Error: Failed to load image") return - print("正在检测图像...") - results, annotated_frame = self._perform_detection(frame) + print("Running detection...") + annotated_frame, results = self.engine.detect(frame) - if annotated_frame is None: - print("错误: 检测未返回有效图像") + if annotated_frame is None or annotated_frame.size == 0: + print("Error: Invalid annotated frame") return - # 显示检测结果文本 self._display_results(results) - - # 显示图像窗口(带固定初始大小) - self._show_image(annotated_frame, 'YOLO 检测结果 - 静态图像') - - def _check_image_exists(self, image_path): - import os - if not os.path.exists(image_path): - print(f"错误: 图像文件不存在 - {image_path}") - return False - return True - - def _load_image(self, image_path): - try: - with open(image_path, "rb") as f: - bytes_data = bytearray(f.read()) - nparr = np.frombuffer(bytes_data, np.uint8) - frame = cv2.imdecode(nparr, cv2.IMREAD_COLOR) - return frame - except Exception as e: - print(f"加载图像时出错: {e}") - return None - - def _perform_detection(self, frame): - try: - annotated_frame, results = self.engine.detect(frame) - return results, annotated_frame - except Exception as e: - print(f"检测过程中发生错误: {e}") - traceback.print_exc() - return [], None + self._show_image(annotated_frame, "YOLO_Static_Detection") def _display_results(self, results): if not results: - print("未检测到任何对象(results 为空)") + print("No objects detected.") return result = results[0] boxes = result.boxes if len(boxes) == 0: - print("未检测到任何对象") + print("No objects detected.") return - print("检测完成,正在显示结果...") - print(f"检测到 {len(boxes)} 个对象:") - + print(f"Detected {len(boxes)} object(s):") names_list = self.engine.model.names for i, box in enumerate(boxes): try: cls_index = int(box.cls.item()) confidence = box.conf.item() - - if 0 <= cls_index < len(names_list): - cls_name = names_list[cls_index] - else: - cls_name = f"unknown_class_{cls_index}" - print(f" ⚠️ 警告:类别索引 {cls_index} 超出范围(共 {len(names_list)} 类)") - - print(f" {i+1}. {cls_name} (置信度: {confidence:.2f})") + cls_name = names_list[cls_index] if 0 <= cls_index < len(names_list) else f"unknown_{cls_index}" + print(f" {i+1}. {cls_name} (confidence: {confidence:.2f})") except Exception as e: - print(f" ⚠️ 解析第 {i+1} 个检测框时出错: {e}") + print(f" Warning: Failed to parse box {i+1}: {e}") - def _show_image(self, annotated_frame, window_name): - """显示图像,并设置窗口为可调整的小尺寸""" - if annotated_frame is None or annotated_frame.size == 0: - print("错误: 标注帧无效,无法显示") - return - - print("\n图像已显示。按任意键关闭窗口...") + def _show_image(self, annotated_frame, window_name="YOLO_Static_Detection"): try: - # 创建可调整大小的窗口 + cv2.destroyAllWindows() cv2.namedWindow(window_name, cv2.WINDOW_NORMAL) - # 设置初始窗口大小为 800x600(你可以按需修改) cv2.resizeWindow(window_name, 800, 600) - # 显示图像(OpenCV 会自动适应窗口) cv2.imshow(window_name, annotated_frame) cv2.waitKey(0) cv2.destroyAllWindows() - print("窗口已关闭。") except Exception as e: - print(f"显示图像时发生错误: {e}") + print(f"Failed to display image: {e}") traceback.print_exc() diff --git a/src/image_object_detection/main.py b/src/image_object_detection/main.py index 6c1bda8efd..dc6d559477 100644 --- a/src/image_object_detection/main.py +++ b/src/image_object_detection/main.py @@ -1,4 +1,7 @@ # main.py +import matplotlib +matplotlib.use('Agg') # 必须在最前,禁用 GUI 后端 + from ui_handler import UIHandler from config import Config @@ -8,4 +11,4 @@ def main(): ui_handler.run() if __name__ == "__main__": - main() \ No newline at end of file + main() diff --git a/src/image_object_detection/ui_handler.py b/src/image_object_detection/ui_handler.py index 53e4036e0d..1e3f1073c3 100644 --- a/src/image_object_detection/ui_handler.py +++ b/src/image_object_detection/ui_handler.py @@ -1,75 +1,49 @@ # ui_handler.py +import os import cv2 +import traceback from detection_engine import DetectionEngine from image_detector import ImageDetector from camera_detector import CameraDetector -import sys -import os class UIHandler: def __init__(self, config): self.config = config - self.detection_engine = DetectionEngine() - self.image_detector = ImageDetector(self.detection_engine) - self.camera_detector = CameraDetector(self.detection_engine) + self.engine = DetectionEngine( + model_path=config.model_path, + conf_threshold=config.confidence_threshold + ) + self.stop_flag = False - def print_debug(self, message): - """调试输出函数""" - print(f"[DEBUG] {message}") - sys.stdout.flush() # 强制刷新输出缓冲区 - - def display_menu(self): - """显示主菜单""" - print("=" * 60) - print(" 欢迎使用 YOLO 对象检测系统") - print("=" * 60) + def run(self): + print("=== YOLO Detection System ===") + print("1. Static Image Detection") + print("2. Live Camera Detection") + print("3. Exit") - print(f"\n测试图像路径: {self.config.test_image_path}") - print(f"图像文件存在: {os.path.exists(self.config.test_image_path)}") + choice = input("Please select an option (1-3): ").strip() - print("\n请选择检测模式:") - print("1. 检测静态图像") - print("2. 检测摄像头实时画面") - print("3. 退出程序") - - def handle_choice(self, choice): - """处理用户选择""" - if choice == '1': - print("\n您选择了: 检测静态图像") - self.image_detector.detect_static_image(self.config.test_image_path) - return True - elif choice == '2': - print("\n您选择了: 检测摄像头实时画面") - print("注意:要退出摄像头模式,请按 Ctrl+C !") - self.camera_detector.detect_camera() - return True - elif choice == '3': - print("\n程序已退出。") - return False + if choice == "1": + self._run_static_detection() + elif choice == "2": + self._run_camera_detection() + elif choice == "3": + print("Exiting program.") else: - print("\n无效选择,请输入 1、2 或 3") - return True + print("Invalid option. Please enter 1, 2, or 3.") - def run(self): - """运行主程序""" - self.print_debug("程序开始执行") - self.display_menu() + def _run_static_detection(self): + image_path = self.config.test_image_path + if not os.path.exists(image_path): + print(f"Image not found: {image_path}") + return - running = True - while running: - try: - choice = input("\n请输入您的选择 (1/2/3): ").strip() - self.print_debug(f"用户输入: {choice}") - running = self.handle_choice(choice) - - except KeyboardInterrupt: - print("\n\n程序被用户中断。") - break - except EOFError: - print("\n输入结束,程序退出。") - break - except Exception as e: - print(f"\n发生错误: {e}") - break - - self.print_debug("程序结束") \ No newline at end of file + detector = ImageDetector(self.engine) + detector.detect_static_image(image_path) + + def _run_camera_detection(self): + detector = CameraDetector( + detection_engine=self.engine, + output_interval=self.config.output_interval + ) + detector.start_detection(camera_index=self.config.camera_index) diff --git a/src/self_driving_car_navigation/carla_environment.py b/src/self_driving_car_navigation/carla_environment.py index c8d46fd14c..5d6cc466b4 100644 --- a/src/self_driving_car_navigation/carla_environment.py +++ b/src/self_driving_car_navigation/carla_environment.py @@ -220,8 +220,13 @@ def _clear_all_non_ego_actors(self): print(f"[清理] 车辆{cleared_count['vehicle']} | 自行车{cleared_count['bicycle']} | 静态车辆{cleared_count['static_vehicle']}") def process_image(self, image): - """处理摄像头数据""" + """处理摄像头数据 - 修复frame属性访问问题""" try: + # 检查是否为有效的图像对象 + if not hasattr(image, 'raw_data') or not hasattr(image, 'height') or not hasattr(image, 'width'): + print("[图像处理错误] 无效的图像数据") + return + array = np.frombuffer(image.raw_data, dtype=np.uint8).reshape((image.height, image.width, 4))[:, :, :3] array = array[:, :, ::-1].copy() if self.image_queue.full(): @@ -231,8 +236,13 @@ def process_image(self, image): print(f"[图像处理错误] {str(e)}") def process_lidar(self, data): - """处理激光雷达数据""" + """处理激光雷达数据 - 修复frame属性访问问题""" try: + # 检查是否为有效的激光雷达数据 + if not hasattr(data, 'raw_data'): + print("[激光雷达处理错误] 无效的激光雷达数据") + return + points = np.frombuffer(data.raw_data, dtype=np.dtype('f4')).reshape(-1, 4)[:, :3] distances = np.linalg.norm(points, axis=1) angles = np.arctan2(points[:, 1], points[:, 0]) * 180 / np.pi @@ -251,8 +261,13 @@ def process_lidar(self, data): print(f"[激光雷达处理错误] {str(e)}") def process_imu(self, data): - """处理IMU数据""" + """处理IMU数据 - 修复frame属性访问问题""" try: + # 检查是否为有效的IMU数据 + if not hasattr(data, 'accelerometer') or not hasattr(data, 'gyroscope'): + print("[IMU处理错误] 无效的IMU数据") + return + imu_data = np.array([ data.accelerometer.x, data.accelerometer.y, data.accelerometer.z, data.gyroscope.x, data.gyroscope.y, data.gyroscope.z diff --git a/src/self_driving_car_navigation/run_simulation.py b/src/self_driving_car_navigation/run_simulation.py index 36ad14e01c..312538d685 100644 --- a/src/self_driving_car_navigation/run_simulation.py +++ b/src/self_driving_car_navigation/run_simulation.py @@ -3,8 +3,13 @@ import sys import pygame import carla +import os +import sys + +# 添加当前目录到Python路径,确保可以导入_agent模块 +sys.path.append(os.path.dirname(os.path.abspath(__file__))) -# 修正导入路径:适配_agent子目录下的carla_environment.py +# 从_agent子目录导入CarlaEnvironment from _agent.carla_environment import CarlaEnvironment print("="*60) @@ -46,6 +51,9 @@ def lerp(a, b, t): return smooth_tf def run_simulation(): + # 初始化pygame + pygame.init() + env = None try: print("\n[CARLA连接] 创建环境...") @@ -71,6 +79,11 @@ def run_simulation(): # 持续运行仿真(不限制步数) step = 0 while True: + # 处理pygame事件 + for event in pygame.event.get(): + if event.type == pygame.QUIT: + raise KeyboardInterrupt + # 同步CARLA帧(关键优化:保证控制时序稳定) env.world.tick() @@ -100,6 +113,8 @@ def run_simulation(): if env is not None: print("\n[资源清理] 销毁资源...") env.close() + # 退出pygame + pygame.quit() print("\n[程序退出]") if __name__ == "__main__": From 27faacffb452c22546fd1365cd6572c55a82ebc5 Mon Sep 17 00:00:00 2001 From: Xinyue-cao <648343923@qq.com> Date: Wed, 17 Dec 2025 09:58:15 +0800 Subject: [PATCH 3/4] =?UTF-8?q?=E6=9B=B4=E6=96=B0=20src/box/perception/mob?= =?UTF-8?q?lARMslndex.py?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- src/box/perception/moblARMslndex.py | 372 ++++++++++++++++++++++++++++ 1 file changed, 372 insertions(+) create mode 100644 src/box/perception/moblARMslndex.py diff --git a/src/box/perception/moblARMslndex.py b/src/box/perception/moblARMslndex.py new file mode 100644 index 0000000000..299bbb4d67 --- /dev/null +++ b/src/box/perception/moblARMslndex.py @@ -0,0 +1,372 @@ +""" +MoblArmsIndex 简化版本 - 只生成人体上肢模型运动视频 +完全修复版 +""" + +import numpy as np +import mujoco +import imageio +import os +import subprocess +import sys + +def install_ffmpeg(): + """安装必要的视频编码依赖""" + try: + # 尝试导入 imageio-ffmpeg + import imageio_ffmpeg + print("✓ imageio-ffmpeg 已安装") + return True + except ImportError: + print("正在安装 imageio-ffmpeg...") + try: + subprocess.check_call([sys.executable, "-m", "pip", "install", "imageio[ffmpeg]"]) + print("✓ imageio[ffmpeg] 安装成功") + return True + except Exception as e: + print(f"⚠ 安装失败: {e}") + print("将使用 GIF 格式替代 MP4") + return False + +# ================ 简化的 BaseBMModel 类 ================ +class BaseBMModel: + """简化的 BaseBMModel 基类""" + def __init__(self, model, data, **kwargs): + self.model = model + self.data = data + + def _update(self, model, data): + """更新方法 - 子类应重写""" + pass + +# ================ MoblArmsIndex 模型 ================ +class MoblArmsIndex(BaseBMModel): + """ + 基于 MoBL ARMS 模型的生物力学模型 + 只专注于生成上肢运动视频 + """ + + def __init__(self, model, data, **kwargs): + super().__init__(model, data, **kwargs) + print("✓ MoblArmsIndex 上肢模型初始化完成") + + def _update(self, model, data): + """更新模型状态 - 简化版本""" + # 这里可以添加任何必要的模型更新逻辑 + pass + +# ================ 主程序:只生成视频 ================ +def main(): + print("=" * 60) + print("人体上肢模型运动视频生成器") + print("=" * 60) + + # 检查并安装必要依赖 + has_ffmpeg = install_ffmpeg() + + # 1. 创建更真实的上肢XML模型(修复了所有问题) + xml_string = """ + + + """ + + # 2. 加载模型 + try: + model = mujoco.MjModel.from_xml_string(xml_string) + data = mujoco.MjData(model) + print("✓ MuJoCo 模型加载成功") + print(f" 关节数: {model.njnt}") + print(f" 几何体数: {model.ngeom}") + print(f" 执行器数: {model.nu}") + except Exception as e: + print(f"❌ 模型加载失败: {e}") + return + + # 3. 创建生物力学模型实例 + bm_model = MoblArmsIndex(model, data) + + # 4. 设置仿真参数 + fps = 30 + duration = 5 # 5秒仿真 + total_steps = int(duration / model.opt.timestep) + + print(f"\n仿真参数:") + print(f" 时长: {duration}秒") + print(f" 时间步长: {model.opt.timestep}") + print(f" 总步数: {total_steps}") + print(f" 视频帧率: {fps} FPS") + + # 5. 设置渲染器(使用较小的分辨率避免问题) + try: + # 创建渲染器(使用640x480避免离屏缓冲区问题) + renderer = mujoco.Renderer(model, height=480, width=640) + + # 设置相机视角 + camera = mujoco.MjvCamera() + camera.azimuth = 30 # 水平角度 + camera.elevation = -20 # 垂直角度 + camera.distance = 2.5 # 距离 + camera.lookat = np.array([0.2, 0, 1.2]) # 看向点 + + # 初始渲染 + renderer.update_scene(data, camera=camera) + print("✓ 渲染器创建成功") + except Exception as e: + print(f"❌ 渲染器创建失败: {e}") + return + + # 6. 运行仿真并捕获视频帧 + frames = [] + + print("\n开始仿真并录制视频...") + + for step in range(total_steps): + # 计算时间(用于控制运动) + t = step * model.opt.timestep + + # 协同运动控制 + # 肩部运动(缓慢的波浪) + shoulder_pitch = 0.3 * np.sin(2 * np.pi * 0.3 * t) + 0.1 + shoulder_roll = 0.2 * np.sin(2 * np.pi * 0.2 * t + 0.5) + + # 肘部运动 + elbow = 0.6 * np.sin(2 * np.pi * 0.4 * t) - 1.0 + + # 手腕运动 + wrist = 0.1 * np.sin(2 * np.pi * 0.6 * t) + + # 手指运动序列 + if t < 1: + # 初始:手指弯曲 + index_angle = -0.4 + thumb_angle = 0.1 + elif t < 3: + # 指向阶段 + phase = (t - 1) / 2 + index_angle = 0.1 * np.sin(2 * np.pi * 1.0 * t) - 0.1 + thumb_angle = 0.15 * np.sin(2 * np.pi * 0.8 * t) + else: + # 返回阶段 + phase = (t - 3) / 2 + index_angle = -0.4 * (1 - phase) + thumb_angle = 0.1 * (1 - phase) + + # 应用控制信号 + data.ctrl[0] = shoulder_pitch # 肩部俯仰 + data.ctrl[1] = shoulder_roll # 肩部滚动 + data.ctrl[2] = elbow # 肘部 + data.ctrl[3] = wrist # 手腕 + data.ctrl[4] = index_angle # 食指 + data.ctrl[5] = thumb_angle # 拇指 + + # 步进仿真 + mujoco.mj_step(model, data) + + # 定期捕获帧 + if step % 2 == 0: # 每2步捕获一帧,获得15fps(GIF比较流畅) + try: + # 动态调整相机视角 + if t < 2: + camera.azimuth = 30 + 15 * t + elif t < 4: + camera.elevation = -20 + 10 * np.sin(t * 0.5) + + # 更新场景并渲染 + renderer.update_scene(data, camera=camera) + frame = renderer.render() + frames.append(frame) + except: + # 如果失败,使用默认视角 + try: + renderer.update_scene(data) + frame = renderer.render() + frames.append(frame) + except: + pass + + # 显示进度 + if step % 100 == 0: + progress = (step / total_steps) * 100 + print(f"进度: {progress:.1f}%", end='\r') + + print(f"进度: 100.0% - 仿真完成!") + + # 7. 生成视频文件 + if frames: + try: + # 确定保存路径(桌面) + desktop = os.path.join(os.path.expanduser("~"), "Desktop") + if not os.path.exists(desktop): + desktop = os.path.dirname(os.path.abspath(__file__)) + + # 首先尝试生成MP4(如果有FFMPEG) + if has_ffmpeg: + try: + video_path = os.path.join(desktop, "human_arm_motion.mp4") + + # 保存MP4视频 + print(f"\n生成MP4视频中...") + imageio.mimsave( + video_path, + frames, + fps=15, # GIF帧率较低 + codec='libx264', + quality=8 + ) + print(f"✓ MP4视频已保存: {video_path}") + print(f" 文件大小: {os.path.getsize(video_path)/1024/1024:.1f} MB") + + except Exception as mp4_error: + print(f"⚠ MP4生成失败: {mp4_error}") + # 回退到GIF + has_ffmpeg = False + + # 如果没有FFMPEG或MP4生成失败,生成GIF + if not has_ffmpeg: + gif_path = os.path.join(desktop, "human_arm_motion.gif") + + print(f"\n生成GIF动画中...") + # 保存为GIF(质量稍低但兼容性好) + imageio.mimsave( + gif_path, + frames, + fps=15, # GIF帧率较低 + subrectangles=True # 优化GIF大小 + ) + print(f"✓ GIF动画已保存: {gif_path}") + print(f" 文件大小: {os.path.getsize(gif_path)/1024:.1f} KB") + + except Exception as e: + print(f"❌ 文件保存失败: {e}") + else: + print("⚠ 没有捕获到视频帧") + + print("\n" + "=" * 60) + print("✅ 任务完成!") + print("=" * 60) + + # 8. 打开文件所在目录(仅Windows) + if 'video_path' in locals() and os.path.exists(video_path): + print(f"📁 MP4视频文件: {video_path}") + try: + os.startfile(os.path.dirname(video_path)) + print("📂 已打开文件所在目录") + except: + pass + elif 'gif_path' in locals() and os.path.exists(gif_path): + print(f"📁 GIF动画文件: {gif_path}") + try: + os.startfile(os.path.dirname(gif_path)) + print("📂 已打开文件所在目录") + except: + pass + +# ================ 运行主程序 ================ +if __name__ == "__main__": + # 检查必要依赖 + try: + import mujoco + import numpy as np + import imageio + print("✓ 核心库已安装") + except ImportError as e: + print(f"❌ 缺少必要的库: {e}") + print("\n请安装以下依赖:") + print("pip install mujoco numpy imageio") + exit(1) + + try: + main() + except KeyboardInterrupt: + print("\n⚠ 用户中断") + except Exception as e: + print(f"\n❌ 运行出错: {type(e).__name__}: {e}") + import traceback + traceback.print_exc() + \ No newline at end of file From 6e70dc592856765d2d031c1f29ad59702d2aec43 Mon Sep 17 00:00:00 2001 From: Xinyue-cao <648343923@qq.com> Date: Thu, 18 Dec 2025 21:11:44 +0800 Subject: [PATCH 4/4] =?UTF-8?q?=20=E6=B7=BB=E5=8A=A0=E8=AE=AD=E7=BB=83?= =?UTF-8?q?=E5=90=AF=E5=8A=A8=E8=84=9A=E6=9C=AC?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- src/box/trainer.py | 121 +++++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 121 insertions(+) create mode 100644 src/box/trainer.py diff --git a/src/box/trainer.py b/src/box/trainer.py new file mode 100644 index 0000000000..7170f873d6 --- /dev/null +++ b/src/box/trainer.py @@ -0,0 +1,121 @@ +import sys +import os, shutil +import logging +from datetime import datetime + +import wandb +from wandb.integration.sb3 import WandbCallback + +from uitb.simulator import Simulator +from uitb.utils.functions import output_path, timeout_input + +from stable_baselines3.common.save_util import load_from_zip_file + +import argparse + +if __name__=="__main__": + + parser = argparse.ArgumentParser(description='Train an agent.') + parser.add_argument('config_file_path', type=str, + help='path to the config file') + parser.add_argument('--checkpoint', type=str, default=None, + help='filename of a specific checkpoint to resume training at ' + '(default: None, start training from the scratch)') + parser.add_argument('--resume', action='store_true', help='resume at latest checkpoint') + parser.add_argument('--eval', type=int, default=None, const=400000, nargs='?', help='run and store evaluations at a specific frequency (every ``eval`` timestep)') + parser.add_argument('--eval_info_keywords', type=str, nargs='*', default=[], help='additional keys of ``info`` dict that should be logged at the end of each evaluation episode') + args = parser.parse_args() + + # Get config file path + config_file_path = args.config_file_path + + # Build the simulator + simulator_folder = Simulator.build(config_file_path) + + # Initialise + simulator = Simulator.get(simulator_folder) + + # Get the config + config = simulator.config + + # Get simulator name + name = config.get("simulator_name", None) + + # Get checkpoint dir + checkpoint_dir = os.path.join(simulator._simulator_folder, 'checkpoints') + + # Restore wandb run_id from checkpoint if available + checkpoint = args.checkpoint + resume_training = args.resume or (checkpoint is not None) + if resume_training: + if os.path.isdir(checkpoint_dir): + existing_checkpoints = [os.path.join(checkpoint_dir, f) for f in os.listdir(checkpoint_dir) if + os.path.isfile(os.path.join(checkpoint_dir, f))] + else: + raise FileNotFoundError(f"Checkpoint directory does not exist: {checkpoint_dir}\nTry to run without --checkpoint or --resume.") + + if checkpoint is not None: + checkpoint_path = os.path.join(checkpoint_dir, checkpoint) + if not os.path.isfile(checkpoint_path): + raise FileNotFoundError(f"Checkpoint does not exist: {checkpoint_path}") + else: + assert len(existing_checkpoints) > 0, f"There are no checkpoints found in checkpoint directory: {checkpoint_dir}\n" \ + f"Maybe existing checkpoints were moved to a backup directory, " \ + f"which can be renamed to 'checkpoints/' to resume training." + # # find largest checkpoint + # checkpoint_path = sorted(existing_checkpoints, key=lambda f: int(f.split("_steps")[0].split("_")[-1]))[-1] + # find latest checkpoint + checkpoint_path = sorted(existing_checkpoints, key=os.path.getctime)[-1] + try: + _data, _, _ = load_from_zip_file(checkpoint_path) + wandb_id = _data["policy_kwargs"]["wandb_id"] + print(f"Resume wandb run {wandb_id} starting at checkpoint {checkpoint_path}.") + except Exception: + logging.warning("Cannot reliably identify wandb run id. Will resume training, but with new wandb instance and with step counter reset to zero.") + wandb_id = None + else: + checkpoint_path = None + wandb_id = None + # Backup existing checkpoint directory + if os.path.isdir(checkpoint_dir): + existing_checkpoints = [os.path.join(checkpoint_dir, f) for f in os.listdir(checkpoint_dir) if + os.path.isfile(os.path.join(checkpoint_dir, f))] + if len(existing_checkpoints) > 0: + last_checkpoint_time = max([os.path.getctime(f) for f in existing_checkpoints]) + last_checkpoint_time = datetime.fromtimestamp(last_checkpoint_time).strftime('%Y%m%d_%H%M%S') + checkpoint_dir_backup = os.path.join(simulator._simulator_folder, f'checkpoints_{last_checkpoint_time}') + shutil.move(checkpoint_dir, checkpoint_dir_backup) + + if name is None: + # Ask user to name this run + name = timeout_input("Give a name for this run. Input empty string or wait for 30 seconds for a random name.", + timeout=30, default="") + config["simulator_name"] = name.replace("-", "_") + + # Get project name + project = config.get("project", "uitb") + + # Prepare evaluation by storing custom log tags + with_evaluation = args.eval is not None + eval_freq = args.eval + eval_info_keywords = tuple(args.eval_info_keywords) + + # Initialise wandb + if wandb_id is None: + wandb_id = wandb.util.generate_id() + run = wandb.init(id=wandb_id, resume="allow", project=project, name=name, config=config, sync_tensorboard=True, save_code=True, dir=output_path()) + + # Initialise RL model + rl_cls = simulator.get_class("rl", config["rl"]["algorithm"]) + rl_model = rl_cls(simulator, checkpoint_path=checkpoint_path, wandb_id=wandb_id) + + # Haven't figured out how to periodically save rl in wandb; this is currently done inside the rl_model class + # TODO this doesn't seem to work; do the files need to be in wandb.run.dir? + #wandb.save(os.path.join(model_folder, run.name, 'checkpoints', "model_*_steps.zip"), + # base_path=os.path.join(model_folder, run.name, 'checkpoints')) + + # Start the training + # rl_model.learn(WandbCallback(verbose=2)) + rl_model.learn(WandbCallback(verbose=2), + with_evaluation=with_evaluation, eval_freq=eval_freq, eval_info_keywords=eval_info_keywords) + run.finish()