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的文件打开**

6. **导入结果如下图**
- 
+ 
+
+# QGIS 下载、安装与中文界面设置完整指南
+以下是QGIS软件从下载、安装到完成中文界面设置的完整步骤。整个过程主要分为下载、安装、设置语言以及获取学习数据四个部分。
+
+### 第一步:下载QGIS
+前往QGIS官方网站的下载页面 https://qgis.org/download/ 。页面会根据你的操作系统推荐合适的版本。
+
+操作系统:软件支持 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已经成功配置可以使用了
+
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()