From 72e7411153a0a7fcd43f5ef7f27c35fa7e41c2e7 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?=E5=AE=9E=E9=AA=8C604=E9=BB=84=E5=85=89=E8=BE=BE?= <2732514173@qq.com> Date: Mon, 1 Dec 2025 10:36:56 +0800 Subject: [PATCH 01/14] Add files via upload --- src/drone_perception/forecast.py | 258 +++++++++++++++++++++++++++++++ 1 file changed, 258 insertions(+) create mode 100644 src/drone_perception/forecast.py diff --git a/src/drone_perception/forecast.py b/src/drone_perception/forecast.py new file mode 100644 index 0000000000..f9b10b7c0b --- /dev/null +++ b/src/drone_perception/forecast.py @@ -0,0 +1,258 @@ +import os +import numpy as np +from PIL import Image +import torch +import torch.nn as nn +from torchvision import transforms, models + +# 构建与训练时相同的模型结构 +class ImageClassifier(nn.Module): + def __init__(self, num_classes): + super(ImageClassifier, self).__init__() + + # 使用与训练时相同的模型结构 + try: + # 新版本用法（torchvision >= 0.13） + self.backbone = models.resnet18(weights=None) # 不加载预训练权重，因为我们会加载自己的 + except TypeError: + # 旧版本兼容（torchvision < 0.13） + self.backbone = models.resnet18(pretrained=False) + + # 冻结预训练层的参数（与训练时一致） + for param in self.backbone.parameters(): + param.requires_grad = False + + # 替换最后的全连接层（必须与训练时结构相同） + in_features = self.backbone.fc.in_features + self.backbone.fc = nn.Sequential( + nn.Linear(in_features, 128), + nn.ReLU(inplace=True), + nn.Dropout(0.5), + nn.Linear(128, num_classes) + ) + + def forward(self, x): + return self.backbone(x) + +def predict_image(model_path, img_path, train_dir, img_size=(128, 128)): + """ + 使用训练好的PyTorch模型进行图像预测 + + 参数: + model_path: 模型文件路径 + img_path: 要预测的图像路径 + train_dir: 训练数据目录（用于获取类别标签） + img_size: 图像尺寸，必须与训练时相同 + """ + # 设置设备 + device = torch.device("cuda" if torch.cuda.is_available() else "cpu") + print(f"使用设备: {device}") + + # 获取类别标签（与训练时相同的方式） + class_labels = sorted([d for d in os.listdir(train_dir) + if os.path.isdir(os.path.join(train_dir, d))]) + num_classes = len(class_labels) + + if num_classes == 0: + print("错误: 在训练目录中未找到任何类别!") + return None + + print(f"检测到 {num_classes} 个类别: {class_labels}") + + # 初始化模型 + model = ImageClassifier(num_classes=num_classes) + + # 加载训练好的权重 + try: + model.load_state_dict(torch.load(model_path, map_location=device)) + model.to(device) + model.eval() # 设置为评估模式 + print(f"成功加载模型: {model_path}") + except Exception as e: + print(f"加载模型失败: {e}") + return None + + # 图像预处理（必须与训练时的测试预处理相同） + 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]) + ]) + + # 检查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 + + # 应用预处理 + input_tensor = transform(image).unsqueeze(0) # 添加batch维度 + input_tensor = input_tensor.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] + + # 显示详细信息 + print("\n" + "=" * 50) + print("📊 预测结果:") + print(f"🔍 预测类别: {predicted_class}") + print(f"📈 置信度: {confidence:.4f} ({confidence*100:.2f}%)") + print(f"🏷️ 类别索引: {predicted_class_idx}") + + # 显示所有类别的概率 + print("\n所有类别概率:") + for i, class_name in enumerate(class_labels): + prob = probabilities[i].item() + print(f" {class_name}: {prob:.4f} ({prob*100:.2f}%)") + + print("=" * 50) + + return predicted_class, confidence + +def main(): + """主函数 - 使用示例""" + # 路径设置 + base_dir = "./data" # 与训练代码相同的基准目录 + model_path = os.path.join(base_dir, "best_model.pth") # 使用训练代码保存的最佳模型 + 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") + + # 检查路径是否存在 + 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("=" * 50) + + if not all([os.path.exists(model_path), os.path.exists(train_dir)]): + print("错误: 模型或训练目录不存在!") + return + + if not os.path.exists(test_dir): + print("错误: 测试路径不存在!") + return + + # 执行预测 + result = predict_image(model_path, test_dir, 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)): + """ + 预测指定目录中的所有图像文件 + """ + device = torch.device("cuda" if torch.cuda.is_available() else "cpu") + + # 获取类别标签 + class_labels = sorted([d for d in os.listdir(train_dir) + if os.path.isdir(os.path.join(train_dir, d))]) + num_classes = len(class_labels) + + # 初始化模型 + model = ImageClassifier(num_classes=num_classes) + model.load_state_dict(torch.load(model_path, map_location=device)) + model.to(device) + model.eval() + + # 预处理 + 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]) + ]) + + 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 + }) + + print(f"📸 {img_name}: {predicted_class} (置信度: {confidence*100:.2f}%)") + + 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} 个图像") + + return results + +if __name__ == "__main__": + main() \ No newline at end of file From b0bd0c18c7f300c795f7eb1c0b55e21d5d3fb2b9 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?=E5=AE=9E=E9=AA=8C604=E9=BB=84=E5=85=89=E8=BE=BE?= <2732514173@qq.com> Date: Tue, 2 Dec 2025 09:28:34 +0800 Subject: [PATCH 02/14] Delete src/drone_perception/main.py --- src/drone_perception/main.py | 266 ----------------------------------- 1 file changed, 266 deletions(-) delete mode 100644 src/drone_perception/main.py diff --git a/src/drone_perception/main.py b/src/drone_perception/main.py deleted file mode 100644 index 99cbcf9975..0000000000 --- a/src/drone_perception/main.py +++ /dev/null @@ -1,266 +0,0 @@ -import os -import numpy as np -import tensorflow as tf -from tensorflow.keras.preprocessing.image import ImageDataGenerator -from tensorflow.keras.models import Sequential -from tensorflow.keras.layers import Conv2D, MaxPooling2D, Flatten, Dense, Dropout -from tensorflow.keras.callbacks import EarlyStopping, ModelCheckpoint -import matplotlib.pyplot as plt -from sklearn.metrics import classification_report, confusion_matrix -import seaborn as sns - -# 路径设置 -base_dir = os.path.abspath("../data") # 数据根目录，包含'train'和'test'文件夹 -train_dir = os.path.join(base_dir, "train")# 训练集目录路径 -test_dir = os.path.join(base_dir, "test") # 测试集目录路径 - -# 模型参数设置 -img_size = (128, 128) # 图像调整尺寸为128x128像素 -batch_size = 32 # 每个训练批次的样本数量 -epochs = 70# 训练总轮数 - -# 图像数据预处理与增强（用于训练集） -train_datagen = ImageDataGenerator( - rescale=1. /255,# 像素值归一化到0-1范围 - rotation_range=30,# 随机旋转角度范围±30度 - width_shift_range=0.1,# 水平随机平移范围10% - height_shift_range=0.1,# 垂直随机平移范围10% - shear_range=0.2,# 剪切变换强度 - zoom_range=0.2,# 随机缩放范围 - horizontal_flip=True# 启用水平翻转 -) - -# 测试集数据预处理（只做归一化，不进行增强） -test_datagen = ImageDataGenerator(rescale=1. / 255) - -# 创建训练数据生成器 -train_gen = train_datagen.flow_from_directory( - train_dir, # 训练集目录 - target_size=img_size, # 调整图像大小 - batch_size=batch_size, # 批次大小 - class_mode="categorical" # 多分类模式 -) - -# 创建测试数据生成器 -test_gen = test_datagen.flow_from_directory( - test_dir,# 测试集目录 - target_size=img_size, # 调整图像大小 - batch_size=batch_size, # 批次大小 - class_mode="categorical" # 多分类模式 -) - -# 导入迁移学习相关模块 -from tensorflow.keras.applications import MobileNetV2 -from tensorflow.keras.layers import GlobalAveragePooling2D - -# 加载预训练的MobileNetV2基础模型 -base_model = MobileNetV2( - input_shape=(128, 128, 3), # 输入图像尺寸 - include_top=False,# 不包含原始顶层分类器 - weights='imagenet' # 使用在ImageNet上预训练的权重 -) -base_model.trainable = False # 冻结基础模型权重，不参与训练 - -# 构建迁移学习模型 -model = tf.keras.Sequential([ - base_model, # 预训练的特征提取器 - GlobalAveragePooling2D(),# 全局平均池化层，减少参数数量 - Dense(128, activation="relu"), # 全连接层，128个神经元，ReLU激活 - Dropout(0.5),# 丢弃层，丢弃率50%，防止过拟合 - Dense(train_gen.num_classes, activation="softmax") # 输出层，使用softmax激活 -]) - -# 编译模型 -model.compile( - optimizer="adam", # 使用Adam优化器 - loss="categorical_crossentropy", # 分类交叉熵损失函数 - metrics=["accuracy"] # 评估指标为准确率 -) - -# 打印模型结构摘要 -model.summary() - -# 设置训练回调函数 -# 早停回调：监控验证集损失，连续5轮无改善则停止训练 -early_stop = EarlyStopping( - monitor='val_loss', # 监控验证集损失 - patience=5,# 容忍轮数 - restore_best_weights=True # 恢复最佳权重 -) - -# 模型检查点回调：保存最佳模型 -checkpoint = ModelCheckpoint( - filepath=os.path.join(base_dir, "best_model.h5"), # 模型保存路径 - monitor='val_accuracy',# 监控验证集准确率 - save_best_only=True,# 只保存最佳模型 - verbose=1# 显示保存信息 -) - -# 开始训练模型 -history = model.fit( - train_gen, # 训练数据生成器 - epochs=epochs, # 训练轮数 - validation_data=test_gen, # 验证数据 - callbacks=[early_stop, checkpoint] # 使用回调函数 -) - -# 保存最终训练完成的模型 -model.save(os.path.join(base_dir, "cnn_model.h5")) - -# 训练完成提示 -print("模型训练完成并已保存。") - - -# 错误分析函数 -def analyze_errors(model, test_gen, class_labels, num_samples=16): - """ - 分析模型在测试集上的错误分类情况 - - 参数: - - model: 训练好的模型 - - test_gen: 测试数据生成器 - - class_labels: 类别标签列表 - - num_samples: 要显示的错误样本数量 - """ - - # 重置测试生成器 - test_gen.reset() - - # 获取所有预测和真实标签 - predictions = model.predict(test_gen, verbose=1) - predicted_classes = np.argmax(predictions, axis=1) - true_classes = test_gen.classes - - # 计算准确率 - accuracy = np.mean(predicted_classes == true_classes) - print(f"测试集准确率: {accuracy:.4f}") - - # 分类报告 - print("\n分类报告:") - print(classification_report(true_classes, predicted_classes, target_names=class_labels)) - - # 混淆矩阵 - cm = confusion_matrix(true_classes, predicted_classes) - plt.figure(figsize=(10, 8)) - sns.heatmap(cm, annot=True, fmt='d', cmap='Blues', - xticklabels=class_labels, yticklabels=class_labels) - plt.title('混淆矩阵') - plt.xlabel('预测标签') - plt.ylabel('真实标签') - plt.xticks(rotation=45) - plt.yticks(rotation=0) - plt.tight_layout() - plt.savefig(os.path.join(base_dir, 'confusion_matrix.png')) - plt.show() - - # 找出错误分类的样本 - misclassified_indices = np.where(predicted_classes != true_classes)[0] - - print(f"\n总错误分类样本数: {len(misclassified_indices)}") - print(f"总样本数: {len(true_classes)}") - print(f"错误率: {len(misclassified_indices) / len(true_classes):.4f}") - - # 显示一些错误分类的样本 - if len(misclassified_indices) > 0: - # 随机选择一些错误样本进行可视化 - if len(misclassified_indices) > num_samples: - selected_indices = np.random.choice(misclassified_indices, num_samples, replace=False) - else: - selected_indices = misclassified_indices - - # 获取文件名 - filenames = test_gen.filenames - - # 创建错误分类可视化 - plot_misclassified_samples(selected_indices, filenames, true_classes, - predicted_classes, predictions, class_labels, test_gen) - - return misclassified_indices - - -def plot_misclassified_samples(indices, filenames, true_classes, predicted_classes, - predictions, class_labels, test_gen): - """ - 绘制错误分类的样本图像 - """ - # 计算网格大小 - n_cols = 4 - n_rows = (len(indices) + n_cols - 1) // n_cols - - fig, axes = plt.subplots(n_rows, n_cols, figsize=(15, n_rows * 3)) - if n_rows == 1: - axes = [axes] if n_cols == 1 else axes - else: - axes = axes.flatten() - - # 重置生成器以获取原始图像 - test_gen.reset() - all_images = [] - all_batches = len(test_gen) - - # 收集所有图像 - for i in range(all_batches): - images, _ = test_gen[i] - all_images.extend(images) - - for i, idx in enumerate(indices): - if i < len(axes): - ax = axes[i] - - # 显示图像 - ax.imshow(all_images[idx]) - - # 设置标题 - true_label = class_labels[true_classes[idx]] - pred_label = class_labels[predicted_classes[idx]] - confidence = np.max(predictions[idx]) - - title = f"True: {true_label}\nPred: {pred_label}\nConf: {confidence:.3f}" - ax.set_title(title, fontsize=10, color='red') - - # 获取文件名（不包含路径） - filename = os.path.basename(filenames[idx]) - ax.set_xlabel(f"File: {filename}", fontsize=8) - - ax.axis('off') - - # 隐藏多余的子图 - for j in range(len(indices), len(axes)): - axes[j].axis('off') - - plt.suptitle('错误分类样本示例', fontsize=16, y=1.02) - plt.tight_layout() - plt.savefig(os.path.join(base_dir, 'misclassified_samples.png'), - bbox_inches='tight', dpi=300) - plt.show() - - # 打印错误样本的详细信息 - print("\n错误分类样本详情:") - print("-" * 80) - for i, idx in enumerate(indices[:10]): # 只显示前10个的详细信息 - true_label = class_labels[true_classes[idx]] - pred_label = class_labels[predicted_classes[idx]] - confidence = np.max(predictions[idx]) - filename = os.path.basename(filenames[idx]) - - print(f"{i + 1:2d}. 文件: {filename:20s} | 真实: {true_label:15s} | " - f"预测: {pred_label:15s} | 置信度: {confidence:.4f}") - - -# 在训练完成后调用错误分析 -print("开始错误分析...") - -# 获取类别标签 -class_labels = list(train_gen.class_indices.keys()) - -# 加载最佳模型进行错误分析（如果有保存的最佳模型） -best_model_path = os.path.join(base_dir, "best_model.h5") -if os.path.exists(best_model_path): - print("加载最佳模型进行错误分析...") - best_model = tf.keras.models.load_model(best_model_path) - misclassified_indices = analyze_errors(best_model, test_gen, class_labels) -else: - print("使用最终训练模型进行错误分析...") - misclassified_indices = analyze_errors(model, test_gen, class_labels) - -print("\n错误分析完成！") \ No newline at end of file From 2c4bf66fbb905cb24dc7dbb0b32f274c453b68ec Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?=E5=AE=9E=E9=AA=8C604=E9=BB=84=E5=85=89=E8=BE=BE?= <2732514173@qq.com> Date: Tue, 2 Dec 2025 09:28:56 +0800 Subject: [PATCH 03/14] Add files via upload --- src/drone_perception/main.py | 242 +++++++++++++++++++++++++++++++++++ 1 file changed, 242 insertions(+) create mode 100644 src/drone_perception/main.py diff --git a/src/drone_perception/main.py b/src/drone_perception/main.py new file mode 100644 index 0000000000..1c796e4d89 --- /dev/null +++ b/src/drone_perception/main.py @@ -0,0 +1,242 @@ +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 From 19e7f7f0e26ed56d17ccef5324a7eeecb280bb22 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?=E5=AE=9E=E9=AA=8C604=E9=BB=84=E5=85=89=E8=BE=BE?= <2732514173@qq.com> Date: Mon, 8 Dec 2025 09:09:46 +0800 Subject: [PATCH 04/14] Update image_classification.py From 2c81480555945a119b4653e8eb0a952d2554f3b0 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?=E5=AE=9E=E9=AA=8C604=E9=BB=84=E5=85=89=E8=BE=BE?= <2732514173@qq.com> Date: Mon, 8 Dec 2025 09:23:20 +0800 Subject: [PATCH 05/14] Update image_classification.py --- src/drone_perception/image_classification.py | 1 + 1 file changed, 1 insertion(+) diff --git a/src/drone_perception/image_classification.py b/src/drone_perception/image_classification.py index 8746da7006..9c4f1b3bf4 100644 --- a/src/drone_perception/image_classification.py +++ b/src/drone_perception/image_classification.py @@ -287,3 +287,4 @@ def train_model(model, train_loader, test_loader, epochs, patience=5): plt.show() print("模型训练完成！") + From 6ccf7a05d5e8283875494d79c502f8fd3ad94969 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?=E5=AE=9E=E9=AA=8C604=E9=BB=84=E5=85=89=E8=BE=BE?= <2732514173@qq.com> Date: Mon, 8 Dec 2025 09:35:15 +0800 Subject: [PATCH 06/14] Update visual_navigation.py --- src/drone_perception/visual_navigation.py | 1 - 1 file changed, 1 deletion(-) diff --git a/src/drone_perception/visual_navigation.py b/src/drone_perception/visual_navigation.py index f6853b9c6e..8e55fe0eb2 100644 --- a/src/drone_perception/visual_navigation.py +++ b/src/drone_perception/visual_navigation.py @@ -286,5 +286,4 @@ def main(): print("🎯 无人机视觉导航系统已安全关闭") if __name__ == "__main__": - main() From f1d1235fb368a207711adf7929ebbb3f17c7b358 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?=E5=AE=9E=E9=AA=8C604=E9=BB=84=E5=85=89=E8=BE=BE?= <2732514173@qq.com> Date: Mon, 8 Dec 2025 09:39:14 +0800 Subject: [PATCH 07/14] Update visual_navigation.py --- src/drone_perception/visual_navigation.py | 176 ++++++---------------- 1 file changed, 46 insertions(+), 130 deletions(-) diff --git a/src/drone_perception/visual_navigation.py b/src/drone_perception/visual_navigation.py index 8e55fe0eb2..2274b00cab 100644 --- a/src/drone_perception/visual_navigation.py +++ b/src/drone_perception/visual_navigation.py @@ -8,7 +8,6 @@ import time import os -# DroneBattery Class to manage battery class DroneBattery: def __init__(self, max_capacity=100, current_charge=100): self.max_capacity = max_capacity @@ -17,24 +16,6 @@ def __init__(self, max_capacity=100, current_charge=100): def display_battery_status(self): print(f"Battery Status: {self.current_charge}%") - def charge_battery(self, charge_rate=10): - while self.current_charge < self.max_capacity: - self.current_charge += charge_rate - if self.current_charge > self.max_capacity: - self.current_charge = self.max_capacity - print(f"Charging... {self.current_charge}%") - time.sleep(1) - print("Battery fully charged!") - - def discharge_battery(self, discharge_rate=10): - while self.current_charge > 0: - self.current_charge -= discharge_rate - if self.current_charge < 0: - self.current_charge = 0 - print(f"Discharging... {self.current_charge}%") - time.sleep(1) - print("Battery completely drained!") - def is_battery_low(self): return self.current_charge < 20 @@ -43,19 +24,14 @@ class ImageClassifier(nn.Module): def __init__(self, num_classes): super(ImageClassifier, self).__init__() - # 使用与训练代码相同的模型结构 try: - # 新版本用法（torchvision >= 0.13） self.backbone = models.resnet18(weights=models.ResNet18_Weights.IMAGENET1K_V1) except TypeError: - # 旧版本兼容（torchvision < 0.13） self.backbone = models.resnet18(pretrained=True) - # 冻结预训练层的参数 for param in self.backbone.parameters(): param.requires_grad = False - # 替换最后的全连接层（与训练代码完全一致） in_features = self.backbone.fc.in_features self.backbone.fc = nn.Sequential( nn.Linear(in_features, 128), @@ -67,223 +43,163 @@ def __init__(self, num_classes): def forward(self, x): return self.backbone(x) -# 检测类别数量和类别映射 -def detect_class_info(): +def detect_class_info(train_dir): """检测训练数据中的类别信息""" - train_dir = "./data/train" if not os.path.exists(train_dir): - print(f"❌ 训练目录不存在: {train_dir}") - return 6, ['Animal', 'City', 'Fire', 'Forest', 'Vehicle', 'Water'] # 默认值 + return 6, ['Animal', 'City', 'Fire', 'Forest', 'Vehicle', 'Water'] classes = sorted([d for d in os.listdir(train_dir) if os.path.isdir(os.path.join(train_dir, d))]) if not classes: - print(f"❌ 在 {train_dir} 中没有找到任何类别文件夹!") - return 6, ['Animal', 'City', 'Fire', 'Forest', 'Vehicle', 'Water'] # 默认值 + return 6, ['Animal', 'City', 'Fire', 'Forest', 'Vehicle', 'Water'] class_to_idx = {cls_name: i for i, cls_name in enumerate(classes)} - print(f"✅ 检测到类别: {class_to_idx}") return len(classes), classes -# ✅ 加载PyTorch模型 -def load_pytorch_model(model_path): - print("📦 Loading PyTorch model...") - - # 检测类别信息 - num_classes, class_names = detect_class_info() - print(f"🎯 模型配置: {num_classes} 个类别 - {class_names}") +def load_pytorch_model(model_path, train_dir): + """加载PyTorch模型""" + num_classes, class_names = detect_class_info(train_dir) try: - # 初始化模型结构（与训练时完全相同） model = ImageClassifier(num_classes=num_classes) - - # 加载训练好的权重 model.load_state_dict(torch.load(model_path, map_location='cpu')) - print("✅ Model loaded successfully!") - - # 设置为评估模式 model.eval() return model, num_classes, class_names - except Exception as e: print(f"❌ Error loading model: {e}") return None, 0, [] -def check_emergency(): - emergency_conditions = ['low_battery', 'emergency'] - return random.choice(emergency_conditions) - -def handle_low_battery(drone_battery): - print("🔋 Low battery! Returning to base.") - drone_battery.charge_battery(charge_rate=15) - exit() - -# 图像预处理（与训练时的test_transform完全一致） def preprocess_frame(frame): - # 将OpenCV BGR图像转换为PIL RGB图像 + """图像预处理""" rgb_frame = cv2.cvtColor(frame, cv2.COLOR_BGR2RGB) pil_image = Image.fromarray(rgb_frame) - # 使用与训练代码中test_transform完全相同的预处理 transform = transforms.Compose([ - transforms.Resize((128, 128)), # 与img_size一致 + transforms.Resize((128, 128)), transforms.ToTensor(), transforms.Normalize(mean=[0.485, 0.456, 0.406], std=[0.229, 0.224, 0.225]) ]) - return transform(pil_image).unsqueeze(0) # 添加batch维度 + return transform(pil_image).unsqueeze(0) -# 预测函数 def predict_frame(model, frame, device, class_names): + """预测函数""" try: - # 预处理 input_tensor = preprocess_frame(frame) input_tensor = input_tensor.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() - # 获取类别名称 if predicted_class_idx < len(class_names): predicted_class = class_names[predicted_class_idx] else: predicted_class = f"Class_{predicted_class_idx}" return predicted_class, confidence * 100 - except Exception as e: print(f"❌ Prediction error: {e}") return "Unknown", 0.0 def decide_navigation(predicted_class): - if predicted_class == 'Fire': - print("🔥 Fire detected! Navigate away.") - elif predicted_class == 'Animal': - print("🦌 Animal ahead. Hovering.") - elif predicted_class == 'Forest': - print("🌲 Forest zone detected. Reduce speed.") - elif predicted_class == 'Water': - print("🌊 Water body detected. Maintain altitude and avoid descent.") - elif predicted_class == 'Vehicle': - print("🚗 Vehicle detected. Hover and wait.") - elif predicted_class == 'City': - print("🏙️ Urban area detected. Enable obstacle avoidance and slow navigation.") - else: - print(f"✅ {predicted_class} detected. Continue normal navigation.") + """导航决策""" + navigation_rules = { + 'Fire': "🔥 Fire detected! Navigate away.", + 'Animal': "🦌 Animal ahead. Hovering.", + 'Forest': "🌲 Forest zone detected. Reduce speed.", + 'Water': "🌊 Water body detected. Maintain altitude and avoid descent.", + 'Vehicle': "🚗 Vehicle detected. Hover and wait.", + 'City': "🏙️ Urban area detected. Enable obstacle avoidance and slow navigation." + } + + message = navigation_rules.get(predicted_class, f"✅ {predicted_class} detected. Continue normal navigation.") + print(message) + +def handle_low_battery(drone_battery): + """处理低电量""" + print("🔋 Low battery! Returning to base.") + exit() -def main(): +def run_visual_navigation(): + """运行视觉导航系统""" print("🚁 Starting the drone vision process with PyTorch model...") - start_time = time.time() - # 设置设备 device = torch.device("cuda" if torch.cuda.is_available() else "cpu") - print(f"Using device: {device}") - - # 加载模型（使用shijuedaohan.py训练好的模型） MODEL_PATH = "./data/best_model.pth" + TRAIN_DIR = "./data/train" - # 检查模型文件是否存在 if not os.path.exists(MODEL_PATH): print(f"❌ 模型文件不存在: {MODEL_PATH}") - print("请先运行 shijuedaohan.py 训练模型") return - model, num_classes, class_names = load_pytorch_model(MODEL_PATH) + model, num_classes, class_names = load_pytorch_model(MODEL_PATH, TRAIN_DIR) if model is None: print("❌ Failed to load model. Exiting.") return model = model.to(device) - print(f"🎯 模型加载完成: {num_classes} 个类别") - print(f"📋 类别列表: {class_names}") # 视频源设置 - VIDEO_SOURCE = 0 # 使用本地摄像头，或者改为您的IP摄像头地址 - # VIDEO_SOURCE = "http://192.168.1.3:4747/video" - + VIDEO_SOURCE = 0 cap = cv2.VideoCapture(VIDEO_SOURCE) - cap.set(cv2.CAP_PROP_BUFFERSIZE, 10) - cap.set(cv2.CAP_PROP_FPS, 30) - - # 检查视频流 + if not cap.isOpened(): print("❌ Failed to open video source.") return - else: - print("✅ Video source opened successfully.") - + drone_battery = DroneBattery() - - # FPS计算 fps_counter = 0 fps_time = time.time() frame_count = 0 - + predicted_class = "Unknown" + confidence = 0.0 + print("\n🎮 控制说明:") print("- 按 'q' 键退出程序") print("- 按 'b' 键模拟电池放电") print("- 按 'c' 键显示电池状态") print("- 开始实时视觉导航...\n") - + while True: - # 检查电池状态 if drone_battery.is_battery_low(): handle_low_battery(drone_battery) - - # 超时检查 - elapsed_time = time.time() - start_time - if elapsed_time > 300: # 5分钟 - print("⏰ Timeout reached! Stopping the drone.") - break - - # 读取帧 + ret, frame = cap.read() if not ret: - print("❌ Failed to capture frame.") break - - # 每隔5帧进行一次预测以提升性能 + frame_count += 1 if frame_count % 5 == 0: predicted_class, confidence = predict_frame(model, frame, device, class_names) - # 显示结果 cv2.putText(frame, f"{predicted_class} ({confidence:.2f}%)", (10, 30), cv2.FONT_HERSHEY_SIMPLEX, 0.7, (0, 255, 0), 2) - # 导航决策 decide_navigation(predicted_class) - + cv2.imshow("Drone Vision Feed - PyTorch", frame) - - # FPS计算和显示 + fps_counter += 1 if time.time() - fps_time >= 1.0: fps = fps_counter / (time.time() - fps_time) - print(f"📊 FPS: {fps:.1f} | 预测: {predicted_class} ({confidence:.1f}%)" if frame_count % 5 == 0 else f"📊 FPS: {fps:.1f}") + print(f"📊 FPS: {fps:.1f} | 预测: {predicted_class} ({confidence:.1f}%)") fps_counter = 0 fps_time = time.time() - - # 键盘控制 + key = cv2.waitKey(1) & 0xFF if key == ord('q'): - print("🛑 Manual stop initiated by the user.") break elif key == ord('b'): - print("🔋 Simulating battery discharge...") - drone_battery.current_charge = 15 # 设置为低电量状态 + drone_battery.current_charge = 15 elif key == ord('c'): drone_battery.display_battery_status() - + cap.release() cv2.destroyAllWindows() print("🎯 无人机视觉导航系统已安全关闭") -if __name__ == "__main__": - main() From 8a7bcb6d45f2b6c282ca0bf9a7eb0196513f20fa Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?=E5=AE=9E=E9=AA=8C604=E9=BB=84=E5=85=89=E8=BE=BE?= <2732514173@qq.com> Date: Mon, 15 Dec 2025 09:12:50 +0800 Subject: [PATCH 08/14] Update forecast.py --- src/drone_perception/forecast.py | 134 ++++++++++++------------------- 1 file changed, 50 insertions(+), 84 deletions(-) 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() + + From 2005fc6392d0098e4ed7052025f90bbf461f22d7 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?=E5=AE=9E=E9=AA=8C604=E9=BB=84=E5=85=89=E8=BE=BE?= <2732514173@qq.com> Date: Tue, 16 Dec 2025 09:39:15 +0800 Subject: [PATCH 09/14] Delete src/drone_perception/dron.py --- src/drone_perception/dron.py | 561 ----------------------------------- 1 file changed, 561 deletions(-) delete mode 100644 src/drone_perception/dron.py 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 From cfba0b29d4d4970dbddb7f27330186920679d280 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?=E5=AE=9E=E9=AA=8C604=E9=BB=84=E5=85=89=E8=BE=BE?= <2732514173@qq.com> Date: Tue, 16 Dec 2025 17:01:35 +0800 Subject: [PATCH 10/14] Delete src/drone_perception/main.py --- src/drone_perception/main.py | 242 ----------------------------------- 1 file changed, 242 deletions(-) delete mode 100644 src/drone_perception/main.py diff --git a/src/drone_perception/main.py b/src/drone_perception/main.py deleted file mode 100644 index 86e32bc20f..0000000000 --- a/src/drone_perception/main.py +++ /dev/null @@ -1,242 +0,0 @@ -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 From 4fedbc11617f65d7fa8c8db03d07bfab8ba4a53b Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?=E5=AE=9E=E9=AA=8C604=E9=BB=84=E5=85=89=E8=BE=BE?= <2732514173@qq.com> Date: Tue, 16 Dec 2025 17:02:17 +0800 Subject: [PATCH 11/14] Add files via upload --- src/drone_perception/environment_3d.py | 1007 ++++++++++++++++++++++++ src/drone_perception/main.py | 742 +++++++++++++++++ src/drone_perception/path_planning.py | 579 ++++++++++++++ 3 files changed, 2328 insertions(+) create mode 100644 src/drone_perception/environment_3d.py create mode 100644 src/drone_perception/main.py create mode 100644 src/drone_perception/path_planning.py 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/main.py b/src/drone_perception/main.py new file mode 100644 index 0000000000..134b9adf35 --- /dev/null +++ b/src/drone_perception/main.py @@ -0,0 +1,742 @@ +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 From d9a42353936b7449dbea196d37b0e10002346415 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?=E5=AE=9E=E9=AA=8C604=E9=BB=84=E5=85=89=E8=BE=BE?= <2732514173@qq.com> Date: Sat, 27 Dec 2025 09:40:40 +0800 Subject: [PATCH 12/14] Delete src/drone_perception/config.py --- src/drone_perception/config.py | 269 --------------------------------- 1 file changed, 269 deletions(-) delete mode 100644 src/drone_perception/config.py diff --git a/src/drone_perception/config.py b/src/drone_perception/config.py deleted file mode 100644 index e647d453bb..0000000000 --- a/src/drone_perception/config.py +++ /dev/null @@ -1,269 +0,0 @@ -""" -AirSimNH 无人机项目配置文件 -所有可调参数集中在此管理 -""" - -import math - -# ==================== 飞行与探索参数 ==================== -EXPLORATION = { - 'TOTAL_TIME': 120, # 总探索时间 (秒)，建议120-180 - 'PREFERRED_SPEED': 2.5, # 巡航速度 (米/秒)，建议2.0-3.0 - 'BASE_HEIGHT': -15.0, # 基础飞行高度 (米，负值)，建议-12到-18 - 'MAX_ALTITUDE': -30.0, # 最大海拔 (米)，限制无人机最高飞行高度 - 'MIN_ALTITUDE': -5.0, # 最小海拔 (米)，限制无人机最低飞行高度 - 'TAKEOFF_HEIGHT': -10.0, # 起飞目标高度，建议-8到-12 -} - -# ==================== 感知参数 ==================== -PERCEPTION = { - 'DEPTH_NEAR_THRESHOLD': 5.0, # 近距离警报阈值 (米)，小于此值触发避障 - 'DEPTH_SAFE_THRESHOLD': 10.0, # 安全距离阈值 (米)，大于此值认为方向安全 - 'MIN_GROUND_CLEARANCE': 2.0, # 最小离地间隙 (米)，防止撞地 - 'MAX_PITCH_ANGLE_DEG': 15, # 最大允许俯仰角 (度) - - # 深度图像扫描角度 (度)，用于多方向安全检测 - 'SCAN_ANGLES': [-60, -45, -30, -15, 0, 15, 30, 45, 60], - - # 高度推荐策略 - 'HEIGHT_STRATEGY': { - 'STEEP_SLOPE': -20.0, # 陡峭地形高度 - 'OPEN_SPACE': -12.0, # 开阔地带高度 - 'DEFAULT': -15.0, # 默认高度 - 'SLOPE_THRESHOLD': 5.0, # 坡度阈值，大于此值认为地形陡峭 - 'OPENNESS_THRESHOLD': 0.7, # 开阔度阈值，大于此值认为开阔 - }, - - # 红色物体检测参数 - 'RED_OBJECT_DETECTION': { - 'ENABLED': True, # 启用红色物体检测 - 'MIN_AREA': 50, # 最小检测面积（像素） - 'MAX_AREA': 10000, # 最大检测面积（像素） - 'UPDATE_INTERVAL': 1.0, # 检测更新间隔（秒） - 'MEMORY_TIME': 5.0, # 物体记忆时间（秒），避免重复计数 - }, - - # 蓝色物体检测参数（新增） - 'BLUE_OBJECT_DETECTION': { - 'ENABLED': True, # 启用蓝色物体检测 - 'MIN_AREA': 50, # 最小检测面积（像素） - 'MAX_AREA': 10000, # 最大检测面积（像素） - 'UPDATE_INTERVAL': 1.0, # 检测更新间隔（秒） - 'MEMORY_TIME': 5.0, # 物体记忆时间（秒） - }, - - # 黑色物体检测参数（新增） - 'BLACK_OBJECT_DETECTION': { - 'ENABLED': True, # 启用黑色物体检测 - 'MIN_AREA': 50, # 最小检测面积（像素） - 'MAX_AREA': 10000, # 最大检测面积（像素） - 'UPDATE_INTERVAL': 1.0, # 检测更新间隔（秒） - 'MEMORY_TIME': 5.0, # 物体记忆时间（秒） - } -} - -# ==================== 智能决策参数 ==================== -INTELLIGENT_DECISION = { - # 向量场算法参数 - 'VECTOR_FIELD_RADIUS': 8.0, # 向量场影响半径 (米) - 'OBSTACLE_REPULSION_GAIN': 3.0, # 障碍物排斥增益 - 'GOAL_ATTRACTION_GAIN': 2.0, # 目标吸引力增益 - 'SMOOTHING_FACTOR': 0.3, # 向量平滑因子 - 'MIN_TURN_ANGLE_DEG': 10, # 最小转弯角度 (度) - 'MAX_TURN_ANGLE_DEG': 60, # 最大转弯角度 (度) - - # 探索网格参数 - 'GRID_RESOLUTION': 2.0, # 网格分辨率 (米) - 'GRID_SIZE': 50, # 网格大小 (单元格数) - 'INFORMATION_GAIN_DECAY': 0.95, # 信息增益衰减率 - 'EXPLORATION_FRONTIER_THRESHOLD': 0.3, # 探索前沿阈值 - - # 控制参数 - 'PID_KP': 1.5, # 比例系数 - 'PID_KI': 0.05, # 积分系数 - 'PID_KD': 0.2, # 微分系数 - 'SMOOTHING_WINDOW_SIZE': 5, # 平滑窗口大小 - - # 自适应参数 - 'ADAPTIVE_SPEED_ENABLED': True, # 启用自适应速度 - 'MIN_SPEED_FACTOR': 0.3, # 最小速度因子 - 'MAX_SPEED_FACTOR': 1.5, # 最大速度因子 - - # 探索策略权重 - 'MEMORY_WEIGHT': 0.7, # 记忆权重 (避免重复访问) - 'CURIOUSITY_WEIGHT': 0.3, # 好奇心权重 (探索新区域) - - # 目标管理 - 'TARGET_LIFETIME': 15.0, # 目标有效期 (秒) - 'TARGET_REACHED_DISTANCE': 3.0, # 目标到达判定距离 (米) - - # 红色物体探索参数 - 'RED_OBJECT_EXPLORATION': { - 'ATTRACTION_GAIN': 1.5, # 红色物体吸引力增益 - 'DETECTION_RADIUS': 10.0, # 检测半径（米） - 'MIN_DISTANCE': 2.0, # 最小接近距离（米） - 'EXPLORATION_BONUS': 0.5, # 探索奖励分数 - }, - - # 蓝色物体探索参数（新增） - 'BLUE_OBJECT_EXPLORATION': { - 'ATTRACTION_GAIN': 1.2, # 蓝色物体吸引力增益 - 'DETECTION_RADIUS': 8.0, # 检测半径（米） - 'MIN_DISTANCE': 2.0, # 最小接近距离（米） - 'EXPLORATION_BONUS': 0.3, # 探索奖励分数 - }, - - # 黑色物体探索参数（新增） - 'BLACK_OBJECT_EXPLORATION': { - 'ATTRACTION_GAIN': 1.0, # 黑色物体吸引力增益 - 'DETECTION_RADIUS': 8.0, # 检测半径（米） - 'MIN_DISTANCE': 2.0, # 最小接近距离（米） - 'EXPLORATION_BONUS': 0.2, # 探索奖励分数 - } -} - -# ==================== 手动控制参数 ==================== -MANUAL = { - 'CONTROL_SPEED': 3.0, # 水平移动速度 (米/秒) - 'ALTITUDE_SPEED': 2.0, # 垂直移动速度 (米/秒) - 'YAW_SPEED': 45.0, # 偏航角速度 (度/秒) - 'ENABLE_AUTO_HOVER': True, # 松开按键时自动悬停 - 'DISPLAY_CONTROLS': True, # 在画面显示控制说明 - 'SAFETY_ENABLED': True, # 启用安全限制 (高度、速度限制) - 'MAX_MANUAL_SPEED': 5.0, # 最大手动控制速度 - 'MIN_ALTITUDE_LIMIT': -5.0, # 最低飞行高度限制 - 'MAX_ALTITUDE_LIMIT': -30.0, # 最高飞行高度限制 -} - -# ==================== 窗口显示参数 ==================== -DISPLAY = { - # 前视窗口参数 - 'FRONT_VIEW_WINDOW': { - 'NAME': "无人机前视画面", - 'WIDTH': 640, - 'HEIGHT': 480, - 'ENABLE_SHARPENING': True, # 启用图像锐化，改善模糊 - 'SHOW_INFO_OVERLAY': True, # 显示信息叠加层 - 'REFRESH_RATE_MS': 30, # 刷新率 (毫秒)，建议30-50 - 'SHOW_RED_OBJECTS': True, # 在画面中标记红色物体 - 'SHOW_BLUE_OBJECTS': True, # 在画面中标记蓝色物体 - 'SHOW_BLACK_OBJECTS': True, # 在画面中标记黑色物体（新增） - # 内存优化参数 - 'QUEUE_MAXSIZE': 2, # 图像队列最大大小，减少内存占用（原为3） - 'REDUCE_IMAGE_COPY': True, # 减少图像复制，仅在必要时复制 - }, - - # 信息显示窗口参数 - 'INFO_WINDOW': { - 'NAME': "无人机信息面板", - 'WIDTH': 800, - 'HEIGHT': 600, - 'BACKGROUND_COLOR': (20, 20, 30), # 深蓝灰色背景 - 'TEXT_COLOR': (220, 220, 255), # 浅蓝色文字 - 'HIGHLIGHT_COLOR': (0, 200, 255), # 青色高亮 - 'WARNING_COLOR': (0, 100, 255), # 橙色警告 - 'SUCCESS_COLOR': (0, 255, 150), # 绿色成功 - 'REFRESH_RATE_MS': 100, # 信息窗口刷新率 - 'SHOW_GRID': True, # 显示探索网格 - 'GRID_SIZE': 300, # 网格显示大小 - 'SHOW_OBJECTS_STATS': True, # 显示物体统计 - 'SHOW_SYSTEM_STATS': True, # 显示系统统计 - 'SHOW_PERFORMANCE': True, # 显示性能信息 - 'SHOW_TRAJECTORY': True, # 显示运动轨迹图 - 'TRAJECTORY_SIZE': 280, # 轨迹图显示大小（像素） - 'TRAJECTORY_MAX_POINTS': 1000, # 轨迹最大记录点数 - 'TRAJECTORY_LINE_COLOR': (0, 255, 255), # 轨迹线颜色（青色） - 'TRAJECTORY_CURRENT_COLOR': (0, 255, 0), # 当前位置颜色（绿色） - 'TRAJECTORY_START_COLOR': (255, 255, 0), # 起始位置颜色（黄色） - } -} - -# ==================== 系统与安全参数 ==================== -SYSTEM = { - 'LOG_LEVEL': 'INFO', # 日志级别: DEBUG, INFO, WARNING, ERROR - 'LOG_TO_FILE': True, # 是否保存日志到文件 - 'LOG_FILENAME': 'drone_log.txt', # 日志文件名 - - 'MAX_RECONNECT_ATTEMPTS': 3, # 最大重连尝试次数 - 'RECONNECT_DELAY': 2.0, # 重连延迟 (秒) - - 'ENABLE_HEALTH_CHECK': True, # 启用健康检查 - 'HEALTH_CHECK_INTERVAL': 20, # 健康检查间隔 (循环次数) - - 'EMERGENCY_RESPONSE_TIME': 10.0, # 紧急响应超时时间 (秒) -} - -# ==================== 相机配置 ==================== -CAMERA = { - 'DEFAULT_NAME': "0", # 默认相机名称 - 'POSSIBLE_NAMES': ["0", "1", "front_center", "front", "CameraActor_0"], - - # 深度相机参数 - 'DEPTH_FOV_DEG': 90, # 深度相机视野 (度) - 'MAX_DEPTH_RANGE': 100.0, # 最大深度范围 (米), - - # 红色物体检测颜色范围（HSV空间） - 'RED_COLOR_RANGE': { - 'LOWER1': [0, 120, 70], # 红色下限1（0-10度） - 'UPPER1': [10, 255, 255], # 红色上限1 - 'LOWER2': [170, 120, 70], # 红色下限2（170-180度） - 'UPPER2': [180, 255, 255], # 红色上限2 - }, - - # 蓝色物体检测颜色范围（HSV空间）（新增） - 'BLUE_COLOR_RANGE': { - 'LOWER': [100, 150, 50], # 蓝色下限 - 'UPPER': [130, 255, 255], # 蓝色上限 - }, - - # 黑色物体检测颜色范围（HSV空间）（新增） - 'BLACK_COLOR_RANGE': { - 'LOWER': [0, 0, 0], # 黑色下限（色相任意，饱和度和亮度都很低） - 'UPPER': [180, 255, 50], # 黑色上限（亮度阈值50，避免过暗） - } -} - -# ==================== 调试参数 ==================== -DEBUG = { - 'SAVE_PERCEPTION_IMAGES': False, # 是否保存感知图像用于调试 - 'IMAGE_SAVE_INTERVAL': 50, # 图像保存间隔 (循环次数) - 'LOG_DECISION_DETAILS': True, # 是否记录详细决策信息 - 'SAVE_GRID_VISUALIZATION': True, # 是否保存网格可视化 - 'LOG_VECTOR_FIELD': False, # 是否记录向量场详细信息 - 'PERFORMANCE_PROFILING': False, # 是否启用性能分析 - 'SAVE_RED_OBJECT_IMAGES': False, # 是否保存检测到红色物体的图像 - 'SAVE_BLUE_OBJECT_IMAGES': False, # 是否保存检测到蓝色物体的图像（新增） - 'SAVE_BLACK_OBJECT_IMAGES': False, # 是否保存检测到黑色物体的图像（新增） -} - -# ==================== 数据记录参数 ==================== -DATA_RECORDING = { - 'ENABLED': True, # 启用数据记录 - 'RECORD_INTERVAL': 0.2, # 记录间隔（秒） - 'SAVE_TO_CSV': True, # 保存为CSV格式 - 'SAVE_TO_JSON': True, # 保存为JSON格式 - 'CSV_FILENAME': 'flight_data.csv', # CSV文件名 - 'JSON_FILENAME': 'flight_data.json', # JSON文件名 - 'PERFORMANCE_MONITORING': True, # 启用性能监控 - 'SYSTEM_METRICS_INTERVAL': 5.0, # 系统指标记录间隔（秒） - 'RECORD_RED_OBJECTS': True, # 记录红色物体信息 - 'RECORD_BLUE_OBJECTS': True, # 记录蓝色物体信息（新增） - 'RECORD_BLACK_OBJECTS': True, # 记录黑色物体信息（新增） - # 内存优化参数 - 'MAX_FLIGHT_DATA_BUFFER': 500, # 最大飞行数据缓冲区大小（条），超过后自动保存并清空 - 'MAX_OBJECTS_BUFFER': 200, # 最大物体记录缓冲区大小（个） - 'MAX_EVENTS_BUFFER': 100, # 最大事件记录缓冲区大小（个） - 'AUTO_SAVE_INTERVAL': 60.0, # 自动保存间隔（秒），定期保存数据到文件 -} - -# ==================== 性能监控参数 ==================== -PERFORMANCE = { - 'ENABLE_REALTIME_METRICS': True, # 启用实时性能监控 - 'CPU_WARNING_THRESHOLD': 80.0, # CPU使用率警告阈值（%） - 'MEMORY_WARNING_THRESHOLD': 80.0, # 内存使用率警告阈值（%） - 'LOOP_TIME_WARNING_THRESHOLD': 0.2, # 循环时间警告阈值（秒） - 'SAVE_PERFORMANCE_REPORT': True, # 保存性能报告 - 'REPORT_INTERVAL': 30.0, # 性能报告间隔（秒） - # 内存优化参数 - 'MAX_METRICS_BUFFER': 500, # 最大性能指标缓冲区大小（个），减少内存占用 -} \ No newline at end of file From ad5b6432cfb26cc00b341f96651d72e796d6e8dc Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?=E5=AE=9E=E9=AA=8C604=E9=BB=84=E5=85=89=E8=BE=BE?= <2732514173@qq.com> Date: Sat, 27 Dec 2025 09:41:03 +0800 Subject: [PATCH 13/14] Delete src/drone_perception/perceptive_drone_explorer.py --- .../perceptive_drone_explorer.py | 4350 ----------------- 1 file changed, 4350 deletions(-) delete mode 100644 src/drone_perception/perceptive_drone_explorer.py diff --git a/src/drone_perception/perceptive_drone_explorer.py b/src/drone_perception/perceptive_drone_explorer.py deleted file mode 100644 index 94a2e5f633..0000000000 --- a/src/drone_perception/perceptive_drone_explorer.py +++ /dev/null @@ -1,4350 +0,0 @@ -# -*- coding: utf-8 -*- -""" -AirSimNH 感知驱动自主探索无人机 - 智能决策增强版（红色、蓝色与黑色物体检测版） -核心：视觉感知 → 语义理解 → 智能决策 → 安全执行 -集成：配置管理、日志系统、异常恢复、前视窗口显示 -新增：向量场避障算法、基于网格的信息增益探索、平滑飞行控制 -新增：性能监控与数据闭环系统、红色、蓝色与黑色物体检测与记录 -新增：信息显示窗口，分离前视画面与系统信息 -版本: 3.6 (双窗口三色物体检测版) -""" - -import airsim -import time -import numpy as np -import cv2 -import math -import json -import csv -from collections import deque -from dataclasses import dataclass -from enum import Enum -import threading -import queue -import signal -import sys -from typing import Tuple, List, Optional, Dict, Set, Any -import traceback -import logging -from datetime import datetime -import random -import psutil -import os -import gc -import platform - -# 导入PIL用于中文文本绘制 -try: - from PIL import Image, ImageDraw, ImageFont - PIL_AVAILABLE = True -except ImportError: - PIL_AVAILABLE = False - print("⚠️ PIL/Pillow未安装，中文显示可能不正常。请运行: pip install Pillow") - -# 全局字体缓存 -_chinese_font_cache = {} -# 全局标志：是否禁用中文显示（如果字体加载失败） -_chinese_font_disabled = False - -# ============ 导入配置文件 ============ -try: - import config - CONFIG_LOADED = True -except ImportError as e: - print(f"❌ 无法加载配置文件 config.py: {e}") - print("正在使用默认配置...") - CONFIG_LOADED = False - class DefaultConfig: - EXPLORATION = {'TOTAL_TIME': 120, 'PREFERRED_SPEED': 2.5, 'BASE_HEIGHT': -15.0, - 'MAX_ALTITUDE': -30.0, 'MIN_ALTITUDE': -5.0, 'TAKEOFF_HEIGHT': -10.0} - PERCEPTION = {'DEPTH_NEAR_THRESHOLD': 5.0, 'DEPTH_SAFE_THRESHOLD': 10.0, - 'MIN_GROUND_CLEARANCE': 2.0, 'MAX_PITCH_ANGLE_DEG': 15, - 'SCAN_ANGLES': [-60, -45, -30, -15, 0, 15, 30, 45, 60], - 'HEIGHT_STRATEGY': {'STEEP_SLOPE': -20.0, 'OPEN_SPACE': -12.0, - 'DEFAULT': -15.0, 'SLOPE_THRESHOLD': 5.0, - 'OPENNESS_THRESHOLD': 0.7}, - 'RED_OBJECT_DETECTION': {'ENABLED': True, 'MIN_AREA': 50, - 'MAX_AREA': 10000, 'UPDATE_INTERVAL': 1.0, - 'MEMORY_TIME': 5.0}, - 'BLUE_OBJECT_DETECTION': {'ENABLED': True, 'MIN_AREA': 50, - 'MAX_AREA': 10000, 'UPDATE_INTERVAL': 1.0, - 'MEMORY_TIME': 5.0}, - 'BLACK_OBJECT_DETECTION': {'ENABLED': True, 'MIN_AREA': 50, - 'MAX_AREA': 10000, 'UPDATE_INTERVAL': 1.0, - 'MEMORY_TIME': 5.0}} - DISPLAY = {'FRONT_VIEW_WINDOW': {'NAME': "无人机前视画面", 'WIDTH': 640, 'HEIGHT': 480, - 'ENABLE_SHARPENING': True, 'SHOW_INFO_OVERLAY': True, - 'REFRESH_RATE_MS': 30, 'SHOW_RED_OBJECTS': True, - 'SHOW_BLUE_OBJECTS': True, 'SHOW_BLACK_OBJECTS': True}, - 'INFO_WINDOW': {'NAME': "无人机信息面板", 'WIDTH': 800, 'HEIGHT': 600, - 'BACKGROUND_COLOR': (20, 20, 30), 'TEXT_COLOR': (220, 220, 255), - 'HIGHLIGHT_COLOR': (0, 200, 255), 'WARNING_COLOR': (0, 100, 255), - 'SUCCESS_COLOR': (0, 255, 150), 'REFRESH_RATE_MS': 100, - 'SHOW_GRID': True, 'GRID_SIZE': 300, - 'SHOW_OBJECTS_STATS': True, 'SHOW_SYSTEM_STATS': True, - 'SHOW_PERFORMANCE': True, 'SHOW_TRAJECTORY': True, - 'TRAJECTORY_SIZE': 280, 'TRAJECTORY_MAX_POINTS': 1000, - 'TRAJECTORY_LINE_COLOR': (0, 255, 255), - 'TRAJECTORY_CURRENT_COLOR': (0, 255, 0), - 'TRAJECTORY_START_COLOR': (255, 255, 0)}} - SYSTEM = {'LOG_LEVEL': 'INFO', 'LOG_TO_FILE': True, 'LOG_FILENAME': 'drone_log.txt', - 'MAX_RECONNECT_ATTEMPTS': 3, 'RECONNECT_DELAY': 2.0, - 'ENABLE_HEALTH_CHECK': True, 'HEALTH_CHECK_INTERVAL': 20} - CAMERA = {'DEFAULT_NAME': "0", - 'RED_COLOR_RANGE': {'LOWER1': [0, 120, 70], 'UPPER1': [10, 255, 255], - 'LOWER2': [170, 120, 70], 'UPPER2': [180, 255, 255]}, - 'BLUE_COLOR_RANGE': {'LOWER': [100, 150, 50], 'UPPER': [130, 255, 255]}, - 'BLACK_COLOR_RANGE': {'LOWER': [0, 0, 0], 'UPPER': [180, 255, 50]}} - MANUAL = { - 'CONTROL_SPEED': 3.0, - 'ALTITUDE_SPEED': 2.0, - 'YAW_SPEED': 30.0, - 'ENABLE_AUTO_HOVER': True, - 'DISPLAY_CONTROLS': True, - 'SAFETY_ENABLED': True, - 'MAX_MANUAL_SPEED': 5.0, - 'MIN_ALTITUDE_LIMIT': -5.0, - 'MAX_ALTITUDE_LIMIT': -30.0 - } - INTELLIGENT_DECISION = { - 'VECTOR_FIELD_RADIUS': 8.0, - 'OBSTACLE_REPULSION_GAIN': 3.0, - 'GOAL_ATTRACTION_GAIN': 2.0, - 'SMOOTHING_FACTOR': 0.3, - 'MIN_TURN_ANGLE_DEG': 10, - 'MAX_TURN_ANGLE_DEG': 60, - 'GRID_RESOLUTION': 2.0, - 'GRID_SIZE': 50, - 'INFORMATION_GAIN_DECAY': 0.95, - 'EXPLORATION_FRONTIER_THRESHOLD': 0.3, - 'PID_KP': 1.5, - 'PID_KI': 0.05, - 'PID_KD': 0.2, - 'SMOOTHING_WINDOW_SIZE': 5, - 'ADAPTIVE_SPEED_ENABLED': True, - 'MIN_SPEED_FACTOR': 0.3, - 'MAX_SPEED_FACTOR': 1.5, - 'MEMORY_WEIGHT': 0.7, - 'CURIOUSITY_WEIGHT': 0.3, - 'TARGET_LIFETIME': 15.0, - 'TARGET_REACHED_DISTANCE': 3.0, - 'RED_OBJECT_EXPLORATION': {'ATTRACTION_GAIN': 1.5, 'DETECTION_RADIUS': 10.0, - 'MIN_DISTANCE': 2.0, 'EXPLORATION_BONUS': 0.5}, - 'BLUE_OBJECT_EXPLORATION': {'ATTRACTION_GAIN': 1.2, 'DETECTION_RADIUS': 8.0, - 'MIN_DISTANCE': 2.0, 'EXPLORATION_BONUS': 0.3}, - 'BLACK_OBJECT_EXPLORATION': {'ATTRACTION_GAIN': 1.0, 'DETECTION_RADIUS': 8.0, - 'MIN_DISTANCE': 2.0, 'EXPLORATION_BONUS': 0.2} - } - DEBUG = { - 'SAVE_PERCEPTION_IMAGES': False, - 'IMAGE_SAVE_INTERVAL': 50, - 'LOG_DECISION_DETAILS': False, - 'SAVE_RED_OBJECT_IMAGES': False, - 'SAVE_BLUE_OBJECT_IMAGES': False, - 'SAVE_BLACK_OBJECT_IMAGES': False - } - DATA_RECORDING = { - 'ENABLED': True, - 'RECORD_INTERVAL': 0.2, - 'SAVE_TO_CSV': True, - 'SAVE_TO_JSON': True, - 'CSV_FILENAME': 'flight_data.csv', - 'JSON_FILENAME': 'flight_data.json', - 'PERFORMANCE_MONITORING': True, - 'SYSTEM_METRICS_INTERVAL': 5.0, - 'RECORD_RED_OBJECTS': True, - 'RECORD_BLUE_OBJECTS': True, - 'RECORD_BLACK_OBJECTS': True - } - PERFORMANCE = { - 'ENABLE_REALTIME_METRICS': True, - 'CPU_WARNING_THRESHOLD': 80.0, - 'MEMORY_WARNING_THRESHOLD': 80.0, - 'LOOP_TIME_WARNING_THRESHOLD': 0.2, - 'SAVE_PERFORMANCE_REPORT': True, - 'REPORT_INTERVAL': 30.0, - } - config = DefaultConfig() - - -class FlightState(Enum): - """无人机飞行状态枚举""" - TAKEOFF = "起飞" - HOVERING = "悬停观测" - EXPLORING = "主动探索" - AVOIDING = "避障机动" - RETURNING = "返航中" - LANDING = "降落" - EMERGENCY = "紧急状态" - MANUAL = "手动控制" - PLANNING = "路径规划" - RED_OBJECT_INSPECTION = "红色物体检查" - BLUE_OBJECT_INSPECTION = "蓝色物体检查" - BLACK_OBJECT_INSPECTION = "黑色物体检查" - - -@dataclass -class RedObject: - """红色物体数据结构""" - id: int - position: Tuple[float, float, float] - pixel_position: Tuple[int, int] - size: float - confidence: float - timestamp: float - last_seen: float - visited: bool = False - - -@dataclass -class BlueObject: - """蓝色物体数据结构""" - id: int - position: Tuple[float, float, float] - pixel_position: Tuple[int, int] - size: float - confidence: float - timestamp: float - last_seen: float - visited: bool = False - - -@dataclass -class BlackObject: - """黑色物体数据结构""" - id: int - position: Tuple[float, float, float] - pixel_position: Tuple[int, int] - size: float - confidence: float - timestamp: float - last_seen: float - visited: bool = False - - -class Vector2D: - """二维向量类""" - def __init__(self, x=0.0, y=0.0): - self.x = x - self.y = y - - def __add__(self, other): - return Vector2D(self.x + other.x, self.y + other.y) - - def __sub__(self, other): - return Vector2D(self.x - other.x, self.y - other.y) - - def __mul__(self, scalar): - return Vector2D(self.x * scalar, self.y * scalar) - - def __truediv__(self, scalar): - return Vector2D(self.x / scalar, self.y / scalar) - - def magnitude(self): - return math.sqrt(self.x**2 + self.y**2) - - def normalize(self): - mag = self.magnitude() - if mag > 0: - return Vector2D(self.x / mag, self.y / mag) - return Vector2D() - - def rotate(self, angle): - cos_a = math.cos(angle) - sin_a = math.sin(angle) - return Vector2D( - self.x * cos_a - self.y * sin_a, - self.x * sin_a + self.y * cos_a - ) - - def to_tuple(self): - return (self.x, self.y) - - @staticmethod - def from_angle(angle, magnitude=1.0): - return Vector2D(magnitude * math.cos(angle), magnitude * math.sin(angle)) - - -class PIDController: - """PID控制器类""" - def __init__(self, kp, ki, kd, integral_limit=5.0, output_limit=10.0): - self.kp = kp - self.ki = ki - self.kd = kd - self.integral_limit = integral_limit - self.output_limit = output_limit - - self.previous_error = 0.0 - self.integral = 0.0 - self.previous_time = time.time() - - def update(self, error, dt=None): - if dt is None: - current_time = time.time() - dt = current_time - self.previous_time - self.previous_time = current_time - - self.integral += error * dt - self.integral = max(-self.integral_limit, min(self.integral_limit, self.integral)) - - derivative = (error - self.previous_error) / dt if dt > 0 else 0.0 - self.previous_error = error - - output = self.kp * error + self.ki * self.integral + self.kd * derivative - return max(-self.output_limit, min(self.output_limit, output)) - - -class ExplorationGrid: - """探索网格地图类""" - def __init__(self, resolution=2.0, grid_size=50): - self.resolution = resolution - self.grid_size = grid_size - self.half_size = grid_size // 2 - - self.grid = np.zeros((grid_size, grid_size), dtype=np.float32) - self.information_gain = np.zeros((grid_size, grid_size), dtype=np.float32) - self.obstacle_grid = np.zeros((grid_size, grid_size), dtype=bool) - self.visit_time = np.zeros((grid_size, grid_size), dtype=np.float32) - self.red_object_grid = np.zeros((grid_size, grid_size), dtype=bool) - self.blue_object_grid = np.zeros((grid_size, grid_size), dtype=bool) - self.black_object_grid = np.zeros((grid_size, grid_size), dtype=bool) - self.current_idx = (self.half_size, self.half_size) - self.frontier_cells = set() - - print(f"🗺️ 初始化探索网格: {grid_size}x{grid_size}, 分辨率: {resolution}m") - - def world_to_grid(self, world_x, world_y): - grid_x = int(world_x / self.resolution) + self.half_size - grid_y = int(world_y / self.resolution) + self.half_size - - grid_x = max(0, min(self.grid_size - 1, grid_x)) - grid_y = max(0, min(self.grid_size - 1, grid_y)) - - return (grid_x, grid_y) - - def grid_to_world(self, grid_x, grid_y): - world_x = (grid_x - self.half_size) * self.resolution - world_y = (grid_y - self.half_size) * self.resolution - return (world_x, world_y) - - def update_position(self, world_x, world_y): - self.current_idx = self.world_to_grid(world_x, world_y) - - x, y = self.current_idx - radius = 3 - - for dx in range(-radius, radius + 1): - for dy in range(-radius, radius + 1): - nx, ny = x + dx, y + dy - if 0 <= nx < self.grid_size and 0 <= ny < self.grid_size: - distance = math.sqrt(dx**2 + dy**2) - exploration_value = max(0, 1.0 - distance / radius) - self.grid[nx, ny] = max(self.grid[nx, ny], exploration_value) - self.visit_time[nx, ny] = time.time() - - self._update_frontiers() - - def _update_frontiers(self): - self.frontier_cells.clear() - - for x in range(1, self.grid_size - 1): - for y in range(1, self.grid_size - 1): - if self.grid[x, y] > 0.7: - neighbors = [ - (x-1, y), (x+1, y), (x, y-1), (x, y+1), - (x-1, y-1), (x-1, y+1), (x+1, y-1), (x+1, y+1) - ] - - for nx, ny in neighbors: - if 0 <= nx < self.grid_size and 0 <= ny < self.grid_size: - if self.grid[nx, ny] < 0.3 and not self.obstacle_grid[nx, ny]: - unexplored_neighbors = 0 - for nnx in range(nx-1, nx+2): - for nny in range(ny-1, ny+2): - if 0 <= nnx < self.grid_size and 0 <= nny < self.grid_size: - if self.grid[nnx, nny] < 0.3: - unexplored_neighbors += 1 - - self.information_gain[nx, ny] = unexplored_neighbors / 9.0 - self.frontier_cells.add((nx, ny)) - - def update_obstacles(self, obstacles_world): - for obs_x, obs_y in obstacles_world: - grid_x, grid_y = self.world_to_grid(obs_x, obs_y) - - radius = 2 - for dx in range(-radius, radius + 1): - for dy in range(-radius, radius + 1): - nx, ny = grid_x + dx, grid_y + dy - if 0 <= nx < self.grid_size and 0 <= ny < self.grid_size: - self.obstacle_grid[nx, ny] = True - self.grid[nx, ny] = 0.0 - - def update_red_objects(self, red_objects): - self.red_object_grid.fill(False) - - for obj in red_objects: - grid_x, grid_y = self.world_to_grid(obj.position[0], obj.position[1]) - - radius = 1 - for dx in range(-radius, radius + 1): - for dy in range(-radius, radius + 1): - nx, ny = grid_x + dx, grid_y + dy - if 0 <= nx < self.grid_size and 0 <= ny < self.grid_size: - self.red_object_grid[nx, ny] = True - - def update_blue_objects(self, blue_objects): - self.blue_object_grid.fill(False) - - for obj in blue_objects: - grid_x, grid_y = self.world_to_grid(obj.position[0], obj.position[1]) - - radius = 1 - for dx in range(-radius, radius + 1): - for dy in range(-radius, radius + 1): - nx, ny = grid_x + dx, grid_y + dy - if 0 <= nx < self.grid_size and 0 <= ny < self.grid_size: - self.blue_object_grid[nx, ny] = True - - def update_black_objects(self, black_objects): - self.black_object_grid.fill(False) - - for obj in black_objects: - grid_x, grid_y = self.world_to_grid(obj.position[0], obj.position[1]) - - radius = 1 - for dx in range(-radius, radius + 1): - for dy in range(-radius, radius + 1): - nx, ny = grid_x + dx, grid_y + dy - if 0 <= nx < self.grid_size and 0 <= ny < self.grid_size: - self.black_object_grid[nx, ny] = True - - def get_best_exploration_target(self, current_pos, red_objects=None, blue_objects=None, black_objects=None): - # 优先检查红色物体 - if red_objects and len(red_objects) > 0: - nearest_obj = None - min_distance = float('inf') - current_x, current_y = current_pos - - for obj in red_objects: - if not obj.visited: - distance = math.sqrt((obj.position[0] - current_x)**2 + - (obj.position[1] - current_y)**2) - if distance < min_distance: - min_distance = distance - nearest_obj = obj - - if nearest_obj and min_distance < 15.0: - return (nearest_obj.position[0], nearest_obj.position[1]) - - # 其次检查蓝色物体 - if blue_objects and len(blue_objects) > 0: - nearest_obj = None - min_distance = float('inf') - current_x, current_y = current_pos - - for obj in blue_objects: - if not obj.visited: - distance = math.sqrt((obj.position[0] - current_x)**2 + - (obj.position[1] - current_y)**2) - if distance < min_distance: - min_distance = distance - nearest_obj = obj - - if nearest_obj and min_distance < 12.0: - return (nearest_obj.position[0], nearest_obj.position[1]) - - # 再次检查黑色物体 - if black_objects and len(black_objects) > 0: - nearest_obj = None - min_distance = float('inf') - current_x, current_y = current_pos - - for obj in black_objects: - if not obj.visited: - distance = math.sqrt((obj.position[0] - current_x)**2 + - (obj.position[1] - current_y)**2) - if distance < min_distance: - min_distance = distance - nearest_obj = obj - - if nearest_obj and min_distance < 12.0: - return (nearest_obj.position[0], nearest_obj.position[1]) - - if not self.frontier_cells: - angle = random.uniform(0, 2 * math.pi) - distance = 10.0 - return ( - current_pos[0] + distance * math.cos(angle), - current_pos[1] + distance * math.sin(angle) - ) - - best_score = -1 - best_target = None - current_x, current_y = current_pos - - for fx, fy in self.frontier_cells: - info_gain = self.information_gain[fx, fy] - - world_x, world_y = self.grid_to_world(fx, fy) - distance = math.sqrt((world_x - current_x)**2 + (world_y - current_y)**2) - distance_cost = min(1.0, distance / 30.0) - - time_since_visit = time.time() - self.visit_time[fx, fy] - time_factor = min(1.0, time_since_visit / 60.0) - - red_bonus = 0.0 - if self.red_object_grid[fx, fy]: - red_bonus = config.INTELLIGENT_DECISION['RED_OBJECT_EXPLORATION']['EXPLORATION_BONUS'] - - blue_bonus = 0.0 - if self.blue_object_grid[fx, fy]: - blue_bonus = config.INTELLIGENT_DECISION['BLUE_OBJECT_EXPLORATION']['EXPLORATION_BONUS'] - - black_bonus = 0.0 - if self.black_object_grid[fx, fy]: - black_bonus = config.INTELLIGENT_DECISION['BLACK_OBJECT_EXPLORATION']['EXPLORATION_BONUS'] - - score = ( - config.INTELLIGENT_DECISION['CURIOUSITY_WEIGHT'] * info_gain + - (1 - config.INTELLIGENT_DECISION['MEMORY_WEIGHT'] * time_factor) - - distance_cost * 0.3 + - red_bonus + blue_bonus + black_bonus - ) - - if score > best_score: - best_score = score - best_target = (world_x, world_y) - - return best_target - - def visualize_grid(self, size=300): - if self.grid.size == 0: - return None - - img_size = min(size, self.grid_size * 5) - img = np.zeros((img_size, img_size, 3), dtype=np.uint8) - - cell_size = img_size // self.grid_size - - for x in range(self.grid_size): - for y in range(self.grid_size): - color = (0, 0, 0) - - if (x, y) == self.current_idx: - color = (0, 255, 0) - elif self.obstacle_grid[x, y]: - color = (0, 0, 255) - elif self.red_object_grid[x, y]: - color = (0, 100, 255) # 红色物体显示为橙色 - elif self.blue_object_grid[x, y]: - color = (255, 100, 0) # 蓝色物体显示为青色 - elif self.black_object_grid[x, y]: - color = (128, 128, 128) # 黑色物体显示为灰色 - elif self.grid[x, y] > 0.7: - color = (200, 200, 200) - elif self.grid[x, y] > 0.3: - color = (100, 100, 100) - elif (x, y) in self.frontier_cells: - gain = self.information_gain[x, y] - color = (0, int(255 * gain), int(255 * (1 - gain))) - - x1 = x * cell_size - y1 = y * cell_size - x2 = (x + 1) * cell_size - y2 = (y + 1) * cell_size - - cv2.rectangle(img, (y1, x1), (y2, x2), color, -1) - - return img - - -class DataLogger: - """数据记录器类""" - - def __init__(self, enable_csv=True, enable_json=True, csv_filename=None, json_filename=None): - self.enable_csv = enable_csv - self.enable_json = enable_json - - timestamp = datetime.now().strftime("%Y%m%d_%H%M%S") - - if csv_filename: - self.csv_filename = csv_filename - else: - self.csv_filename = f"flight_data_{timestamp}.csv" - - if json_filename: - self.json_filename = json_filename - else: - self.json_filename = f"flight_data_{timestamp}.json" - - self.data_buffer = [] - self.json_data = { - "flight_info": { - "start_time": datetime.now().isoformat(), - "config_loaded": CONFIG_LOADED, - "system": config.SYSTEM, - "exploration": config.EXPLORATION, - "perception": config.PERCEPTION, - "intelligent_decision": config.INTELLIGENT_DECISION, - "performance": config.PERFORMANCE - }, - "flight_data": [] - } - - self.performance_metrics = { - "start_time": time.time(), - "cpu_usage": [], - "memory_usage": [], - "loop_times": [], - "data_points": 0 - } - - self.red_objects_detected = [] - self.blue_objects_detected = [] - self.black_objects_detected = [] - - # 内存优化：限制缓冲区大小 - self.max_flight_data = config.DATA_RECORDING.get('MAX_FLIGHT_DATA_BUFFER', 500) - self.max_objects_buffer = config.DATA_RECORDING.get('MAX_OBJECTS_BUFFER', 200) - self.max_events_buffer = config.DATA_RECORDING.get('MAX_EVENTS_BUFFER', 100) - self.auto_save_interval = config.DATA_RECORDING.get('AUTO_SAVE_INTERVAL', 60.0) - self.last_auto_save_time = time.time() - self.max_metrics_buffer = config.PERFORMANCE.get('MAX_METRICS_BUFFER', 500) - - self.csv_columns = [ - 'timestamp', 'loop_count', 'state', 'pos_x', 'pos_y', 'pos_z', - 'vel_x', 'vel_y', 'vel_z', 'yaw', 'pitch', 'roll', - 'obstacle_distance', 'open_space_score', 'terrain_slope', - 'has_obstacle', 'obstacle_direction', 'recommended_height', - 'target_x', 'target_y', 'target_z', 'velocity_command_x', - 'velocity_command_y', 'velocity_command_z', 'yaw_command', - 'battery_level', 'cpu_usage', 'memory_usage', 'loop_time', - 'grid_frontiers', 'grid_explored', 'vector_field_magnitude', - 'adaptive_speed_factor', 'decision_making_time', 'perception_time', - 'red_objects_count', 'red_objects_detected', 'red_objects_visited', - 'blue_objects_count', 'blue_objects_detected', 'blue_objects_visited', - 'black_objects_count', 'black_objects_detected', 'black_objects_visited' - ] - - if self.enable_csv: - self._init_csv_file() - - print(f"📊 数据记录器初始化完成") - print(f" CSV文件: {self.csv_filename}") - print(f" JSON文件: {self.json_filename}") - - def _init_csv_file(self): - try: - with open(self.csv_filename, 'w', newline='', encoding='utf-8') as f: - writer = csv.DictWriter(f, fieldnames=self.csv_columns) - writer.writeheader() - except Exception as e: - print(f"❌ 无法初始化CSV文件: {e}") - self.enable_csv = False - - def record_flight_data(self, data_dict): - if not config.DATA_RECORDING['ENABLED']: - return - - try: - data_dict['timestamp'] = datetime.now().isoformat() - - if self.enable_csv: - with open(self.csv_filename, 'a', newline='', encoding='utf-8') as f: - writer = csv.DictWriter(f, fieldnames=self.csv_columns) - row = {col: data_dict.get(col, '') for col in self.csv_columns} - writer.writerow(row) - - if self.enable_json: - self.json_data['flight_data'].append(data_dict) - # 内存优化：限制flight_data长度，超过限制时保存并清空 - if len(self.json_data['flight_data']) >= self.max_flight_data: - self._auto_save_and_clear() - - self.performance_metrics['data_points'] += 1 - - # 内存优化：定期自动保存 - current_time = time.time() - if current_time - self.last_auto_save_time >= self.auto_save_interval: - self._auto_save_and_clear() - self.last_auto_save_time = current_time - - if self.performance_metrics['data_points'] % 10 == 0: - self._collect_system_metrics() - - except Exception as e: - print(f"⚠️ 记录飞行数据时出错: {e}") - - def record_red_object(self, red_object): - try: - red_object_data = { - 'id': red_object.id, - 'position': red_object.position, - 'pixel_position': red_object.pixel_position, - 'size': red_object.size, - 'confidence': red_object.confidence, - 'timestamp': red_object.timestamp, - 'visited': red_object.visited - } - - # 内存优化：限制物体记录列表长度 - if len(self.red_objects_detected) >= self.max_objects_buffer: - self.red_objects_detected = self.red_objects_detected[-self.max_objects_buffer//2:] - self.red_objects_detected.append(red_object_data) - - if 'red_objects' not in self.json_data: - self.json_data['red_objects'] = [] - - # 内存优化：限制JSON中的物体列表长度 - if len(self.json_data['red_objects']) >= self.max_objects_buffer: - self.json_data['red_objects'] = self.json_data['red_objects'][-self.max_objects_buffer//2:] - self.json_data['red_objects'].append(red_object_data) - - except Exception as e: - print(f"⚠️ 记录红色物体时出错: {e}") - - def record_blue_object(self, blue_object): - try: - blue_object_data = { - 'id': blue_object.id, - 'position': blue_object.position, - 'pixel_position': blue_object.pixel_position, - 'size': blue_object.size, - 'confidence': blue_object.confidence, - 'timestamp': blue_object.timestamp, - 'visited': blue_object.visited - } - - # 内存优化：限制物体记录列表长度 - if len(self.blue_objects_detected) >= self.max_objects_buffer: - self.blue_objects_detected = self.blue_objects_detected[-self.max_objects_buffer//2:] - self.blue_objects_detected.append(blue_object_data) - - if 'blue_objects' not in self.json_data: - self.json_data['blue_objects'] = [] - - # 内存优化：限制JSON中的物体列表长度 - if len(self.json_data['blue_objects']) >= self.max_objects_buffer: - self.json_data['blue_objects'] = self.json_data['blue_objects'][-self.max_objects_buffer//2:] - self.json_data['blue_objects'].append(blue_object_data) - - except Exception as e: - print(f"⚠️ 记录蓝色物体时出错: {e}") - - def record_black_object(self, black_object): - try: - black_object_data = { - 'id': black_object.id, - 'position': black_object.position, - 'pixel_position': black_object.pixel_position, - 'size': black_object.size, - 'confidence': black_object.confidence, - 'timestamp': black_object.timestamp, - 'visited': black_object.visited - } - - # 内存优化：限制物体记录列表长度 - if len(self.black_objects_detected) >= self.max_objects_buffer: - self.black_objects_detected = self.black_objects_detected[-self.max_objects_buffer//2:] - self.black_objects_detected.append(black_object_data) - - if 'black_objects' not in self.json_data: - self.json_data['black_objects'] = [] - - # 内存优化：限制JSON中的物体列表长度 - if len(self.json_data['black_objects']) >= self.max_objects_buffer: - self.json_data['black_objects'] = self.json_data['black_objects'][-self.max_objects_buffer//2:] - self.json_data['black_objects'].append(black_object_data) - - except Exception as e: - print(f"⚠️ 记录黑色物体时出错: {e}") - - def _collect_system_metrics(self): - try: - cpu_percent = psutil.cpu_percent(interval=0.1) - self.performance_metrics['cpu_usage'].append(cpu_percent) - - memory_info = psutil.virtual_memory() - memory_percent = memory_info.percent - self.performance_metrics['memory_usage'].append(memory_percent) - - # 内存优化：使用配置的最大缓冲区大小 - max_length = self.max_metrics_buffer - if len(self.performance_metrics['cpu_usage']) > max_length: - self.performance_metrics['cpu_usage'] = self.performance_metrics['cpu_usage'][-max_length:] - if len(self.performance_metrics['memory_usage']) > max_length: - self.performance_metrics['memory_usage'] = self.performance_metrics['memory_usage'][-max_length:] - - except Exception as e: - print(f"⚠️ 收集系统指标时出错: {e}") - - def record_loop_time(self, loop_time): - self.performance_metrics['loop_times'].append(loop_time) - - # 内存优化：使用配置的最大缓冲区大小 - max_length = self.max_metrics_buffer - if len(self.performance_metrics['loop_times']) > max_length: - self.performance_metrics['loop_times'] = self.performance_metrics['loop_times'][-max_length:] - - def record_event(self, event_type, event_data): - try: - event_record = { - 'timestamp': datetime.now().isoformat(), - 'event_type': event_type, - 'event_data': event_data - } - - if 'events' not in self.json_data: - self.json_data['events'] = [] - - # 内存优化：限制events列表长度 - if len(self.json_data['events']) >= self.max_events_buffer: - self.json_data['events'] = self.json_data['events'][-self.max_events_buffer//2:] - self.json_data['events'].append(event_record) - - except Exception as e: - print(f"⚠️ 记录事件时出错: {e}") - - def _auto_save_and_clear(self): - """自动保存数据并清空缓冲区（内存优化）""" - if not self.enable_json or len(self.json_data['flight_data']) == 0: - return - - try: - # 创建临时文件名 - timestamp = datetime.now().strftime("%Y%m%d_%H%M%S") - temp_filename = self.json_filename.replace('.json', f'_temp_{timestamp}.json') - - # 保存当前数据 - with open(temp_filename, 'w', encoding='utf-8') as f: - json.dump(self.json_data, f, indent=2, ensure_ascii=False) - - # 清空flight_data，保留其他数据 - saved_count = len(self.json_data['flight_data']) - self.json_data['flight_data'] = [] - - # 强制垃圾回收 - gc.collect() - - print(f"💾 自动保存 {saved_count} 条数据到: {temp_filename} (已清空缓冲区)") - except Exception as e: - print(f"⚠️ 自动保存数据时出错: {e}") - - def save_json_data(self): - if not self.enable_json: - return - - try: - self._calculate_performance_stats() - - # 红色物体统计 - if 'red_objects' in self.json_data: - red_count = len(self.json_data['red_objects']) - visited_count = sum(1 for obj in self.json_data['red_objects'] if obj.get('visited', False)) - self.json_data['red_objects_summary'] = { - 'total_detected': red_count, - 'total_visited': visited_count, - 'visit_rate': visited_count / red_count if red_count > 0 else 0 - } - - # 蓝色物体统计 - if 'blue_objects' in self.json_data: - blue_count = len(self.json_data['blue_objects']) - visited_count = sum(1 for obj in self.json_data['blue_objects'] if obj.get('visited', False)) - self.json_data['blue_objects_summary'] = { - 'total_detected': blue_count, - 'total_visited': visited_count, - 'visit_rate': visited_count / blue_count if blue_count > 0 else 0 - } - - # 黑色物体统计 - if 'black_objects' in self.json_data: - black_count = len(self.json_data['black_objects']) - visited_count = sum(1 for obj in self.json_data['black_objects'] if obj.get('visited', False)) - self.json_data['black_objects_summary'] = { - 'total_detected': black_count, - 'total_visited': visited_count, - 'visit_rate': visited_count / black_count if black_count > 0 else 0 - } - - with open(self.json_filename, 'w', encoding='utf-8') as f: - json.dump(self.json_data, f, indent=2, ensure_ascii=False) - - print(f"✅ JSON数据已保存: {self.json_filename}") - - except Exception as e: - print(f"❌ 保存JSON数据时出错: {e}") - - def _calculate_performance_stats(self): - if not self.performance_metrics['cpu_usage']: - return - - cpu_avg = np.mean(self.performance_metrics['cpu_usage']) - cpu_max = np.max(self.performance_metrics['cpu_usage']) - cpu_min = np.min(self.performance_metrics['cpu_usage']) - - mem_avg = np.mean(self.performance_metrics['memory_usage']) - mem_max = np.max(self.performance_metrics['memory_usage']) - mem_min = np.min(self.performance_metrics['memory_usage']) - - if self.performance_metrics['loop_times']: - loop_avg = np.mean(self.performance_metrics['loop_times']) - loop_max = np.max(self.performance_metrics['loop_times']) - loop_min = np.min(self.performance_metrics['loop_times']) - else: - loop_avg = loop_max = loop_min = 0 - - self.json_data['performance_summary'] = { - 'total_data_points': self.performance_metrics['data_points'], - 'total_time_seconds': time.time() - self.performance_metrics['start_time'], - 'cpu_usage': { - 'average': float(cpu_avg), - 'maximum': float(cpu_max), - 'minimum': float(cpu_min) - }, - 'memory_usage': { - 'average': float(mem_avg), - 'maximum': float(mem_max), - 'minimum': float(mem_min) - }, - 'loop_times': { - 'average_seconds': float(loop_avg), - 'maximum_seconds': float(loop_max), - 'minimum_seconds': float(loop_min) - } - } - - def generate_performance_report(self): - try: - if not self.performance_metrics['cpu_usage']: - return "无性能数据可用" - - self._calculate_performance_stats() - - report = "\n" + "="*60 + "\n" - report += "📊 系统性能报告\n" - report += "="*60 + "\n" - - report += f"总数据点数: {self.performance_metrics['data_points']}\n" - report += f"运行时间: {time.time() - self.performance_metrics['start_time']:.1f}秒\n" - - if self.performance_metrics['cpu_usage']: - cpu_avg = np.mean(self.performance_metrics['cpu_usage']) - cpu_max = np.max(self.performance_metrics['cpu_usage']) - report += f"CPU使用率: 平均{cpu_avg:.1f}%, 最大{cpu_max:.1f}%\n" - - if self.performance_metrics['memory_usage']: - mem_avg = np.mean(self.performance_metrics['memory_usage']) - mem_max = np.max(self.performance_metrics['memory_usage']) - report += f"内存使用率: 平均{mem_avg:.1f}%, 最大{mem_max:.1f}%\n" - - if self.performance_metrics['loop_times']: - loop_avg = np.mean(self.performance_metrics['loop_times']) - loop_max = np.max(self.performance_metrics['loop_times']) - report += f"循环时间: 平均{loop_avg*1000:.1f}ms, 最大{loop_max*1000:.1f}ms\n" - - if 'red_objects' in self.json_data: - red_count = len(self.json_data['red_objects']) - visited_count = sum(1 for obj in self.json_data['red_objects'] if obj.get('visited', False)) - report += f"红色物体检测: 总数{red_count}个, 已访问{visited_count}个\n" - - if 'blue_objects' in self.json_data: - blue_count = len(self.json_data['blue_objects']) - visited_count = sum(1 for obj in self.json_data['blue_objects'] if obj.get('visited', False)) - report += f"蓝色物体检测: 总数{blue_count}个, 已访问{visited_count}个\n" - - if 'black_objects' in self.json_data: - black_count = len(self.json_data['black_objects']) - visited_count = sum(1 for obj in self.json_data['black_objects'] if obj.get('visited', False)) - report += f"黑色物体检测: 总数{black_count}个, 已访问{visited_count}个\n" - - report += "="*60 + "\n" - - warnings = [] - if cpu_avg > config.PERFORMANCE['CPU_WARNING_THRESHOLD']: - warnings.append(f"⚠️ CPU使用率过高: {cpu_avg:.1f}%") - - if mem_avg > config.PERFORMANCE['MEMORY_WARNING_THRESHOLD']: - warnings.append(f"⚠️ 内存使用率过高: {mem_avg:.1f}%") - - if loop_avg > config.PERFORMANCE['LOOP_TIME_WARNING_THRESHOLD']: - warnings.append(f"⚠️ 循环时间过长: {loop_avg*1000:.1f}ms") - - if warnings: - report += "\n⚠️ 性能警告:\n" - for warning in warnings: - report += f" {warning}\n" - - return report - - except Exception as e: - return f"生成性能报告时出错: {e}" - - -@dataclass -class PerceptionResult: - """感知结果数据结构""" - has_obstacle: bool = False - obstacle_distance: float = 100.0 - obstacle_direction: float = 0.0 - terrain_slope: float = 0.0 - open_space_score: float = 0.0 - recommended_height: float = config.PERCEPTION['HEIGHT_STRATEGY']['DEFAULT'] - safe_directions: List[float] = None - front_image: Optional[np.ndarray] = None - obstacle_positions: List[Tuple[float, float]] = None - red_objects: List[RedObject] = None - red_objects_count: int = 0 - red_objects_image: Optional[np.ndarray] = None - blue_objects: List[BlueObject] = None - blue_objects_count: int = 0 - blue_objects_image: Optional[np.ndarray] = None - black_objects: List[BlackObject] = None - black_objects_count: int = 0 - black_objects_image: Optional[np.ndarray] = None - - def __post_init__(self): - if self.safe_directions is None: - self.safe_directions = [] - if self.obstacle_positions is None: - self.obstacle_positions = [] - if self.red_objects is None: - self.red_objects = [] - if self.blue_objects is None: - self.blue_objects = [] - if self.black_objects is None: - self.black_objects = [] - - -class VectorFieldPlanner: - """向量场规划器""" - def __init__(self): - self.repulsion_gain = config.INTELLIGENT_DECISION['OBSTACLE_REPULSION_GAIN'] - self.attraction_gain = config.INTELLIGENT_DECISION['GOAL_ATTRACTION_GAIN'] - self.field_radius = config.INTELLIGENT_DECISION['VECTOR_FIELD_RADIUS'] - self.smoothing_factor = config.INTELLIGENT_DECISION['SMOOTHING_FACTOR'] - self.red_attraction_gain = config.INTELLIGENT_DECISION['RED_OBJECT_EXPLORATION']['ATTRACTION_GAIN'] - self.blue_attraction_gain = config.INTELLIGENT_DECISION['BLUE_OBJECT_EXPLORATION']['ATTRACTION_GAIN'] - self.black_attraction_gain = config.INTELLIGENT_DECISION['BLACK_OBJECT_EXPLORATION']['ATTRACTION_GAIN'] - - self.min_turn_angle = math.radians(config.INTELLIGENT_DECISION['MIN_TURN_ANGLE_DEG']) - self.max_turn_angle = math.radians(config.INTELLIGENT_DECISION['MAX_TURN_ANGLE_DEG']) - - self.vector_history = deque(maxlen=config.INTELLIGENT_DECISION['SMOOTHING_WINDOW_SIZE']) - self.current_vector = Vector2D() - - def compute_vector(self, current_pos, goal_pos, obstacles, red_objects=None, blue_objects=None, black_objects=None): - attraction_vector = self._compute_attraction(current_pos, goal_pos) - repulsion_vector = self._compute_repulsion(current_pos, obstacles) - red_attraction_vector = Vector2D() - blue_attraction_vector = Vector2D() - black_attraction_vector = Vector2D() - - if red_objects: - red_attraction_vector = self._compute_red_attraction(current_pos, red_objects) - - if blue_objects: - blue_attraction_vector = self._compute_blue_attraction(current_pos, blue_objects) - - if black_objects: - black_attraction_vector = self._compute_black_attraction(current_pos, black_objects) - - combined_vector = attraction_vector + repulsion_vector + red_attraction_vector + blue_attraction_vector + black_attraction_vector - smoothed_vector = self._smooth_vector(combined_vector) - limited_vector = self._limit_turn_angle(smoothed_vector) - - self.current_vector = limited_vector - return limited_vector - - def _compute_attraction(self, current_pos, goal_pos): - if goal_pos is None: - return Vector2D() - - dx = goal_pos[0] - current_pos[0] - dy = goal_pos[1] - current_pos[1] - distance = math.sqrt(dx**2 + dy**2) - - if distance < 0.1: - return Vector2D() - - strength = min(self.attraction_gain, self.attraction_gain / max(1.0, distance)) - return Vector2D(dx, dy).normalize() * strength - - def _compute_repulsion(self, current_pos, obstacles): - repulsion = Vector2D() - - for obs_x, obs_y in obstacles: - dx = current_pos[0] - obs_x - dy = current_pos[1] - obs_y - distance = math.sqrt(dx**2 + dy**2) - - if distance < self.field_radius and distance > 0.1: - strength = self.repulsion_gain * (1.0 / distance**2) - direction = Vector2D(dx, dy).normalize() - repulsion += direction * strength - - return repulsion - - def _compute_red_attraction(self, current_pos, red_objects): - attraction = Vector2D() - - for obj in red_objects: - if not obj.visited: - dx = obj.position[0] - current_pos[0] - dy = obj.position[1] - current_pos[1] - distance = math.sqrt(dx**2 + dy**2) - - if distance < config.INTELLIGENT_DECISION['RED_OBJECT_EXPLORATION']['DETECTION_RADIUS']: - strength = self.red_attraction_gain / max(1.0, distance) - direction = Vector2D(dx, dy).normalize() - attraction += direction * strength - - return attraction - - def _compute_blue_attraction(self, current_pos, blue_objects): - attraction = Vector2D() - - for obj in blue_objects: - if not obj.visited: - dx = obj.position[0] - current_pos[0] - dy = obj.position[1] - current_pos[1] - distance = math.sqrt(dx**2 + dy**2) - - if distance < config.INTELLIGENT_DECISION['BLUE_OBJECT_EXPLORATION']['DETECTION_RADIUS']: - strength = self.blue_attraction_gain / max(1.0, distance) - direction = Vector2D(dx, dy).normalize() - attraction += direction * strength - - return attraction - - def _compute_black_attraction(self, current_pos, black_objects): - attraction = Vector2D() - - for obj in black_objects: - if not obj.visited: - dx = obj.position[0] - current_pos[0] - dy = obj.position[1] - current_pos[1] - distance = math.sqrt(dx**2 + dy**2) - - if distance < config.INTELLIGENT_DECISION['BLACK_OBJECT_EXPLORATION']['DETECTION_RADIUS']: - strength = self.black_attraction_gain / max(1.0, distance) - direction = Vector2D(dx, dy).normalize() - attraction += direction * strength - - return attraction - - def _smooth_vector(self, new_vector): - self.vector_history.append(new_vector) - - if len(self.vector_history) < 2: - return new_vector - - smoothed = Vector2D() - total_weight = 0.0 - - for i, vec in enumerate(reversed(self.vector_history)): - weight = math.exp(-i * self.smoothing_factor) - smoothed += vec * weight - total_weight += weight - - if total_weight > 0: - smoothed = smoothed / total_weight - - return smoothed - - def _limit_turn_angle(self, vector): - if self.current_vector.magnitude() < 0.1: - return vector - - current_angle = math.atan2(self.current_vector.y, self.current_vector.x) - new_angle = math.atan2(vector.y, vector.x) - - angle_diff = new_angle - current_angle - angle_diff = (angle_diff + math.pi) % (2 * math.pi) - math.pi - - if abs(angle_diff) > self.max_turn_angle: - angle_diff = math.copysign(self.max_turn_angle, angle_diff) - elif abs(angle_diff) < self.min_turn_angle and vector.magnitude() > 0.1: - angle_diff = math.copysign(self.min_turn_angle, angle_diff) - - magnitude = vector.magnitude() - limited_angle = current_angle + angle_diff - - return Vector2D.from_angle(limited_angle, magnitude) - - -def _load_chinese_font(font_size=20): - """加载中文字体，使用缓存避免重复加载""" - global _chinese_font_cache - - # 检查缓存 - cache_key = font_size - if cache_key in _chinese_font_cache: - return _chinese_font_cache[cache_key] - - if not PIL_AVAILABLE: - return None - - font = None - font_paths = [] - - # Windows系统字体路径 - if platform.system() == "Windows": - windir = os.environ.get('WINDIR', 'C:\\Windows') - font_dir = os.path.join(windir, 'Fonts') - - # 首先尝试已知的中文字体文件名 - known_fonts = [ - "simhei.ttf", # 黑体 - "msyh.ttc", # 微软雅黑 - "msyhbd.ttc", # 微软雅黑 Bold - "simsun.ttc", # 宋体 - "simkai.ttf", # 楷体 - "simli.ttf", # 隶书 - "STHeiti.ttf", # 华文黑体 - "STSong.ttf", # 华文宋体 - ] - - for font_name in known_fonts: - font_path = os.path.join(font_dir, font_name) - font_paths.append(font_path) - - # 如果找不到，尝试扫描字体目录 - if os.path.exists(font_dir): - try: - for filename in os.listdir(font_dir): - filename_lower = filename.lower() - # 检查文件名是否包含中文字体关键词 - if any(keyword in filename_lower for keyword in ['simhei', 'msyh', 'simsun', 'simkai', 'simli', 'stheit', 'stsong', 'chinese', 'cjk']): - font_path = os.path.join(font_dir, filename) - if font_path not in font_paths: - font_paths.append(font_path) - except: - pass - - # 也尝试常见的路径格式 - common_paths = [ - "C:/Windows/Fonts/simhei.ttf", - "C:/Windows/Fonts/msyh.ttc", - "C:/Windows/Fonts/simsun.ttc", - "C:/Windows/Fonts/msyhbd.ttc", - "C:/Windows/Fonts/simkai.ttf", - "C:/Windows/Fonts/simli.ttf", - ] - font_paths.extend(common_paths) - - # 去重 - font_paths = list(dict.fromkeys(font_paths)) - - # 尝试加载字体 - loaded_font_path = None - for font_path in font_paths: - if os.path.exists(font_path): - try: - font = ImageFont.truetype(font_path, font_size) - # 如果成功加载，缓存字体 - loaded_font_path = font_path - _chinese_font_cache[cache_key] = font - break - except Exception as e: - continue - - # 如果找到了字体，打印信息（仅第一次） - if loaded_font_path and cache_key == 20: # 只在第一次加载时打印 - print(f"✅ 成功加载中文字体: {os.path.basename(loaded_font_path)}") - - # 如果找不到字体，缓存None并打印警告（仅第一次） - global _chinese_font_disabled - if font is None: - # 立即设置禁用标志，避免后续尝试 - _chinese_font_disabled = True - if cache_key == 20: # 只在第一次加载时打印 - print("⚠️ 未找到中文字体，将使用英文显示") - if platform.system() == "Windows": - windir = os.environ.get('WINDIR', 'C:\\Windows') - font_dir = os.path.join(windir, 'Fonts') - print(f" 字体目录: {font_dir}") - print(f" 请确保该目录存在中文字体文件（如simhei.ttf, msyh.ttc等）") - - _chinese_font_cache[cache_key] = font - return font - - -def _translate_to_english(text): - """将中文文本转换为英文""" - # 按长度从长到短排序，确保先替换长的短语，避免部分替换问题 - translations = [ - ("手动控制中...", "Manual Ctrl"), - ("等待无人机图像...", "Waiting..."), - ("系统正在初始化，请稍候...", "Initializing..."), - ("按 Q 或 ESC 关闭窗口", "Press Q/ESC to close"), - ("无人机信息面板", "Info Panel"), - ("红色物体检查", "Red Object Inspection"), - ("蓝色物体检查", "Blue Object Inspection"), - ("黑色物体检查", "Black Object Inspection"), - ("飞行状态:", "State:"), - ("障碍距离:", "Obs:"), - ("CPU使用率:", "CPU:"), - ("内存使用率:", "Mem:"), - ("循环时间:", "Loop:"), - ("更新时间:", "Time:"), - ("探索网格:", "Grid:"), - ("红色物体:", "Red:"), - ("红色物体", "Red Object"), # 图例标签（无冒号） - ("蓝色物体:", "Blue:"), - ("蓝色物体", "Blue Object"), # 图例标签（无冒号） - ("黑色物体:", "Black:"), - ("黑色物体", "Black Object"), # 图例标签（无冒号） - ("主动探索", "Exploring"), # 完整状态值（必须在"探索"之前） - ("探索前沿", "Frontier"), - ("当前位置", "Current"), - ("等待数据...", "Waiting..."), - ("渲染错误", "Render Error"), - ("状态:", "State:"), - ("位置:", "Pos:"), - ("障碍:", "Obs:"), - ("开阔度:", "Open:"), - ("高度:", "Height:"), - ("图例:", "Legend:"), - ("障碍物", "Obstacle"), - ("已探索", "Explored"), - ("前沿", "Frontier"), - ("悬停", "Hovering"), - ("手动控制", "Manual"), - ("紧急", "Emergency"), - ("探索", "Exploring"), # 放在最后，避免与"主动探索"冲突 - ("主动", ""), # 单独处理剩余的"主动"，避免显示问号 - ] - - result = text - for chinese, english in translations: - result = result.replace(chinese, english) - - return result - - -def put_chinese_text(img, text, position, font_size=20, color=(255, 255, 255), thickness=1): - """ - 在OpenCV图像上绘制中文文本 - 使用PIL/Pillow来支持中文显示，如果失败则回退到英文 - """ - global _chinese_font_disabled - - # 如果已禁用中文显示，直接使用英文 - if _chinese_font_disabled: - text_en = _translate_to_english(text) - cv2.putText(img, text_en, position, cv2.FONT_HERSHEY_SIMPLEX, font_size / 30.0, color, thickness) - return img - - # 首先检查PIL是否可用 - if not PIL_AVAILABLE: - _chinese_font_disabled = True - text_en = _translate_to_english(text) - cv2.putText(img, text_en, position, cv2.FONT_HERSHEY_SIMPLEX, font_size / 30.0, color, thickness) - return img - - # 尝试加载中文字体 - font = _load_chinese_font(font_size) - - # 如果字体加载失败，立即回退到英文并设置禁用标志 - if font is None: - _chinese_font_disabled = True - text_en = _translate_to_english(text) - cv2.putText(img, text_en, position, cv2.FONT_HERSHEY_SIMPLEX, font_size / 30.0, color, thickness) - return img - - # 尝试使用PIL绘制中文 - try: - # 检查文本是否包含中文字符 - has_chinese = any('\u4e00' <= char <= '\u9fff' for char in text) - - if not has_chinese: - # 如果没有中文字符，直接使用cv2.putText - cv2.putText(img, text, position, cv2.FONT_HERSHEY_SIMPLEX, font_size / 30.0, color, thickness) - return img - - # 将OpenCV图像转换为PIL图像 - img_pil = Image.fromarray(cv2.cvtColor(img, cv2.COLOR_BGR2RGB)) - draw = ImageDraw.Draw(img_pil) - - # 绘制文本（PIL使用RGB颜色） - color_rgb = (color[2], color[1], color[0]) # BGR转RGB - - # PIL的text函数位置参数是(x, y) - x, y = position - - # 尝试绘制文本，如果失败则回退到英文 - try: - draw.text((x, y), text, font=font, fill=color_rgb) - # 转换回OpenCV格式 - img = cv2.cvtColor(np.array(img_pil), cv2.COLOR_RGB2BGR) - except Exception as draw_error: - # 如果绘制失败（可能是字体不支持某些字符），回退到英文 - raise draw_error - - except Exception as e: - # 如果绘制失败，回退到英文显示并设置禁用标志 - _chinese_font_disabled = True - text_en = _translate_to_english(text) - cv2.putText(img, text_en, position, cv2.FONT_HERSHEY_SIMPLEX, font_size / 30.0, color, thickness) - - return img - - -class FrontViewWindow: - """前视窗口 - 显示摄像头画面和手动控制""" - - def __init__(self, window_name=None, width=None, height=None, - enable_sharpening=None, show_info=None): - # 窗口标题使用英文，避免乱码 - if window_name: - # 如果传入的窗口名包含中文，翻译为英文 - self.window_name = _translate_to_english(window_name) if any('\u4e00' <= char <= '\u9fff' for char in window_name) else window_name - else: - default_name = _translate_to_english(config.DISPLAY['FRONT_VIEW_WINDOW']['NAME']) - self.window_name = default_name - self.window_width = width if width is not None else config.DISPLAY['FRONT_VIEW_WINDOW']['WIDTH'] - self.window_height = height if height is not None else config.DISPLAY['FRONT_VIEW_WINDOW']['HEIGHT'] - self.enable_sharpening = (enable_sharpening if enable_sharpening is not None - else config.DISPLAY['FRONT_VIEW_WINDOW']['ENABLE_SHARPENING']) - self.show_info = (show_info if show_info is not None - else config.DISPLAY['FRONT_VIEW_WINDOW']['SHOW_INFO_OVERLAY']) - - # 内存优化：使用配置的队列大小 - queue_maxsize = config.DISPLAY['FRONT_VIEW_WINDOW'].get('QUEUE_MAXSIZE', 2) - self.image_queue = queue.Queue(maxsize=queue_maxsize) - self.reduce_image_copy = config.DISPLAY['FRONT_VIEW_WINDOW'].get('REDUCE_IMAGE_COPY', True) - self.display_active = True - self.display_thread = None - self.paused = False - - self.manual_mode = False - self.key_states = {} - self.last_keys = {} - - self.exit_manual_flag = False - self.exit_display_flag = False - - self.display_stats = { - 'fps': 0.0, - 'last_update': time.time(), - 'frame_count': 0 - } - - self.start() - - def start(self): - if self.display_thread and self.display_thread.is_alive(): - return - - self.display_active = True - self.display_thread = threading.Thread( - target=self._display_loop, - daemon=True, - name="FrontViewWindow" - ) - self.display_thread.start() - - def stop(self): - self.display_active = False - self.exit_display_flag = True - if self.display_thread: - self.display_thread.join(timeout=2.0) - - def update_image(self, image_data: np.ndarray, info: Optional[Dict] = None, - manual_info: Optional[List[str]] = None): - if not self.display_active or self.paused or image_data is None: - return - - try: - if self.enable_sharpening and image_data is not None and image_data.size > 0: - kernel = np.array([[0, -1, 0], - [-1, 5, -1], - [0, -1, 0]]) - image_data = cv2.filter2D(image_data, -1, kernel) - - if self.image_queue.full(): - try: - self.image_queue.get_nowait() - except queue.Empty: - pass - - # 内存优化：仅在必要时复制图像 - if self.reduce_image_copy and image_data is not None: - # 如果队列为空或只有一个元素，直接使用引用（避免复制） - if self.image_queue.qsize() == 0: - display_image = image_data - else: - display_image = image_data.copy() - else: - display_image = image_data.copy() if image_data is not None else None - - display_packet = { - 'image': display_image, - 'info': info.copy() if info else {}, - 'manual_info': manual_info.copy() if manual_info else [], - 'timestamp': time.time() - } - - self.image_queue.put_nowait(display_packet) - - except Exception as e: - print(f"⚠️ 更新图像时出错: {e}") - - def set_manual_mode(self, manual_mode): - self.manual_mode = manual_mode - self.exit_manual_flag = False - self.key_states = {} - self.last_keys = {} - print(f"🔄 {'进入' if manual_mode else '退出'}手动控制模式") - - def get_control_inputs(self): - return self.key_states.copy() - - def should_exit_manual(self): - return self.exit_manual_flag - - def _display_loop(self): - cv2.namedWindow(self.window_name, cv2.WINDOW_NORMAL) - cv2.resizeWindow(self.window_name, self.window_width, self.window_height) - - # 等待画面：只显示黑色背景，不显示任何文字 - wait_img = np.zeros((self.window_height, self.window_width, 3), dtype=np.uint8) - cv2.imshow(self.window_name, wait_img) - cv2.waitKey(100) - - print("💡 前视窗口控制:") - print(" - 通用控制: P=暂停/继续, I=信息显示, H=锐化效果") - print(" - 非手动模式: Q=关闭窗口, S=保存截图") - print(" - 手动模式: ESC=退出手动模式") - print("\n🎮 手动控制键位:") - print(" - W/S: 前进/后退, A/D: 左移/右移") - print(" - Q/E: 上升/下降, Z/X: 左转/右转") - print(" - 空格: 悬停, ESC: 退出手动模式") - - while self.display_active and not self.exit_display_flag: - display_image = None - info = {} - manual_info = [] - - try: - if not self.image_queue.empty(): - packet = self.image_queue.get_nowait() - display_image = packet['image'] - info = packet['info'] - manual_info = packet['manual_info'] - - self._update_stats() - - while not self.image_queue.empty(): - try: - self.image_queue.get_nowait() - except queue.Empty: - break - except queue.Empty: - pass - - if display_image is not None: - # 前视窗口只显示纯图像，不添加任何信息叠加 - cv2.imshow(self.window_name, display_image) - elif self.paused: - # 暂停时也显示黑色背景，不显示任何文字 - blank = np.zeros((self.window_height, self.window_width, 3), dtype=np.uint8) - cv2.imshow(self.window_name, blank) - - key = cv2.waitKey(config.DISPLAY['FRONT_VIEW_WINDOW'].get('REFRESH_RATE_MS', 30)) & 0xFF - - current_keys = {} - if key != 255: - current_keys[key] = True - - if self.manual_mode: - self._handle_manual_mode_key(key) - else: - self._handle_window_control_key(key, display_image) - - self._update_key_states(current_keys) - - try: - if cv2.getWindowProperty(self.window_name, cv2.WND_PROP_VISIBLE) < 1: - print("🔄 用户关闭了前视窗口") - self.display_active = False - break - except: - self.display_active = False - break - - try: - cv2.destroyWindow(self.window_name) - except: - pass - cv2.waitKey(1) - - def _handle_manual_mode_key(self, key): - if key == 27: - print("收到退出手动模式指令") - self.exit_manual_flag = True - return - - self.key_states[key] = True - - if key == 32: - print("⏸️ 悬停指令") - - def _handle_window_control_key(self, key, display_image): - key_char = chr(key).lower() if 0 <= key <= 255 else '' - - if key_char == 'q': - print("🔄 用户关闭显示窗口") - self.display_active = False - elif key_char == 's' and display_image is not None: - self._save_screenshot(display_image) - elif key_char == 'p': - self.paused = not self.paused - status = "已暂停" if self.paused else "已恢复" - print(f"⏸️ 视频流{status}") - elif key_char == 'i': - self.show_info = not self.show_info - status = "开启" if self.show_info else "关闭" - print(f"📊 信息叠加层{status}") - elif key_char == 'h': - self.enable_sharpening = not self.enable_sharpening - status = "开启" if self.enable_sharpening else "关闭" - print(f"🔍 图像锐化{status}") - - def _update_key_states(self, current_keys): - released_keys = [] - for key in list(self.key_states.keys()): - if key not in current_keys: - released_keys.append(key) - - for key in released_keys: - del self.key_states[key] - - self.last_keys = current_keys.copy() - - def _update_stats(self): - now = time.time() - self.display_stats['frame_count'] += 1 - - if now - self.display_stats['last_update'] >= 1.0: - self.display_stats['fps'] = self.display_stats['frame_count'] / (now - self.display_stats['last_update']) - self.display_stats['frame_count'] = 0 - self.display_stats['last_update'] = now - - def _add_info_overlay(self, image: np.ndarray, info: Dict, manual_info: List[str] = None) -> np.ndarray: - """前视窗口不再显示任何信息叠加，只显示纯图像""" - # 直接返回原始图像，不添加任何文字或叠加层 - return image - - def _save_screenshot(self, image: Optional[np.ndarray]): - if image is not None and image.size > 0: - timestamp = time.strftime("%Y%m%d_%H%M%S") - filename = f"drone_snapshot_{timestamp}.png" - cv2.imwrite(filename, image) - print(f"📸 截图已保存: {filename}") - else: - print("⚠️ 无法保存截图：无有效图像数据") - - -class InfoDisplayWindow: - """信息显示窗口 - 显示系统状态、探索网格、物体统计等信息""" - - def __init__(self, window_name=None, width=None, height=None): - # 窗口标题使用英文，避免乱码 - if window_name: - # 如果传入的窗口名包含中文，翻译为英文 - self.window_name = _translate_to_english(window_name) if any('\u4e00' <= char <= '\u9fff' for char in window_name) else window_name - else: - default_name = _translate_to_english(config.DISPLAY['INFO_WINDOW']['NAME']) - self.window_name = default_name - self.window_width = width if width is not None else config.DISPLAY['INFO_WINDOW']['WIDTH'] - self.window_height = height if height is not None else config.DISPLAY['INFO_WINDOW']['HEIGHT'] - - self.display_config = config.DISPLAY['INFO_WINDOW'] - self.info_queue = queue.Queue(maxsize=3) - self.display_active = True - self.display_thread = None - self.last_update = time.time() - - # 轨迹记录 - self.trajectory_points = deque(maxlen=config.DISPLAY['INFO_WINDOW'].get('TRAJECTORY_MAX_POINTS', 1000)) - self.start_position = None - - self.start() - - def start(self): - if self.display_thread and self.display_thread.is_alive(): - return - - self.display_active = True - self.display_thread = threading.Thread( - target=self._display_loop, - daemon=True, - name="InfoDisplayWindow" - ) - self.display_thread.start() - print(f"📊 信息显示窗口已启动: {self.window_name}") - - def stop(self): - self.display_active = False - if self.display_thread: - self.display_thread.join(timeout=2.0) - - def update_info(self, info_data: Dict): - if not self.display_active: - return - - try: - # 更新轨迹数据 - if 'position' in info_data: - pos = info_data['position'] - self.trajectory_points.append((pos[0], pos[1], pos[2])) - - # 记录起始位置 - if self.start_position is None: - self.start_position = (pos[0], pos[1], pos[2]) - - if self.info_queue.full(): - try: - self.info_queue.get_nowait() - except queue.Empty: - pass - - self.info_queue.put_nowait(info_data.copy()) - - except Exception as e: - print(f"⚠️ 更新信息数据时出错: {e}") - - def _display_loop(self): - cv2.namedWindow(self.window_name, cv2.WINDOW_NORMAL) - cv2.resizeWindow(self.window_name, self.window_width, self.window_height) - - wait_img = self._create_waiting_screen() - cv2.imshow(self.window_name, wait_img) - cv2.waitKey(100) - - print("📊 信息显示窗口已就绪") - print(" 显示内容: 探索网格、系统状态、物体统计、性能信息") - - last_render_time = time.time() - info_data = {} - - while self.display_active: - current_time = time.time() - - # 从队列获取最新信息 - try: - while not self.info_queue.empty(): - info_data = self.info_queue.get_nowait() - except queue.Empty: - pass - - # 定期刷新显示 - if current_time - last_render_time >= self.display_config['REFRESH_RATE_MS'] / 1000.0: - display_image = self._render_info_display(info_data) - if display_image is not None: - cv2.imshow(self.window_name, display_image) - last_render_time = current_time - - # 处理窗口事件 - key = cv2.waitKey(10) & 0xFF - if key == ord('q') or key == 27: # Q或ESC关闭窗口 - print("🔄 用户关闭信息窗口") - self.display_active = False - break - - try: - if cv2.getWindowProperty(self.window_name, cv2.WND_PROP_VISIBLE) < 1: - print("🔄 信息窗口被关闭") - self.display_active = False - break - except: - self.display_active = False - break - - try: - cv2.destroyWindow(self.window_name) - except: - pass - cv2.waitKey(1) - - def _create_waiting_screen(self): - """创建等待屏幕""" - img = np.zeros((self.window_height, self.window_width, 3), dtype=np.uint8) - bg_color = self.display_config['BACKGROUND_COLOR'] - img[:, :] = bg_color - - center_x = self.window_width // 2 - center_y = self.window_height // 2 - - # 标题 - title = "无人机信息面板" - img = put_chinese_text(img, title, (center_x - 150, center_y - 100), 36, self.display_config['HIGHLIGHT_COLOR'], 2) - - # 状态信息 - status = "等待数据..." - img = put_chinese_text(img, status, (center_x - 80, center_y), 24, self.display_config['TEXT_COLOR'], 1) - - # 提示 - tip = "系统正在初始化，请稍候..." - img = put_chinese_text(img, tip, (center_x - 120, center_y + 50), 18, self.display_config['TEXT_COLOR'], 1) - - return img - - def _draw_trajectory(self, trajectory_points, current_pos, start_pos, size): - """绘制运动轨迹图""" - try: - # 创建轨迹图（始终返回有效图像） - traj_img = np.zeros((size, size, 3), dtype=np.uint8) - traj_img.fill(30) # 深灰色背景 - - # 收集所有点用于计算边界 - all_points = [] - if trajectory_points: - try: - all_points.extend([(float(p[0]), float(p[1])) for p in trajectory_points if len(p) >= 2]) - except (IndexError, TypeError, ValueError): - pass - if current_pos and len(current_pos) >= 2: - try: - all_points.append((float(current_pos[0]), float(current_pos[1]))) - except (TypeError, ValueError, IndexError): - pass - if start_pos and len(start_pos) >= 2: - try: - all_points.append((float(start_pos[0]), float(start_pos[1]))) - except (TypeError, ValueError, IndexError): - pass - - if not all_points: - # 没有轨迹点时，返回空白图像 - return traj_img - - # 计算边界和缩放 - xs = [p[0] for p in all_points] - ys = [p[1] for p in all_points] - - min_x, max_x = min(xs), max(xs) - min_y, max_y = min(ys), max(ys) - - # 添加边距 - margin = max((max_x - min_x) * 0.1, (max_y - min_y) * 0.1, 5.0) - range_x = max_x - min_x + 2 * margin - range_y = max_y - min_y + 2 * margin - - if range_x == 0: - range_x = 10.0 - if range_y == 0: - range_y = 10.0 - - # 坐标转换函数 - def world_to_pixel(wx, wy): - try: - px = int((wx - min_x + margin) / range_x * size) - py = int((wy - min_y + margin) / range_y * size) - px = max(0, min(size - 1, px)) - py = max(0, min(size - 1, py)) - return px, py - except (ZeroDivisionError, ValueError, TypeError): - return size // 2, size // 2 - - # 绘制轨迹线 - if len(trajectory_points) > 1: - try: - line_color = self.display_config.get('TRAJECTORY_LINE_COLOR', (0, 255, 255)) - for i in range(len(trajectory_points) - 1): - try: - p1 = trajectory_points[i] - p2 = trajectory_points[i + 1] - if len(p1) >= 2 and len(p2) >= 2: - px1, py1 = world_to_pixel(p1[0], p1[1]) - px2, py2 = world_to_pixel(p2[0], p2[1]) - cv2.line(traj_img, (px1, py1), (px2, py2), line_color, 2) - except (IndexError, TypeError, ValueError): - continue - except Exception: - pass - - # 绘制起始点 - if start_pos and len(start_pos) >= 2: - try: - start_color = self.display_config.get('TRAJECTORY_START_COLOR', (255, 255, 0)) - sx, sy = world_to_pixel(start_pos[0], start_pos[1]) - cv2.circle(traj_img, (sx, sy), 6, start_color, -1) - cv2.circle(traj_img, (sx, sy), 6, (255, 255, 255), 1) - except (IndexError, TypeError, ValueError): - pass - - # 绘制当前位置 - if current_pos and len(current_pos) >= 2: - try: - current_color = self.display_config.get('TRAJECTORY_CURRENT_COLOR', (0, 255, 0)) - cx, cy = world_to_pixel(current_pos[0], current_pos[1]) - cv2.circle(traj_img, (cx, cy), 8, current_color, -1) - cv2.circle(traj_img, (cx, cy), 8, (255, 255, 255), 2) - except (IndexError, TypeError, ValueError): - pass - - # 绘制坐标轴 - try: - axis_color = (100, 100, 100) - center_x_px, center_y_px = world_to_pixel(0, 0) - if 0 <= center_x_px < size: - cv2.line(traj_img, (center_x_px, 0), (center_x_px, size), axis_color, 1) - if 0 <= center_y_px < size: - cv2.line(traj_img, (0, center_y_px), (size, center_y_px), axis_color, 1) - except Exception: - pass - - return traj_img - except Exception as e: - # 发生任何错误时返回空白图像 - print(f"⚠️ 绘制轨迹图内部错误: {e}") - traj_img = np.zeros((size, size, 3), dtype=np.uint8) - traj_img.fill(30) - return traj_img - - def _render_info_display(self, info_data: Dict) -> np.ndarray: - """渲染信息显示""" - try: - # 创建背景 - img = np.zeros((self.window_height, self.window_width, 3), dtype=np.uint8) - bg_color = self.display_config['BACKGROUND_COLOR'] - img[:, :] = bg_color - - text_color = self.display_config['TEXT_COLOR'] - highlight_color = self.display_config['HIGHLIGHT_COLOR'] - warning_color = self.display_config['WARNING_COLOR'] - success_color = self.display_config['SUCCESS_COLOR'] - - y_offset = 40 - x_offset = 20 - - # 标题栏 - title = "无人机信息面板" - img = put_chinese_text(img, title, (self.window_width // 2 - 100, 30), 30, highlight_color, 2) - - # 分隔线 - cv2.line(img, (10, 50), (self.window_width - 10, 50), text_color, 1) - - y_offset = 80 - - # 1. 飞行状态信息 - if 'state' in info_data: - state = info_data['state'] - # 状态值已经在_update_info_window中翻译过了，这里直接使用 - state_color = success_color if 'Exploring' in state else highlight_color if 'Hovering' in state else warning_color if 'Emergency' in state else text_color - img = put_chinese_text(img, f"飞行状态: {state}", (x_offset, y_offset), 21, state_color, 2) - y_offset += 30 - - # 2. 位置信息 - if 'position' in info_data: - pos = info_data['position'] - pos_text = f"位置: X:{pos[0]:.1f}m Y:{pos[1]:.1f}m 高度:{-pos[2]:.1f}m" - img = put_chinese_text(img, pos_text, (x_offset, y_offset), 18, text_color, 1) - y_offset += 25 - - # 3. 环境感知信息 - if 'perception' in info_data: - perception = info_data['perception'] - obs_text = f"障碍距离: {perception.get('obstacle_distance', 0):.1f}m" - obs_color = warning_color if perception.get('obstacle_distance', 0) < 5.0 else text_color - img = put_chinese_text(img, obs_text, (x_offset, y_offset), 18, obs_color, 1) - y_offset += 25 - - open_text = f"开阔度: {perception.get('open_space_score', 0):.2f}" - img = put_chinese_text(img, open_text, (x_offset, y_offset), 18, text_color, 1) - y_offset += 25 - - # 4. 物体检测统计 - if 'objects_stats' in info_data: - objects_stats = info_data['objects_stats'] - - # 红色物体统计 - red_total = objects_stats.get('red_total', 0) - red_visited = objects_stats.get('red_visited', 0) - red_text = f"红色物体: {red_visited}/{red_total}" - red_color = success_color if red_visited > 0 else text_color - img = put_chinese_text(img, red_text, (x_offset, y_offset), 21, red_color, 1) - y_offset += 30 - - # 蓝色物体统计 - blue_total = objects_stats.get('blue_total', 0) - blue_visited = objects_stats.get('blue_visited', 0) - blue_text = f"蓝色物体: {blue_visited}/{blue_total}" - blue_color = success_color if blue_visited > 0 else text_color - img = put_chinese_text(img, blue_text, (x_offset, y_offset), 21, blue_color, 1) - y_offset += 30 - - # 黑色物体统计 - black_total = objects_stats.get('black_total', 0) - black_visited = objects_stats.get('black_visited', 0) - if black_total > 0: - black_text = f"黑色物体: {black_visited}/{black_total}" - black_color = success_color if black_visited > 0 else text_color - img = put_chinese_text(img, black_text, (x_offset, y_offset), 21, black_color, 1) - y_offset += 30 - - # 5. 探索网格信息 - if 'grid_stats' in info_data: - grid_stats = info_data['grid_stats'] - frontiers = grid_stats.get('frontiers', 0) - explored = grid_stats.get('explored', 0) - total = grid_stats.get('total', 1) - - grid_text = f"探索网格: {frontiers}前沿 | {explored}/{total}已探索" - img = put_chinese_text(img, grid_text, (x_offset, y_offset), 18, text_color, 1) - y_offset += 25 - - # 探索进度条 - progress = explored / total if total > 0 else 0 - bar_width = 200 - bar_height = 15 - bar_x = x_offset - bar_y = y_offset - - # 背景条 - cv2.rectangle(img, (bar_x, bar_y), (bar_x + bar_width, bar_y + bar_height), (50, 50, 50), -1) - # 进度条 - progress_width = int(bar_width * progress) - progress_color = (0, int(255 * progress), int(255 * (1 - progress))) - cv2.rectangle(img, (bar_x, bar_y), (bar_x + progress_width, bar_y + bar_height), progress_color, -1) - # 边框 - cv2.rectangle(img, (bar_x, bar_y), (bar_x + bar_width, bar_y + bar_height), text_color, 1) - # 进度文本 - progress_text = f"{progress*100:.1f}%" - cv2.putText(img, progress_text, (bar_x + bar_width + 10, bar_y + 12), - cv2.FONT_HERSHEY_SIMPLEX, 0.5, text_color, 1) - y_offset += 35 - - # 6. 系统性能信息 - if 'performance' in info_data: - performance = info_data['performance'] - cpu_usage = performance.get('cpu_usage', 0) - memory_usage = performance.get('memory_usage', 0) - loop_time = performance.get('loop_time', 0) * 1000 # 转换为毫秒 - - cpu_color = warning_color if cpu_usage > 80 else text_color - mem_color = warning_color if memory_usage > 80 else text_color - loop_color = warning_color if loop_time > 200 else text_color - - img = put_chinese_text(img, f"CPU使用率: {cpu_usage:.1f}%", (x_offset, y_offset), 18, cpu_color, 1) - y_offset += 25 - - img = put_chinese_text(img, f"内存使用率: {memory_usage:.1f}%", (x_offset, y_offset), 18, mem_color, 1) - y_offset += 25 - - img = put_chinese_text(img, f"循环时间: {loop_time:.1f}ms", (x_offset, y_offset), 18, loop_color, 1) - y_offset += 25 - - # 7. 手动控制信息（如果处于手动控制模式） - if 'manual_info' in info_data and info_data['manual_info']: - y_offset += 10 # 添加一些间距 - manual_title = "手动控制信息:" - img = put_chinese_text(img, manual_title, (x_offset, y_offset), 18, highlight_color, 2) - y_offset += 25 - - for line in info_data['manual_info']: - img = put_chinese_text(img, line, (x_offset + 10, y_offset), 16, text_color, 1) - y_offset += 22 - - # 8. 探索网格图像（右侧） - if self.display_config['SHOW_GRID'] and 'grid_image' in info_data: - grid_img = info_data['grid_image'] - if grid_img is not None and grid_img.size > 0: - grid_size = self.display_config['GRID_SIZE'] - grid_resized = cv2.resize(grid_img, (grid_size, grid_size)) - - grid_x = self.window_width - grid_size - 20 - grid_y = 80 - - # 添加网格标题（直接使用英文，避免问号问题） - grid_title = "Grid Map" # 直接使用英文，不再翻译 - cv2.putText(img, grid_title, (grid_x, grid_y - 10), cv2.FONT_HERSHEY_SIMPLEX, 18 / 30.0, highlight_color, 1) - - # 不再在网格上方显示状态值，避免问号问题 - - # 添加图例 - legend_y = grid_y + grid_size + 20 - img = put_chinese_text(img, "图例:", (grid_x, legend_y), 15, text_color, 1) - legend_y += 20 - - # 当前位置 - cv2.rectangle(img, (grid_x, legend_y), (grid_x + 15, legend_y + 15), (0, 255, 0), -1) - img = put_chinese_text(img, "当前位置", (grid_x + 20, legend_y + 12), 12, text_color, 1) - legend_y += 25 - - # 障碍物 - cv2.rectangle(img, (grid_x, legend_y), (grid_x + 15, legend_y + 15), (0, 0, 255), -1) - img = put_chinese_text(img, "障碍物", (grid_x + 20, legend_y + 12), 12, text_color, 1) - legend_y += 25 - - # 红色物体 - cv2.rectangle(img, (grid_x, legend_y), (grid_x + 15, legend_y + 15), (0, 100, 255), -1) - img = put_chinese_text(img, "红色物体", (grid_x + 20, legend_y + 12), 12, text_color, 1) - legend_y += 25 - - # 蓝色物体 - cv2.rectangle(img, (grid_x, legend_y), (grid_x + 15, legend_y + 15), (255, 100, 0), -1) - img = put_chinese_text(img, "蓝色物体", (grid_x + 20, legend_y + 12), 12, text_color, 1) - legend_y += 25 - - # 黑色物体 - cv2.rectangle(img, (grid_x, legend_y), (grid_x + 15, legend_y + 15), (128, 128, 128), -1) - img = put_chinese_text(img, "黑色物体", (grid_x + 20, legend_y + 12), 12, text_color, 1) - legend_y += 25 - - # 前沿区域 - cv2.rectangle(img, (grid_x, legend_y), (grid_x + 15, legend_y + 15), (0, 200, 0), -1) - img = put_chinese_text(img, "探索前沿", (grid_x + 20, legend_y + 12), 12, text_color, 1) - - # 将网格图像放到主图像上 - img[grid_y:grid_y+grid_size, grid_x:grid_x+grid_size] = grid_resized - - # 9. 运动轨迹图（最左下角） - if self.display_config.get('SHOW_TRAJECTORY', True): - try: - trajectory_size = self.display_config.get('TRAJECTORY_SIZE', 280) - current_pos = info_data.get('position') - - # 绘制轨迹图 - trajectory_img = self._draw_trajectory( - list(self.trajectory_points), - current_pos, - self.start_position, - trajectory_size - ) - - if trajectory_img is not None and trajectory_img.size > 0: - # 轨迹图放在最左下角，从底部开始 - traj_x = 20 # 左边距 - # 从窗口底部向上，留出底部提示文字的空间（约50像素，确保不重叠） - bottom_hint_height = 50 - traj_y = self.window_height - trajectory_size - bottom_hint_height - - # 确保轨迹图不会超出窗口范围，且不与上方内容重叠（至少距离顶部80像素） - if traj_y >= 80 and traj_y + trajectory_size <= self.window_height - bottom_hint_height: - # 添加轨迹图标题（在轨迹图上方） - traj_title = "Trajectory" # 直接使用英文 - cv2.putText(img, traj_title, (traj_x, traj_y - 10), - cv2.FONT_HERSHEY_SIMPLEX, 18 / 30.0, highlight_color, 1) - - # 确保图像尺寸匹配 - if (traj_y + trajectory_size <= self.window_height and - traj_x + trajectory_size <= self.window_width): - # 将轨迹图放到主图像上 - img[traj_y:traj_y+trajectory_size, traj_x:traj_x+trajectory_size] = trajectory_img - - # 添加轨迹图图例（在轨迹图右侧，使用英文避免乱码） - legend_x = traj_x + trajectory_size + 15 - legend_y = traj_y - - if legend_x + 150 < self.window_width: - # 使用英文避免乱码 - cv2.putText(img, "Legend:", (legend_x, legend_y), - cv2.FONT_HERSHEY_SIMPLEX, 0.5, text_color, 1) - legend_y += 20 - - # 起始位置 - cv2.circle(img, (legend_x + 7, legend_y + 7), 6, - self.display_config.get('TRAJECTORY_START_COLOR', (255, 255, 0)), -1) - cv2.circle(img, (legend_x + 7, legend_y + 7), 6, (255, 255, 255), 1) - cv2.putText(img, "Start", (legend_x + 20, legend_y + 12), - cv2.FONT_HERSHEY_SIMPLEX, 0.4, text_color, 1) - legend_y += 25 - - # 当前位置 - cv2.circle(img, (legend_x + 7, legend_y + 7), 8, - self.display_config.get('TRAJECTORY_CURRENT_COLOR', (0, 255, 0)), -1) - cv2.circle(img, (legend_x + 7, legend_y + 7), 8, (255, 255, 255), 2) - cv2.putText(img, "Current", (legend_x + 20, legend_y + 12), - cv2.FONT_HERSHEY_SIMPLEX, 0.4, text_color, 1) - legend_y += 25 - - # 轨迹线 - cv2.line(img, (legend_x, legend_y + 7), (legend_x + 20, legend_y + 7), - self.display_config.get('TRAJECTORY_LINE_COLOR', (0, 255, 255)), 2) - cv2.putText(img, "Path", (legend_x + 25, legend_y + 12), - cv2.FONT_HERSHEY_SIMPLEX, 0.4, text_color, 1) - - # 显示轨迹点数 - point_count = len(self.trajectory_points) - count_text = f"Points: {point_count}" - cv2.putText(img, count_text, (legend_x, legend_y + 30), - cv2.FONT_HERSHEY_SIMPLEX, 0.4, text_color, 1) - except Exception as e: - # 轨迹图绘制失败时不影响其他显示 - print(f"⚠️ 绘制轨迹图时出错: {e}") - import traceback - traceback.print_exc() - - # 10. 时间戳 - if 'timestamp' in info_data: - timestamp = info_data['timestamp'] - time_text = f"更新时间: {timestamp}" - img = put_chinese_text(img, time_text, (self.window_width - 200, self.window_height - 10), 15, text_color, 1) - - # 11. 底部提示 - hint_text = "按 Q 或 ESC 关闭窗口" - img = put_chinese_text(img, hint_text, (self.window_width // 2 - 80, self.window_height - 30), 15, text_color, 1) - - return img - - except Exception as e: - print(f"⚠️ 渲染信息显示时出错: {e}") - error_img = np.zeros((self.window_height, self.window_width, 3), dtype=np.uint8) - error_img[:, :] = self.display_config['BACKGROUND_COLOR'] - error_img = put_chinese_text(error_img, "渲染错误", (self.window_width // 2 - 50, self.window_height // 2), 30, warning_color, 2) - return error_img - - -class PerceptiveExplorer: - """基于感知的自主探索无人机 - 智能决策增强版（双色物体检测版）""" - - def __init__(self, drone_name=""): - self._setup_logging() - self.logger.info("=" * 60) - self.logger.info("AirSimNH 感知驱动自主探索系统 - 双窗口双色物体检测版") - self.logger.info("=" * 60) - - self.client = None - self.drone_name = drone_name - self._connect_to_airsim() - - try: - self.client.enableApiControl(True, vehicle_name=drone_name) - self.client.armDisarm(True, vehicle_name=drone_name) - self.logger.info("✅ API控制已启用") - except Exception as e: - self.logger.error(f"❌ 启用API控制失败: {e}") - raise - - self.state = FlightState.TAKEOFF - self.state_history = deque(maxlen=20) - self.emergency_flag = False - - self.depth_threshold_near = config.PERCEPTION['DEPTH_NEAR_THRESHOLD'] - self.depth_threshold_safe = config.PERCEPTION['DEPTH_SAFE_THRESHOLD'] - self.min_ground_clearance = config.PERCEPTION['MIN_GROUND_CLEARANCE'] - self.max_pitch_angle = math.radians(config.PERCEPTION['MAX_PITCH_ANGLE_DEG']) - self.scan_angles = config.PERCEPTION['SCAN_ANGLES'] - - self.exploration_time = config.EXPLORATION['TOTAL_TIME'] - self.preferred_speed = config.EXPLORATION['PREFERRED_SPEED'] - self.max_altitude = config.EXPLORATION['MAX_ALTITUDE'] - self.min_altitude = config.EXPLORATION['MIN_ALTITUDE'] - self.base_height = config.EXPLORATION['BASE_HEIGHT'] - self.takeoff_height = config.EXPLORATION['TAKEOFF_HEIGHT'] - - self.vector_planner = VectorFieldPlanner() - self.exploration_grid = ExplorationGrid( - resolution=config.INTELLIGENT_DECISION['GRID_RESOLUTION'], - grid_size=config.INTELLIGENT_DECISION['GRID_SIZE'] - ) - - self.velocity_pid = PIDController( - config.INTELLIGENT_DECISION['PID_KP'], - config.INTELLIGENT_DECISION['PID_KI'], - config.INTELLIGENT_DECISION['PID_KD'] - ) - self.height_pid = PIDController(1.0, 0.1, 0.3) - - self.exploration_target = None - self.target_update_time = 0 - self.target_lifetime = config.INTELLIGENT_DECISION.get('TARGET_LIFETIME', 15.0) - self.target_reached_distance = config.INTELLIGENT_DECISION.get('TARGET_REACHED_DISTANCE', 3.0) - - self.red_objects = [] - self.red_object_id_counter = 0 - self.last_red_detection_time = 0 - self.red_detection_interval = config.PERCEPTION['RED_OBJECT_DETECTION']['UPDATE_INTERVAL'] - self.red_object_memory_time = config.PERCEPTION['RED_OBJECT_DETECTION']['MEMORY_TIME'] - - self.blue_objects = [] - self.blue_object_id_counter = 0 - self.last_blue_detection_time = 0 - self.blue_detection_interval = config.PERCEPTION['BLUE_OBJECT_DETECTION']['UPDATE_INTERVAL'] - self.blue_object_memory_time = config.PERCEPTION['BLUE_OBJECT_DETECTION']['MEMORY_TIME'] - - self.black_objects = [] - self.black_object_id_counter = 0 - self.last_black_detection_time = 0 - self.black_detection_interval = config.PERCEPTION['BLACK_OBJECT_DETECTION']['UPDATE_INTERVAL'] - self.black_object_memory_time = config.PERCEPTION['BLACK_OBJECT_DETECTION']['MEMORY_TIME'] - - self.visited_positions = deque(maxlen=100) - - self.loop_count = 0 - self.start_time = time.time() - self.last_health_check = 0 - self.reconnect_attempts = 0 - self.last_successful_loop = time.time() - - self.data_logger = None - self.last_data_record_time = 0 - self.data_record_interval = config.DATA_RECORDING.get('RECORD_INTERVAL', 0.2) - if config.DATA_RECORDING['ENABLED']: - self._setup_data_logger() - - self.last_performance_report = time.time() - self.performance_report_interval = config.PERFORMANCE.get('REPORT_INTERVAL', 30.0) - - self.stats = { - 'perception_cycles': 0, - 'decision_cycles': 0, - 'exceptions_caught': 0, - 'obstacles_detected': 0, - 'state_changes': 0, - 'front_image_updates': 0, - 'manual_control_time': 0.0, - 'vector_field_updates': 0, - 'grid_updates': 0, - 'data_points_recorded': 0, - 'average_loop_time': 0.0, - 'max_loop_time': 0.0, - 'min_loop_time': 100.0, - 'red_objects_detected': 0, - 'red_objects_visited': 0, - 'blue_objects_detected': 0, - 'blue_objects_visited': 0, - 'black_objects_detected': 0, - 'black_objects_visited': 0, - } - - # 初始化两个窗口 - self.front_window = None - self.info_window = None - self._setup_windows() - - self.manual_control_start = 0 - self.control_keys = {} - - self.logger.info("✅ 系统初始化完成") - self.logger.info(f" 开始时间: {datetime.now().strftime('%H:%M:%S')}") - self.logger.info(f" 预计探索时长: {self.exploration_time}秒") - self.logger.info(f" 智能决策: 向量场避障 + 网格探索 + 三色物体检测") - self.logger.info(f" 显示系统: 双窗口模式 (前视窗口 + 信息窗口)") - if config.DATA_RECORDING['ENABLED']: - self.logger.info(f" 数据记录: CSV + JSON 格式") - if config.PERCEPTION['RED_OBJECT_DETECTION']['ENABLED']: - self.logger.info(f" 红色物体检测: 已启用") - if config.PERCEPTION['BLUE_OBJECT_DETECTION']['ENABLED']: - self.logger.info(f" 蓝色物体检测: 已启用") - if config.PERCEPTION['BLACK_OBJECT_DETECTION']['ENABLED']: - self.logger.info(f" 黑色物体检测: 已启用") - - def _setup_logging(self): - self.logger = logging.getLogger('DroneExplorer') - self.logger.setLevel(getattr(logging, config.SYSTEM['LOG_LEVEL'])) - - if self.logger.hasHandlers(): - self.logger.handlers.clear() - - console_handler = logging.StreamHandler(sys.stdout) - console_format = logging.Formatter('%(asctime)s | %(levelname)-8s | %(message)s', datefmt='%H:%M:%S') - console_handler.setFormatter(console_format) - self.logger.addHandler(console_handler) - - if config.SYSTEM['LOG_TO_FILE']: - try: - file_handler = logging.FileHandler(config.SYSTEM['LOG_FILENAME'], encoding='utf-8') - file_format = logging.Formatter('%(asctime)s | %(name)s | %(levelname)-8s | %(message)s') - file_handler.setFormatter(file_format) - self.logger.addHandler(file_handler) - self.logger.info(f"📝 日志将保存至: {config.SYSTEM['LOG_FILENAME']}") - except Exception as e: - print(f"⚠️ 无法创建日志文件: {e}") - - def _setup_data_logger(self): - try: - self.data_logger = DataLogger( - enable_csv=config.DATA_RECORDING['SAVE_TO_CSV'], - enable_json=config.DATA_RECORDING['SAVE_TO_JSON'], - csv_filename=config.DATA_RECORDING.get('CSV_FILENAME'), - json_filename=config.DATA_RECORDING.get('JSON_FILENAME') - ) - self.logger.info("📊 数据记录器初始化完成") - except Exception as e: - self.logger.error(f"❌ 数据记录器初始化失败: {e}") - self.data_logger = None - - def _connect_to_airsim(self): - max_attempts = config.SYSTEM['MAX_RECONNECT_ATTEMPTS'] - for attempt in range(1, max_attempts + 1): - try: - self.logger.info(f"🔄 尝试连接到AirSim (第{attempt}次)...") - self.client = airsim.MultirotorClient() - self.client.confirmConnection() - self.logger.info("✅ 成功连接到AirSim") - self.reconnect_attempts = 0 - return - except ConnectionRefusedError: - self.logger.warning(f"❌ 连接被拒绝，请确保AirSim正在运行") - except Exception as e: - self.logger.warning(f"❌ 连接失败: {e}") - - if attempt < max_attempts: - self.logger.info(f"⏳ {config.SYSTEM['RECONNECT_DELAY']}秒后重试...") - time.sleep(config.SYSTEM['RECONNECT_DELAY']) - - self.logger.error(f"❌ 经过{max_attempts}次尝试后仍无法连接到AirSim") - self.logger.error("请检查：1. AirSim是否启动 2. 网络设置 3. 防火墙") - sys.exit(1) - - def _check_connection_health(self): - try: - self.client.ping() - self.logger.debug("✅ 连接健康检查通过") - return True - except Exception as e: - self.logger.warning(f"⚠️ 连接健康检查失败: {e}") - try: - self._connect_to_airsim() - return True - except: - return False - - def _setup_windows(self): - """初始化两个显示窗口""" - try: - # 前视窗口（窗口标题使用英文，避免乱码） - front_window_name = _translate_to_english(config.DISPLAY['FRONT_VIEW_WINDOW']['NAME']) - self.front_window = FrontViewWindow( - window_name=f"{front_window_name} - {self.drone_name or 'AirSimNH'}", - width=config.DISPLAY['FRONT_VIEW_WINDOW']['WIDTH'], - height=config.DISPLAY['FRONT_VIEW_WINDOW']['HEIGHT'], - enable_sharpening=config.DISPLAY['FRONT_VIEW_WINDOW']['ENABLE_SHARPENING'], - show_info=config.DISPLAY['FRONT_VIEW_WINDOW']['SHOW_INFO_OVERLAY'] - ) - self.logger.info("🎥 前视窗口已初始化") - - # 信息显示窗口（窗口标题使用英文，避免乱码） - info_window_name = _translate_to_english(config.DISPLAY['INFO_WINDOW']['NAME']) - self.info_window = InfoDisplayWindow( - window_name=f"{info_window_name} - {self.drone_name or 'AirSimNH'}", - width=config.DISPLAY['INFO_WINDOW']['WIDTH'], - height=config.DISPLAY['INFO_WINDOW']['HEIGHT'] - ) - self.logger.info("📊 信息显示窗口已初始化") - - except Exception as e: - self.logger.error(f"❌ 窗口初始化失败: {e}") - - def _update_info_window(self, perception: PerceptionResult): - """更新信息显示窗口""" - if not self.info_window: - return - - try: - # 获取无人机状态 - state = self.client.getMultirotorState(vehicle_name=self.drone_name) - pos = state.kinematics_estimated.position - - # 收集性能信息 - cpu_usage = psutil.cpu_percent(interval=0) if config.PERFORMANCE['ENABLE_REALTIME_METRICS'] else 0.0 - memory_usage = psutil.virtual_memory().percent if config.PERFORMANCE['ENABLE_REALTIME_METRICS'] else 0.0 - - # 准备信息数据 - # 无论字体是否禁用，都提前翻译状态值，避免部分翻译导致问号 - state_value = self.state.value - if any('\u4e00' <= char <= '\u9fff' for char in state_value): - state_value = _translate_to_english(state_value) - - # 获取手动控制信息 - manual_info = None - if self.state == FlightState.MANUAL: - manual_info = self._get_manual_control_info() - - info_data = { - 'timestamp': datetime.now().strftime("%H:%M:%S"), - 'state': state_value, - 'position': (pos.x_val, pos.y_val, pos.z_val), - 'perception': { - 'obstacle_distance': perception.obstacle_distance, - 'open_space_score': perception.open_space_score, - 'has_obstacle': perception.has_obstacle - }, - 'objects_stats': { - 'red_total': len(self.red_objects), - 'red_visited': sum(1 for obj in self.red_objects if obj.visited), - 'blue_total': len(self.blue_objects), - 'blue_visited': sum(1 for obj in self.blue_objects if obj.visited), - 'black_total': len(self.black_objects), - 'black_visited': sum(1 for obj in self.black_objects if obj.visited), - 'red_in_view': perception.red_objects_count, - 'blue_in_view': perception.blue_objects_count, - 'black_in_view': perception.black_objects_count - }, - 'grid_stats': { - 'frontiers': len(self.exploration_grid.frontier_cells), - 'explored': np.sum(self.exploration_grid.grid > 0.7), - 'total': self.exploration_grid.grid_size * self.exploration_grid.grid_size - }, - 'performance': { - 'cpu_usage': cpu_usage, - 'memory_usage': memory_usage, - 'loop_time': self.stats.get('average_loop_time', 0) - }, - 'manual_info': manual_info # 添加手动控制信息 - } - - # 添加网格图像 - if config.DISPLAY['INFO_WINDOW']['SHOW_GRID']: - grid_img = self.exploration_grid.visualize_grid(size=config.DISPLAY['INFO_WINDOW']['GRID_SIZE']) - info_data['grid_image'] = grid_img - - # 更新信息窗口 - self.info_window.update_info(info_data) - - except Exception as e: - self.logger.warning(f"⚠️ 更新信息窗口时出错: {e}") - - def _detect_red_objects(self, image: np.ndarray, depth_array: Optional[np.ndarray] = None) -> Tuple[List[RedObject], np.ndarray]: - red_objects = [] - marked_image = image.copy() if image is not None else None - - if not config.PERCEPTION['RED_OBJECT_DETECTION']['ENABLED'] or image is None: - return red_objects, marked_image - - try: - hsv = cv2.cvtColor(image, cv2.COLOR_BGR2HSV) - - lower_red1 = np.array(config.CAMERA['RED_COLOR_RANGE']['LOWER1']) - upper_red1 = np.array(config.CAMERA['RED_COLOR_RANGE']['UPPER1']) - lower_red2 = np.array(config.CAMERA['RED_COLOR_RANGE']['LOWER2']) - upper_red2 = np.array(config.CAMERA['RED_COLOR_RANGE']['UPPER2']) - - mask1 = cv2.inRange(hsv, lower_red1, upper_red1) - mask2 = cv2.inRange(hsv, lower_red2, upper_red2) - red_mask = cv2.bitwise_or(mask1, mask2) - - kernel = np.ones((5, 5), np.uint8) - red_mask = cv2.morphologyEx(red_mask, cv2.MORPH_CLOSE, kernel) - red_mask = cv2.morphologyEx(red_mask, cv2.MORPH_OPEN, kernel) - - contours, _ = cv2.findContours(red_mask, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE) - - try: - state = self.client.getMultirotorState(vehicle_name=self.drone_name) - drone_pos = state.kinematics_estimated.position - orientation = state.kinematics_estimated.orientation - roll, pitch, yaw = airsim.to_eularian_angles(orientation) - except: - drone_pos = None - yaw = 0.0 - - for contour in contours: - area = cv2.contourArea(contour) - min_area = config.PERCEPTION['RED_OBJECT_DETECTION']['MIN_AREA'] - max_area = config.PERCEPTION['RED_OBJECT_DETECTION']['MAX_AREA'] - - if min_area <= area <= max_area: - x, y, w, h = cv2.boundingRect(contour) - center_x = x + w // 2 - center_y = y + h // 2 - - aspect_ratio = w / h if h > 0 else 1.0 - confidence = min(1.0, area / 1000.0) * (1.0 / (1.0 + abs(aspect_ratio - 1.0))) - - world_pos = None - if drone_pos is not None and depth_array is not None: - try: - if 0 <= center_y < depth_array.shape[0] and 0 <= center_x < depth_array.shape[1]: - distance = depth_array[center_y, center_x] - - if 0.5 < distance < 50.0: - height, width = depth_array.shape - fov_h = math.radians(90) - - pixel_angle_x = (center_x - width/2) / (width/2) * (fov_h/2) - pixel_angle_y = (center_y - height/2) / (height/2) * (fov_h/2) - - z = distance - x_rel = z * math.tan(pixel_angle_x) - y_rel = z * math.tan(pixel_angle_y) - - world_x = x_rel * math.cos(yaw) - y_rel * math.sin(yaw) + drone_pos.x_val - world_y = x_rel * math.sin(yaw) + y_rel * math.cos(yaw) + drone_pos.y_val - world_z = drone_pos.z_val - - world_pos = (world_x, world_y, world_z) - except: - pass - - red_object = RedObject( - id=self.red_object_id_counter, - position=world_pos if world_pos else (0.0, 0.0, 0.0), - pixel_position=(center_x, center_y), - size=area, - confidence=confidence, - timestamp=time.time(), - last_seen=time.time(), - visited=False - ) - - is_new_object = True - for existing_obj in self.red_objects: - if self._is_same_object(red_object, existing_obj): - existing_obj.last_seen = time.time() - existing_obj.pixel_position = red_object.pixel_position - existing_obj.confidence = max(existing_obj.confidence, confidence) - if world_pos: - existing_obj.position = world_pos - red_object = existing_obj - is_new_object = False - break - - if is_new_object: - self.red_object_id_counter += 1 - red_objects.append(red_object) - self.stats['red_objects_detected'] += 1 - self.logger.info(f"🔴 检测到红色物体 #{red_object.id} (置信度: {confidence:.2f})") - - if self.data_logger and config.DATA_RECORDING['RECORD_RED_OBJECTS']: - self.data_logger.record_red_object(red_object) - else: - red_objects.append(red_object) - - if marked_image is not None: - color = (0, 100, 255) - if red_object.visited: - color = (0, 200, 0) - - cv2.rectangle(marked_image, (x, y), (x+w, y+h), color, 2) - cv2.circle(marked_image, (center_x, center_y), 5, color, -1) - - label = f"R:{red_object.id} ({confidence:.2f})" - cv2.putText(marked_image, label, (x, y-10), - cv2.FONT_HERSHEY_SIMPLEX, 0.5, color, 1) - - current_time = time.time() - self.red_objects = [obj for obj in self.red_objects - if current_time - obj.last_seen < self.red_object_memory_time] - - visited_count = sum(1 for obj in self.red_objects if obj.visited) - if len(red_objects) > 0: - self.logger.debug(f"🔴 当前红色物体: {len(self.red_objects)}个, 已访问: {visited_count}个") - - except Exception as e: - self.logger.warning(f"⚠️ 红色物体检测失败: {e}") - - return red_objects, marked_image - - def _detect_blue_objects(self, image: np.ndarray, depth_array: Optional[np.ndarray] = None) -> Tuple[List[BlueObject], np.ndarray]: - blue_objects = [] - marked_image = image.copy() if image is not None else None - - if not config.PERCEPTION['BLUE_OBJECT_DETECTION']['ENABLED'] or image is None: - return blue_objects, marked_image - - try: - hsv = cv2.cvtColor(image, cv2.COLOR_BGR2HSV) - - lower_blue = np.array(config.CAMERA['BLUE_COLOR_RANGE']['LOWER']) - upper_blue = np.array(config.CAMERA['BLUE_COLOR_RANGE']['UPPER']) - - blue_mask = cv2.inRange(hsv, lower_blue, upper_blue) - - kernel = np.ones((5, 5), np.uint8) - blue_mask = cv2.morphologyEx(blue_mask, cv2.MORPH_CLOSE, kernel) - blue_mask = cv2.morphologyEx(blue_mask, cv2.MORPH_OPEN, kernel) - - contours, _ = cv2.findContours(blue_mask, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE) - - try: - state = self.client.getMultirotorState(vehicle_name=self.drone_name) - drone_pos = state.kinematics_estimated.position - orientation = state.kinematics_estimated.orientation - roll, pitch, yaw = airsim.to_eularian_angles(orientation) - except: - drone_pos = None - yaw = 0.0 - - for contour in contours: - area = cv2.contourArea(contour) - min_area = config.PERCEPTION['BLUE_OBJECT_DETECTION']['MIN_AREA'] - max_area = config.PERCEPTION['BLUE_OBJECT_DETECTION']['MAX_AREA'] - - if min_area <= area <= max_area: - x, y, w, h = cv2.boundingRect(contour) - center_x = x + w // 2 - center_y = y + h // 2 - - aspect_ratio = w / h if h > 0 else 1.0 - confidence = min(1.0, area / 1000.0) * (1.0 / (1.0 + abs(aspect_ratio - 1.0))) - - world_pos = None - if drone_pos is not None and depth_array is not None: - try: - if 0 <= center_y < depth_array.shape[0] and 0 <= center_x < depth_array.shape[1]: - distance = depth_array[center_y, center_x] - - if 0.5 < distance < 50.0: - height, width = depth_array.shape - fov_h = math.radians(90) - - pixel_angle_x = (center_x - width/2) / (width/2) * (fov_h/2) - pixel_angle_y = (center_y - height/2) / (height/2) * (fov_h/2) - - z = distance - x_rel = z * math.tan(pixel_angle_x) - y_rel = z * math.tan(pixel_angle_y) - - world_x = x_rel * math.cos(yaw) - y_rel * math.sin(yaw) + drone_pos.x_val - world_y = x_rel * math.sin(yaw) + y_rel * math.cos(yaw) + drone_pos.y_val - world_z = drone_pos.z_val - - world_pos = (world_x, world_y, world_z) - except: - pass - - blue_object = BlueObject( - id=self.blue_object_id_counter, - position=world_pos if world_pos else (0.0, 0.0, 0.0), - pixel_position=(center_x, center_y), - size=area, - confidence=confidence, - timestamp=time.time(), - last_seen=time.time(), - visited=False - ) - - is_new_object = True - for existing_obj in self.blue_objects: - if self._is_same_object_blue(blue_object, existing_obj): - existing_obj.last_seen = time.time() - existing_obj.pixel_position = blue_object.pixel_position - existing_obj.confidence = max(existing_obj.confidence, confidence) - if world_pos: - existing_obj.position = world_pos - blue_object = existing_obj - is_new_object = False - break - - if is_new_object: - self.blue_object_id_counter += 1 - blue_objects.append(blue_object) - self.stats['blue_objects_detected'] += 1 - self.logger.info(f"🔵 检测到蓝色物体 #{blue_object.id} (置信度: {confidence:.2f})") - - if self.data_logger and config.DATA_RECORDING['RECORD_BLUE_OBJECTS']: - self.data_logger.record_blue_object(blue_object) - else: - blue_objects.append(blue_object) - - if marked_image is not None: - color = (255, 100, 0) - if blue_object.visited: - color = (0, 200, 0) - - cv2.rectangle(marked_image, (x, y), (x+w, y+h), color, 2) - cv2.circle(marked_image, (center_x, center_y), 5, color, -1) - - label = f"B:{blue_object.id} ({confidence:.2f})" - cv2.putText(marked_image, label, (x, y-10), - cv2.FONT_HERSHEY_SIMPLEX, 0.5, color, 1) - - current_time = time.time() - self.blue_objects = [obj for obj in self.blue_objects - if current_time - obj.last_seen < self.blue_object_memory_time] - - visited_count = sum(1 for obj in self.blue_objects if obj.visited) - if len(blue_objects) > 0: - self.logger.debug(f"🔵 当前蓝色物体: {len(self.blue_objects)}个, 已访问: {visited_count}个") - - except Exception as e: - self.logger.warning(f"⚠️ 蓝色物体检测失败: {e}") - - return blue_objects, marked_image - - def _detect_black_objects(self, image: np.ndarray, depth_array: Optional[np.ndarray] = None) -> Tuple[List[BlackObject], np.ndarray]: - black_objects = [] - marked_image = image.copy() if image is not None else None - - if not config.PERCEPTION['BLACK_OBJECT_DETECTION']['ENABLED'] or image is None: - return black_objects, marked_image - - try: - hsv = cv2.cvtColor(image, cv2.COLOR_BGR2HSV) - - lower_black = np.array(config.CAMERA['BLACK_COLOR_RANGE']['LOWER']) - upper_black = np.array(config.CAMERA['BLACK_COLOR_RANGE']['UPPER']) - - black_mask = cv2.inRange(hsv, lower_black, upper_black) - - kernel = np.ones((5, 5), np.uint8) - black_mask = cv2.morphologyEx(black_mask, cv2.MORPH_CLOSE, kernel) - black_mask = cv2.morphologyEx(black_mask, cv2.MORPH_OPEN, kernel) - - contours, _ = cv2.findContours(black_mask, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE) - - try: - state = self.client.getMultirotorState(vehicle_name=self.drone_name) - drone_pos = state.kinematics_estimated.position - orientation = state.kinematics_estimated.orientation - roll, pitch, yaw = airsim.to_eularian_angles(orientation) - except: - drone_pos = None - yaw = 0.0 - - for contour in contours: - area = cv2.contourArea(contour) - min_area = config.PERCEPTION['BLACK_OBJECT_DETECTION']['MIN_AREA'] - max_area = config.PERCEPTION['BLACK_OBJECT_DETECTION']['MAX_AREA'] - - if min_area <= area <= max_area: - x, y, w, h = cv2.boundingRect(contour) - center_x = x + w // 2 - center_y = y + h // 2 - - aspect_ratio = w / h if h > 0 else 1.0 - confidence = min(1.0, area / 1000.0) * (1.0 / (1.0 + abs(aspect_ratio - 1.0))) - - world_pos = None - if drone_pos is not None and depth_array is not None: - try: - if 0 <= center_y < depth_array.shape[0] and 0 <= center_x < depth_array.shape[1]: - distance = depth_array[center_y, center_x] - - if 0.5 < distance < 50.0: - height, width = depth_array.shape - fov_h = math.radians(90) - - pixel_angle_x = (center_x - width/2) / (width/2) * (fov_h/2) - pixel_angle_y = (center_y - height/2) / (height/2) * (fov_h/2) - - z = distance - x_rel = z * math.tan(pixel_angle_x) - y_rel = z * math.tan(pixel_angle_y) - - world_x = x_rel * math.cos(yaw) - y_rel * math.sin(yaw) + drone_pos.x_val - world_y = x_rel * math.sin(yaw) + y_rel * math.cos(yaw) + drone_pos.y_val - world_z = drone_pos.z_val - - world_pos = (world_x, world_y, world_z) - except: - pass - - black_object = BlackObject( - id=self.black_object_id_counter, - position=world_pos if world_pos else (0.0, 0.0, 0.0), - pixel_position=(center_x, center_y), - size=area, - confidence=confidence, - timestamp=time.time(), - last_seen=time.time(), - visited=False - ) - - is_new_object = True - for existing_obj in self.black_objects: - if self._is_same_object_black(black_object, existing_obj): - existing_obj.last_seen = time.time() - existing_obj.pixel_position = black_object.pixel_position - existing_obj.confidence = max(existing_obj.confidence, confidence) - if world_pos: - existing_obj.position = world_pos - black_object = existing_obj - is_new_object = False - break - - if is_new_object: - self.black_object_id_counter += 1 - black_objects.append(black_object) - self.stats['black_objects_detected'] += 1 - self.logger.info(f"⚫ 检测到黑色物体 #{black_object.id} (置信度: {confidence:.2f})") - - if self.data_logger and config.DATA_RECORDING['RECORD_BLACK_OBJECTS']: - self.data_logger.record_black_object(black_object) - else: - black_objects.append(black_object) - - if marked_image is not None: - color = (128, 128, 128) - if black_object.visited: - color = (0, 200, 0) - - cv2.rectangle(marked_image, (x, y), (x+w, y+h), color, 2) - cv2.circle(marked_image, (center_x, center_y), 5, color, -1) - - label = f"K:{black_object.id} ({confidence:.2f})" - cv2.putText(marked_image, label, (x, y-10), - cv2.FONT_HERSHEY_SIMPLEX, 0.5, color, 1) - - current_time = time.time() - self.black_objects = [obj for obj in self.black_objects - if current_time - obj.last_seen < self.black_object_memory_time] - - visited_count = sum(1 for obj in self.black_objects if obj.visited) - if len(black_objects) > 0: - self.logger.debug(f"⚫ 当前黑色物体: {len(self.black_objects)}个, 已访问: {visited_count}个") - - except Exception as e: - self.logger.warning(f"⚠️ 黑色物体检测失败: {e}") - - return black_objects, marked_image - - def _is_same_object(self, obj1: RedObject, obj2: RedObject, distance_threshold=2.0) -> bool: - if obj1.position != (0.0, 0.0, 0.0) and obj2.position != (0.0, 0.0, 0.0): - distance = math.sqrt( - (obj1.position[0] - obj2.position[0])**2 + - (obj1.position[1] - obj2.position[1])**2 - ) - return distance < distance_threshold - - pixel_distance = math.sqrt( - (obj1.pixel_position[0] - obj2.pixel_position[0])**2 + - (obj1.pixel_position[1] - obj2.pixel_position[1])**2 - ) - time_diff = abs(obj1.timestamp - obj2.timestamp) - - return pixel_distance < 50 and time_diff < 5.0 - - def _is_same_object_blue(self, obj1: BlueObject, obj2: BlueObject, distance_threshold=2.0) -> bool: - if obj1.position != (0.0, 0.0, 0.0) and obj2.position != (0.0, 0.0, 0.0): - distance = math.sqrt( - (obj1.position[0] - obj2.position[0])**2 + - (obj1.position[1] - obj2.position[1])**2 - ) - return distance < distance_threshold - - pixel_distance = math.sqrt( - (obj1.pixel_position[0] - obj2.pixel_position[0])**2 + - (obj1.pixel_position[1] - obj2.pixel_position[1])**2 - ) - time_diff = abs(obj1.timestamp - obj2.timestamp) - - return pixel_distance < 50 and time_diff < 5.0 - - def _is_same_object_black(self, obj1: BlackObject, obj2: BlackObject, distance_threshold=2.0) -> bool: - if obj1.position != (0.0, 0.0, 0.0) and obj2.position != (0.0, 0.0, 0.0): - distance = math.sqrt( - (obj1.position[0] - obj2.position[0])**2 + - (obj1.position[1] - obj2.position[1])**2 - ) - return distance < distance_threshold - - pixel_distance = math.sqrt( - (obj1.pixel_position[0] - obj2.pixel_position[0])**2 + - (obj1.pixel_position[1] - obj2.pixel_position[1])**2 - ) - time_diff = abs(obj1.timestamp - obj2.timestamp) - - return pixel_distance < 50 and time_diff < 5.0 - - def _check_red_object_proximity(self, current_pos): - for obj in self.red_objects: - if not obj.visited: - distance = math.sqrt( - (obj.position[0] - current_pos[0])**2 + - (obj.position[1] - current_pos[1])**2 - ) - - min_distance = config.INTELLIGENT_DECISION['RED_OBJECT_EXPLORATION']['MIN_DISTANCE'] - if distance < min_distance: - obj.visited = True - obj.last_seen = time.time() - self.stats['red_objects_visited'] += 1 - - self.logger.info(f"✅ 已访问红色物体 #{obj.id} (距离: {distance:.1f}m)") - - if self.data_logger: - event_data = { - 'object_id': obj.id, - 'position': obj.position, - 'distance': distance, - 'timestamp': time.time() - } - self.data_logger.record_event('red_object_visited', event_data) - - self.change_state(FlightState.RED_OBJECT_INSPECTION) - return True - - return False - - def _check_blue_object_proximity(self, current_pos): - for obj in self.blue_objects: - if not obj.visited: - distance = math.sqrt( - (obj.position[0] - current_pos[0])**2 + - (obj.position[1] - current_pos[1])**2 - ) - - min_distance = config.INTELLIGENT_DECISION['BLUE_OBJECT_EXPLORATION']['MIN_DISTANCE'] - if distance < min_distance: - obj.visited = True - obj.last_seen = time.time() - self.stats['blue_objects_visited'] += 1 - - self.logger.info(f"✅ 已访问蓝色物体 #{obj.id} (距离: {distance:.1f}m)") - - if self.data_logger: - event_data = { - 'object_id': obj.id, - 'position': obj.position, - 'distance': distance, - 'timestamp': time.time() - } - self.data_logger.record_event('blue_object_visited', event_data) - - self.change_state(FlightState.BLUE_OBJECT_INSPECTION) - return True - - return False - - def _check_black_object_proximity(self, current_pos): - for obj in self.black_objects: - if not obj.visited: - distance = math.sqrt( - (obj.position[0] - current_pos[0])**2 + - (obj.position[1] - current_pos[1])**2 - ) - - min_distance = config.INTELLIGENT_DECISION['BLACK_OBJECT_EXPLORATION']['MIN_DISTANCE'] - if distance < min_distance: - obj.visited = True - obj.last_seen = time.time() - self.stats['black_objects_visited'] += 1 - - self.logger.info(f"✅ 已访问黑色物体 #{obj.id} (距离: {distance:.1f}m)") - - if self.data_logger: - event_data = { - 'object_id': obj.id, - 'position': obj.position, - 'distance': distance, - 'timestamp': time.time() - } - self.data_logger.record_event('black_object_visited', event_data) - - self.change_state(FlightState.BLACK_OBJECT_INSPECTION) - return True - - return False - - def get_depth_perception(self) -> PerceptionResult: - result = PerceptionResult() - self.stats['perception_cycles'] += 1 - - try: - if config.SYSTEM['ENABLE_HEALTH_CHECK']: - current_time = time.time() - if current_time - self.last_successful_loop > 10.0: - self.logger.warning("⚠️ 感知循环长时间无响应，尝试恢复...") - self._check_connection_health() - - camera_name = config.CAMERA['DEFAULT_NAME'] - responses = self.client.simGetImages([ - airsim.ImageRequest( - camera_name, - airsim.ImageType.DepthPlanar, - pixels_as_float=True, - compress=False - ), - airsim.ImageRequest( - camera_name, - airsim.ImageType.Scene, - False, - False - ) - ]) - - if not responses or len(responses) < 2: - self.logger.warning("⚠️ 图像获取失败：响应为空或数量不足") - return result - - depth_img = responses[0] - depth_array = None - if depth_img and hasattr(depth_img, 'image_data_float'): - try: - depth_array = np.array(depth_img.image_data_float, dtype=np.float32) - depth_array = depth_array.reshape(depth_img.height, depth_img.width) - - h, w = depth_array.shape - - front_near = depth_array[h // 2:, w // 3:2 * w // 3] - min_front_distance = np.min(front_near) if front_near.size > 0 else 100 - - directions = [] - for angle_deg in self.scan_angles: - angle_rad = math.radians(angle_deg) - col = int(w / 2 + (w / 2) * math.tan(angle_rad) * 0.5) - col = max(0, min(w - 1, col)) - - col_data = depth_array[h // 2:, col] - if col_data.size > 0: - dir_distance = np.percentile(col_data, 25) - directions.append((angle_rad, dir_distance)) - - if dir_distance > self.depth_threshold_safe: - result.safe_directions.append(angle_rad) - - result.obstacle_positions = self._extract_obstacle_positions(depth_array, h, w) - - ground_region = depth_array[3 * h // 4:, :] - if ground_region.size > 10: - row_variances = np.var(ground_region, axis=1) - result.terrain_slope = np.mean(row_variances) * 100 - - open_pixels = np.sum(depth_array[h // 2:, :] > self.depth_threshold_safe) - total_pixels = depth_array[h // 2:, :].size - result.open_space_score = open_pixels / total_pixels if total_pixels > 0 else 0 - - result.has_obstacle = min_front_distance < self.depth_threshold_near - result.obstacle_distance = min_front_distance - if result.has_obstacle: - self.stats['obstacles_detected'] += 1 - - if directions: - closest_dir = min(directions, key=lambda x: x[1]) - result.obstacle_direction = closest_dir[0] - - if result.terrain_slope > config.PERCEPTION['HEIGHT_STRATEGY']['SLOPE_THRESHOLD']: - result.recommended_height = config.PERCEPTION['HEIGHT_STRATEGY']['STEEP_SLOPE'] - elif result.open_space_score > config.PERCEPTION['HEIGHT_STRATEGY']['OPENNESS_THRESHOLD']: - result.recommended_height = config.PERCEPTION['HEIGHT_STRATEGY']['OPEN_SPACE'] - - except ValueError as e: - self.logger.error(f"❌ 深度图像数据转换错误: {e}") - except Exception as e: - self.logger.error(f"❌ 深度图像处理异常: {e}") - - front_response = responses[1] - if front_response and hasattr(front_response, 'image_data_uint8'): - try: - img_array = np.frombuffer(front_response.image_data_uint8, dtype=np.uint8) - - if len(img_array) > 0: - img_rgb = img_array.reshape(front_response.height, front_response.width, 3) - img_bgr = cv2.cvtColor(img_rgb, cv2.COLOR_RGB2BGR) - - current_time = time.time() - - # 检测红色物体 - if current_time - self.last_red_detection_time >= self.red_detection_interval: - red_objects, red_marked_image = self._detect_red_objects(img_bgr, depth_array) - result.red_objects = red_objects - result.red_objects_count = len(red_objects) - result.red_objects_image = red_marked_image - self.last_red_detection_time = current_time - - # 检测蓝色物体 - if current_time - self.last_blue_detection_time >= self.blue_detection_interval: - blue_objects, blue_marked_image = self._detect_blue_objects(img_bgr, depth_array) - result.blue_objects = blue_objects - result.blue_objects_count = len(blue_objects) - result.blue_objects_image = blue_marked_image - self.last_blue_detection_time = current_time - - # 检测黑色物体 - if current_time - self.last_black_detection_time >= self.black_detection_interval: - black_objects, black_marked_image = self._detect_black_objects(img_bgr, depth_array) - result.black_objects = black_objects - result.black_objects_count = len(black_objects) - result.black_objects_image = black_marked_image - self.last_black_detection_time = current_time - - result.front_image = img_bgr - - display_info = self._prepare_display_info(result) - - self._update_exploration_grid(result) - - self._record_flight_data(result) - - # 更新信息窗口 - self._update_info_window(result) - - if self.front_window: - manual_info = None - if self.state == FlightState.MANUAL: - manual_info = self._get_manual_control_info() - - # 内存优化：仅在需要标记时才复制图像 - has_markers = (config.DISPLAY['FRONT_VIEW_WINDOW']['SHOW_RED_OBJECTS'] and result.red_objects_image is not None) or \ - (config.DISPLAY['FRONT_VIEW_WINDOW']['SHOW_BLUE_OBJECTS'] and result.blue_objects_image is not None) or \ - (config.DISPLAY['FRONT_VIEW_WINDOW']['SHOW_BLACK_OBJECTS'] and result.black_objects_image is not None) - - if has_markers: - display_image = img_bgr.copy() - if config.DISPLAY['FRONT_VIEW_WINDOW']['SHOW_RED_OBJECTS'] and result.red_objects_image is not None: - red_mask = cv2.inRange(result.red_objects_image, (0, 100, 0), (0, 255, 255)) - display_image[red_mask > 0] = result.red_objects_image[red_mask > 0] - - if config.DISPLAY['FRONT_VIEW_WINDOW']['SHOW_BLUE_OBJECTS'] and result.blue_objects_image is not None: - blue_mask = cv2.inRange(result.blue_objects_image, (255, 100, 0), (255, 255, 255)) - display_image[blue_mask > 0] = result.blue_objects_image[blue_mask > 0] - - if config.DISPLAY['FRONT_VIEW_WINDOW']['SHOW_BLACK_OBJECTS'] and result.black_objects_image is not None: - black_mask = cv2.inRange(result.black_objects_image, (128, 128, 0), (128, 255, 255)) - display_image[black_mask > 0] = result.black_objects_image[black_mask > 0] - else: - # 没有标记时直接使用原图像引用 - display_image = img_bgr - - self.front_window.update_image(display_image, display_info, manual_info) - self.stats['front_image_updates'] += 1 - - except Exception as e: - self.logger.warning(f"⚠️ 前视图像处理异常: {e}") - - self.last_successful_loop = time.time() - - if self.loop_count % 50 == 0 and config.DEBUG.get('LOG_DECISION_DETAILS', False): - self.logger.debug(f"感知结果: 障碍={result.has_obstacle}, 距离={result.obstacle_distance:.1f}m, " - f"开阔度={result.open_space_score:.2f}, 红色物体={result.red_objects_count}个, " - f"蓝色物体={result.blue_objects_count}个, 黑色物体={result.black_objects_count}个") - - except Exception as e: - if "ClientException" in str(type(e)) or "Connection" in str(e): - self.logger.error(f"❌ AirSim客户端异常: {e}") - self.stats['exceptions_caught'] += 1 - if self.data_logger: - self.data_logger.record_event('airsim_exception', {'error': str(e)}) - self._check_connection_health() - else: - self.logger.error(f"❌ 感知过程中发生未知异常: {e}") - self.logger.debug(f"异常详情: {traceback.format_exc()}") - self.stats['exceptions_caught'] += 1 - if self.data_logger: - self.data_logger.record_event('perception_exception', {'error': str(e)}) - - return result - - def _record_flight_data(self, perception: PerceptionResult): - if not config.DATA_RECORDING['ENABLED'] or not self.data_logger: - return - - current_time = time.time() - if current_time - self.last_data_record_time < self.data_record_interval: - return - - try: - state = self.client.getMultirotorState(vehicle_name=self.drone_name) - pos = state.kinematics_estimated.position - vel = state.kinematics_estimated.linear_velocity - orientation = state.kinematics_estimated.orientation - - roll, pitch, yaw = airsim.to_eularian_angles(orientation) - - cpu_usage = psutil.cpu_percent(interval=0) if config.PERFORMANCE['ENABLE_REALTIME_METRICS'] else 0.0 - memory_usage = psutil.virtual_memory().percent if config.PERFORMANCE['ENABLE_REALTIME_METRICS'] else 0.0 - - red_objects_count = perception.red_objects_count - red_objects_visited = sum(1 for obj in self.red_objects if obj.visited) - - blue_objects_count = perception.blue_objects_count - blue_objects_visited = sum(1 for obj in self.blue_objects if obj.visited) - - black_objects_count = perception.black_objects_count - black_objects_visited = sum(1 for obj in self.black_objects if obj.visited) - - data_dict = { - 'timestamp': datetime.now().isoformat(), - 'loop_count': self.loop_count, - 'state': self.state.value, - 'pos_x': pos.x_val, - 'pos_y': pos.y_val, - 'pos_z': pos.z_val, - 'vel_x': vel.x_val, - 'vel_y': vel.y_val, - 'vel_z': vel.z_val, - 'yaw': yaw, - 'pitch': pitch, - 'roll': roll, - 'obstacle_distance': perception.obstacle_distance, - 'open_space_score': perception.open_space_score, - 'terrain_slope': perception.terrain_slope, - 'has_obstacle': perception.has_obstacle, - 'obstacle_direction': perception.obstacle_direction, - 'recommended_height': perception.recommended_height, - 'target_x': self.exploration_target[0] if self.exploration_target else 0.0, - 'target_y': self.exploration_target[1] if self.exploration_target else 0.0, - 'target_z': perception.recommended_height, - 'cpu_usage': cpu_usage, - 'memory_usage': memory_usage, - 'grid_frontiers': len(self.exploration_grid.frontier_cells), - 'grid_explored': np.sum(self.exploration_grid.grid > 0.7), - 'adaptive_speed_factor': self._calculate_adaptive_speed(perception, 0) if hasattr(self, '_calculate_adaptive_speed') else 1.0, - 'red_objects_count': red_objects_count, - 'red_objects_detected': self.stats['red_objects_detected'], - 'red_objects_visited': red_objects_visited, - 'blue_objects_count': blue_objects_count, - 'blue_objects_detected': self.stats['blue_objects_detected'], - 'blue_objects_visited': blue_objects_visited, - 'black_objects_count': black_objects_count, - 'black_objects_detected': self.stats['black_objects_detected'], - 'black_objects_visited': black_objects_visited, - } - - self.data_logger.record_flight_data(data_dict) - self.stats['data_points_recorded'] += 1 - self.last_data_record_time = current_time - - except Exception as e: - self.logger.warning(f"⚠️ 记录飞行数据时出错: {e}") - - def _extract_obstacle_positions(self, depth_array, height, width): - obstacles = [] - - try: - state = self.client.getMultirotorState(vehicle_name=self.drone_name) - pos = state.kinematics_estimated.position - orientation = state.kinematics_estimated.orientation - roll, pitch, yaw = airsim.to_eularian_angles(orientation) - - near_mask = depth_array < self.depth_threshold_near * 1.5 - - step = 4 - for i in range(0, height, step): - for j in range(0, width, step): - if near_mask[i, j]: - distance = depth_array[i, j] - - fov_h = math.radians(90) - pixel_angle_x = (j - width/2) / (width/2) * (fov_h/2) - pixel_angle_y = (i - height/2) / (height/2) * (fov_h/2) - - z = distance - x = z * math.tan(pixel_angle_x) - y = z * math.tan(pixel_angle_y) - - world_x = x * math.cos(yaw) - y * math.sin(yaw) + pos.x_val - world_y = x * math.sin(yaw) + y * math.cos(yaw) + pos.y_val - - obstacles.append((world_x, world_y)) - - max_obstacles = 20 - if len(obstacles) > max_obstacles: - obstacles = random.sample(obstacles, max_obstacles) - - except Exception as e: - self.logger.warning(f"⚠️ 提取障碍物位置失败: {e}") - - return obstacles - - def _update_exploration_grid(self, perception: PerceptionResult): - try: - state = self.client.getMultirotorState(vehicle_name=self.drone_name) - pos = state.kinematics_estimated.position - - self.exploration_grid.update_position(pos.x_val, pos.y_val) - - if perception.obstacle_positions: - self.exploration_grid.update_obstacles(perception.obstacle_positions) - - if perception.red_objects: - self.exploration_grid.update_red_objects(perception.red_objects) - - if perception.blue_objects: - self.exploration_grid.update_blue_objects(perception.blue_objects) - - if perception.black_objects: - self.exploration_grid.update_black_objects(perception.black_objects) - - self.stats['grid_updates'] += 1 - - except Exception as e: - self.logger.warning(f"⚠️ 更新探索网格失败: {e}") - - def _prepare_display_info(self, perception: PerceptionResult) -> Dict: - try: - state = self.client.getMultirotorState(vehicle_name=self.drone_name) - pos = state.kinematics_estimated.position - - info = { - 'state': self.state.value, - 'obstacle_distance': perception.obstacle_distance, - 'position': (pos.x_val, pos.y_val, pos.z_val), - 'loop_count': self.loop_count, - 'red_objects_count': perception.red_objects_count, - 'red_objects_visited': sum(1 for obj in self.red_objects if obj.visited), - 'blue_objects_count': perception.blue_objects_count, - 'blue_objects_visited': sum(1 for obj in self.blue_objects if obj.visited), - 'black_objects_count': perception.black_objects_count, - 'black_objects_visited': sum(1 for obj in self.black_objects if obj.visited), - } - - if hasattr(self, 'last_decision_info'): - info['decision_info'] = self.last_decision_info - - if config.DATA_RECORDING['ENABLED']: - info['data_points'] = self.stats['data_points_recorded'] - - return info - except: - return {} - - def _get_manual_control_info(self): - info_lines = [] - - if self.control_keys: - key_names = [] - for key in self.control_keys: - if key == ord('w'): - key_names.append("前进") - elif key == ord('s'): - key_names.append("后退") - elif key == ord('a'): - key_names.append("左移") - elif key == ord('d'): - key_names.append("右移") - elif key == ord('q'): - key_names.append("上升") - elif key == ord('e'): - key_names.append("下降") - elif key == ord('z'): - key_names.append("左转") - elif key == ord('x'): - key_names.append("右转") - elif key == 32: - key_names.append("悬停") - - if key_names: - info_lines.append(f"控制: {', '.join(key_names)}") - else: - info_lines.append("控制: 悬停") - - if self.red_objects: - visited_count = sum(1 for obj in self.red_objects if obj.visited) - info_lines.append(f"红色物体: {visited_count}/{len(self.red_objects)}") - - if self.blue_objects: - visited_count = sum(1 for obj in self.blue_objects if obj.visited) - info_lines.append(f"蓝色物体: {visited_count}/{len(self.blue_objects)}") - - if self.black_objects: - visited_count = sum(1 for obj in self.black_objects if obj.visited) - info_lines.append(f"黑色物体: {visited_count}/{len(self.black_objects)}") - - if self.manual_control_start > 0: - elapsed = time.time() - self.manual_control_start - info_lines.append(f"手动模式: {elapsed:.1f}秒") - - info_lines.append("ESC: 退出手动模式") - - return info_lines - - def apply_manual_control(self): - if self.state != FlightState.MANUAL: - return - - try: - state = self.client.getMultirotorState(vehicle_name=self.drone_name) - pos = state.kinematics_estimated.position - orientation = state.kinematics_estimated.orientation - - _, _, yaw = airsim.to_eularian_angles(orientation) - - vx, vy, vz, yaw_rate = 0.0, 0.0, 0.0, 0.0 - - for key in list(self.control_keys.keys()): - key_char = chr(key).lower() if 0 <= key <= 255 else '' - - if key_char == 'w': - vx += config.MANUAL['CONTROL_SPEED'] * math.cos(yaw) - vy += config.MANUAL['CONTROL_SPEED'] * math.sin(yaw) - elif key_char == 's': - vx -= config.MANUAL['CONTROL_SPEED'] * math.cos(yaw) - vy -= config.MANUAL['CONTROL_SPEED'] * math.sin(yaw) - - if key_char == 'a': - vx += config.MANUAL['CONTROL_SPEED'] * math.cos(yaw + math.pi/2) - vy += config.MANUAL['CONTROL_SPEED'] * math.sin(yaw + math.pi/2) - elif key_char == 'd': - vx += config.MANUAL['CONTROL_SPEED'] * math.cos(yaw - math.pi/2) - vy += config.MANUAL['CONTROL_SPEED'] * math.sin(yaw - math.pi/2) - - if key_char == 'q': - vz = -config.MANUAL['ALTITUDE_SPEED'] - elif key_char == 'e': - vz = config.MANUAL['ALTITUDE_SPEED'] - - if key_char == 'z': - yaw_rate = -math.radians(config.MANUAL['YAW_SPEED']) - elif key_char == 'x': - yaw_rate = math.radians(config.MANUAL['YAW_SPEED']) - - if key == 32: - self.client.hoverAsync(vehicle_name=self.drone_name) - self.control_keys = {} - return - - if config.MANUAL['SAFETY_ENABLED']: - speed = math.sqrt(vx**2 + vy**2) - if speed > config.MANUAL['MAX_MANUAL_SPEED']: - scale = config.MANUAL['MAX_MANUAL_SPEED'] / speed - vx *= scale - vy *= scale - - target_z = pos.z_val + vz * 0.1 - if target_z > config.MANUAL['MIN_ALTITUDE_LIMIT']: - vz = max(vz, (config.MANUAL['MIN_ALTITUDE_LIMIT'] - pos.z_val) * 10) - if target_z < config.MANUAL['MAX_ALTITUDE_LIMIT']: - vz = min(vz, (config.MANUAL['MAX_ALTITUDE_LIMIT'] - pos.z_val) * 10) - - if vx != 0.0 or vy != 0.0 or vz != 0.0: - self.client.moveByVelocityAsync(vx, vy, vz, 0.1, vehicle_name=self.drone_name) - elif yaw_rate != 0.0: - self.client.rotateByYawRateAsync(yaw_rate, 0.1, vehicle_name=self.drone_name) - elif config.MANUAL['ENABLE_AUTO_HOVER'] and not self.control_keys: - self.client.hoverAsync(vehicle_name=self.drone_name) - - except Exception as e: - self.logger.warning(f"⚠️ 手动控制应用失败: {e}") - - def change_state(self, new_state: FlightState): - if self.state != new_state: - old_state = self.state.value - self.logger.info(f"🔄 状态转换: {old_state} → {new_state.value}") - self.state = new_state - self.state_history.append((time.time(), new_state)) - self.stats['state_changes'] += 1 - - if self.data_logger: - event_data = { - 'old_state': old_state, - 'new_state': new_state.value, - 'loop_count': self.loop_count - } - self.data_logger.record_event('state_change', event_data) - - def run_manual_control(self): - self.logger.info("=" * 60) - self.logger.info("启动手动控制模式") - self.logger.info("=" * 60) - - if not self.front_window: - self.logger.error("❌ 前视窗口未初始化") - return - - try: - self.change_state(FlightState.MANUAL) - self.manual_control_start = time.time() - - self.front_window.set_manual_mode(True) - - self.logger.info("🕹️ 进入手动控制模式") - print("\n" + "="*60) - print("🎮 手动控制模式已启动") - print("="*60) - print("控制键位:") - print(" W: 前进 | S: 后退 | A: 左移 | D: 右移") - print(" Q: 上升 | E: 下降 | Z: 左转 | X: 右转") - print(" 空格: 悬停 | ESC: 退出手动模式") - print("="*60) - print("💡 提示: 按键时控制持续生效，松开自动停止") - print(" 请在无人机前视窗口操作") - print("="*60) - - self.control_keys = {} - - manual_active = True - last_control_time = time.time() - last_image_time = time.time() - - while manual_active and not self.emergency_flag: - try: - if self.front_window.should_exit_manual(): - self.logger.info("收到退出手动模式指令") - manual_active = False - break - - if self.front_window: - window_keys = self.front_window.get_control_inputs() - self.control_keys = window_keys.copy() - - if not self.front_window.display_active: - self.logger.info("前视窗口已关闭，退出手动模式") - manual_active = False - break - - current_time = time.time() - if current_time - last_control_time >= 0.05: - self.apply_manual_control() - last_control_time = current_time - - if current_time - last_image_time >= 0.1: - try: - camera_name = config.CAMERA['DEFAULT_NAME'] - responses = self.client.simGetImages([ - airsim.ImageRequest( - camera_name, - airsim.ImageType.Scene, - False, - False - ) - ]) - - if responses and responses[0] and hasattr(responses[0], 'image_data_uint8'): - img_array = np.frombuffer(responses[0].image_data_uint8, dtype=np.uint8) - if len(img_array) > 0: - img_rgb = img_array.reshape(responses[0].height, responses[0].width, 3) - img_bgr = cv2.cvtColor(img_rgb, cv2.COLOR_RGB2BGR) - - try: - state = self.client.getMultirotorState(vehicle_name=self.drone_name) - pos = state.kinematics_estimated.position - display_info = { - 'state': self.state.value, - 'position': (pos.x_val, pos.y_val, pos.z_val), - 'loop_count': self.loop_count, - } - except: - display_info = {} - - if self.front_window: - manual_info = self._get_manual_control_info() - self.front_window.update_image(img_bgr, display_info, manual_info) - last_image_time = current_time - except Exception as img_error: - pass - - try: - state = self.client.getMultirotorState(vehicle_name=self.drone_name) - pos = state.kinematics_estimated.position - current_pos = (pos.x_val, pos.y_val) - self._check_red_object_proximity(current_pos) - self._check_blue_object_proximity(current_pos) - self._check_black_object_proximity(current_pos) - - # 更新信息窗口（手动控制模式下） - if current_time - last_image_time >= 0.1 and self.info_window: - # 创建一个简化的感知结果用于信息窗口更新 - simple_perception = PerceptionResult() - simple_perception.obstacle_distance = 100.0 - simple_perception.open_space_score = 1.0 - simple_perception.has_obstacle = False - simple_perception.red_objects_count = len(self.red_objects) - simple_perception.blue_objects_count = len(self.blue_objects) - simple_perception.black_objects_count = len(self.black_objects) - self._update_info_window(simple_perception) - except: - pass - - time.sleep(0.01) - - except KeyboardInterrupt: - self.logger.warning("⏹️ 用户中断手动控制") - manual_active = False - break - except Exception as e: - self.logger.error(f"❌ 手动控制循环异常: {e}") - time.sleep(0.1) - - manual_time = time.time() - self.manual_control_start - self.stats['manual_control_time'] = manual_time - - self.manual_control_start = 0 - self.control_keys = {} - if self.front_window: - self.front_window.set_manual_mode(False) - - try: - self.client.hoverAsync(vehicle_name=self.drone_name).join() - except: - pass - - self.logger.info(f"⏱️ 手动控制结束，持续时间: {manual_time:.1f}秒") - - self.change_state(FlightState.HOVERING) - - print("\n" + "="*60) - print("手动控制模式已结束") - print(f"控制时间: {manual_time:.1f}秒") - print(f"检测到红色物体: {self.stats['red_objects_detected']}个") - print(f"检测到蓝色物体: {self.stats['blue_objects_detected']}个") - print("="*60) - print("请选择下一步:") - print(" 1. 继续自动探索") - print(" 2. 再次进入手动模式") - print(" 3. 降落并结束任务") - print("="*60) - - choice = input("请输入选择 (1/2/3): ").strip() - - if choice == '1': - self.logger.info("🔄 返回自动探索模式") - remaining_time = self.exploration_time - (time.time() - self.start_time) - if remaining_time > 10: - self.exploration_time = remaining_time - self.run_perception_loop() - else: - self.logger.info("⏰ 剩余探索时间不足，开始返航") - self._finish_mission() - elif choice == '2': - self.logger.info("🔄 重新进入手动控制模式") - self.run_manual_control() - else: - self.logger.info("🛬 用户选择结束任务") - self._finish_mission() - - except Exception as e: - self.logger.error(f"❌ 手动控制模式发生异常: {e}") - self.logger.debug(f"异常堆栈: {traceback.format_exc()}") - self.emergency_stop() - - def run_perception_loop(self): - self.logger.info("=" * 60) - self.logger.info("启动感知-决策-控制主循环") - self.logger.info("=" * 60) - - try: - self.logger.info("🚀 起飞中...") - self.client.takeoffAsync(vehicle_name=self.drone_name).join() - time.sleep(2) - - self.client.moveToZAsync(self.takeoff_height, 3, vehicle_name=self.drone_name).join() - self.change_state(FlightState.HOVERING) - time.sleep(2) - - exploration_start = time.time() - - while (time.time() - exploration_start < self.exploration_time and - not self.emergency_flag): - - self.loop_count += 1 - loop_start = time.time() - - perception = self.get_depth_perception() - - try: - state = self.client.getMultirotorState(vehicle_name=self.drone_name) - pos = state.kinematics_estimated.position - current_pos = (pos.x_val, pos.y_val) - if self._check_red_object_proximity(current_pos): - time.sleep(2) - self.change_state(FlightState.EXPLORING) - if self._check_blue_object_proximity(current_pos): - time.sleep(2) - self.change_state(FlightState.EXPLORING) - if self._check_black_object_proximity(current_pos): - time.sleep(2) - self.change_state(FlightState.EXPLORING) - except: - pass - - decision = self.make_intelligent_decision(perception) - - self._execute_control_decision(decision) - - loop_time = time.time() - loop_start - self.stats['average_loop_time'] = (self.stats['average_loop_time'] * (self.loop_count-1) + loop_time) / self.loop_count - self.stats['max_loop_time'] = max(self.stats['max_loop_time'], loop_time) - self.stats['min_loop_time'] = min(self.stats['min_loop_time'], loop_time) - - if self.data_logger: - self.data_logger.record_loop_time(loop_time) - - current_time = time.time() - if current_time - self.last_performance_report >= self.performance_report_interval: - self._generate_performance_report() - self.last_performance_report = current_time - - if self.loop_count % config.SYSTEM.get('HEALTH_CHECK_INTERVAL', 20) == 0: - self._report_status(exploration_start, perception) - # 内存优化：定期垃圾回收 - gc.collect() - - loop_time = time.time() - loop_start - if loop_time < 0.1: - time.sleep(0.1 - loop_time) - - self.logger.info("⏰ 探索时间到，开始返航") - self._finish_mission() - - except KeyboardInterrupt: - self.logger.warning("⏹️ 用户中断探索") - self.emergency_stop() - except Exception as e: - self.logger.error(f"❌ 主循环发生异常: {e}") - self.logger.debug(f"异常堆栈: {traceback.format_exc()}") - self.emergency_stop() - - def _generate_performance_report(self): - try: - if not config.PERFORMANCE['ENABLE_REALTIME_METRICS']: - return - - cpu_usage = psutil.cpu_percent(interval=0) - memory_usage = psutil.virtual_memory().percent - - warnings = [] - if cpu_usage > config.PERFORMANCE['CPU_WARNING_THRESHOLD']: - warnings.append(f"⚠️ CPU使用率过高: {cpu_usage:.1f}%") - - if memory_usage > config.PERFORMANCE['MEMORY_WARNING_THRESHOLD']: - warnings.append(f"⚠️ 内存使用率过高: {memory_usage:.1f}%") - - avg_loop_time = self.stats.get('average_loop_time', 0) - if avg_loop_time > config.PERFORMANCE['LOOP_TIME_WARNING_THRESHOLD']: - warnings.append(f"⚠️ 平均循环时间过长: {avg_loop_time*1000:.1f}ms") - - if warnings: - self.logger.warning("📊 性能警告:") - for warning in warnings: - self.logger.warning(f" {warning}") - - if self.data_logger: - performance_data = { - 'timestamp': datetime.now().isoformat(), - 'cpu_usage': cpu_usage, - 'memory_usage': memory_usage, - 'average_loop_time': avg_loop_time, - 'max_loop_time': self.stats.get('max_loop_time', 0), - 'min_loop_time': self.stats.get('min_loop_time', 0), - 'warnings': warnings - } - self.data_logger.record_event('performance_report', performance_data) - - except Exception as e: - self.logger.warning(f"⚠️ 生成性能报告时出错: {e}") - - def make_intelligent_decision(self, perception: PerceptionResult) -> Tuple[float, float, float, float]: - self.stats['decision_cycles'] += 1 - decision_start = time.time() - - try: - state = self.client.getMultirotorState(vehicle_name=self.drone_name) - pos = state.kinematics_estimated.position - vel = state.kinematics_estimated.linear_velocity - - target_vx, target_vy, target_z, target_yaw = 0.0, 0.0, perception.recommended_height, 0.0 - - if self.state == FlightState.TAKEOFF: - target_z = self.takeoff_height - if pos.z_val < self.takeoff_height + 0.5: - self.change_state(FlightState.HOVERING) - - elif self.state == FlightState.HOVERING: - target_yaw = (time.time() % 10) * 0.2 - - current_time = time.time() - if (self.exploration_target is None or - current_time - self.target_update_time > self.target_lifetime): - - self.exploration_target = self.exploration_grid.get_best_exploration_target( - (pos.x_val, pos.y_val), - perception.red_objects, - perception.blue_objects, - perception.black_objects - ) - self.target_update_time = current_time - - if self.exploration_target: - self.logger.info(f"🎯 新探索目标: {self.exploration_target[0]:.1f}, {self.exploration_target[1]:.1f}") - - if self.exploration_target: - self.change_state(FlightState.EXPLORING) - - elif self.state == FlightState.EXPLORING: - if perception.has_obstacle: - self.change_state(FlightState.AVOIDING) - target_vx, target_vy = -vel.x_val * 2, -vel.y_val * 2 - else: - current_pos = (pos.x_val, pos.y_val) - - if self.exploration_target is None: - self.exploration_target = self.exploration_grid.get_best_exploration_target( - current_pos, - perception.red_objects, - perception.blue_objects, - perception.black_objects - ) - self.target_update_time = time.time() - - vector = self.vector_planner.compute_vector( - current_pos, - self.exploration_target, - perception.obstacle_positions, - perception.red_objects, - perception.blue_objects, - perception.black_objects - ) - - speed_factor = self._calculate_adaptive_speed(perception, vector.magnitude()) - - target_speed = self.preferred_speed * speed_factor - current_speed = math.sqrt(vel.x_val**2 + vel.y_val**2) - speed_error = target_speed - current_speed - speed_adjustment = self.velocity_pid.update(speed_error) - - final_vector = vector.normalize() * (target_speed + speed_adjustment) - target_vx = final_vector.x - target_vy = final_vector.y - - self.stats['vector_field_updates'] += 1 - - self.last_decision_info = { - 'vector_angle': math.atan2(vector.y, vector.x), - 'vector_magnitude': vector.magnitude(), - 'grid_score': len(self.exploration_grid.frontier_cells) / 100.0, - 'speed_factor': speed_factor, - 'red_objects_in_view': perception.red_objects_count, - 'blue_objects_in_view': perception.blue_objects_count, - 'black_objects_in_view': perception.black_objects_count, - 'decision_time': time.time() - decision_start - } - - if self.exploration_target: - distance_to_target = math.sqrt( - (self.exploration_target[0] - current_pos[0])**2 + - (self.exploration_target[1] - current_pos[1])**2 - ) - if distance_to_target < self.target_reached_distance: - self.exploration_target = None - self.change_state(FlightState.HOVERING) - self.logger.info("✅ 到达探索目标") - - elif self.state == FlightState.AVOIDING: - if perception.has_obstacle: - current_pos = (pos.x_val, pos.y_val) - - avoid_vector = self.vector_planner.compute_vector( - current_pos, - None, - perception.obstacle_positions, - perception.red_objects, - perception.blue_objects, - perception.black_objects - ) - - if avoid_vector.magnitude() > 0.1: - avoid_vector = avoid_vector.normalize() * 1.5 - target_vx = avoid_vector.x - target_vy = avoid_vector.y - - target_z = pos.z_val - 3 - else: - self.change_state(FlightState.EXPLORING) - time.sleep(1) - - elif self.state == FlightState.RED_OBJECT_INSPECTION: - target_vx, target_vy = 0.0, 0.0 - time.sleep(2) - self.change_state(FlightState.EXPLORING) - - elif self.state == FlightState.BLUE_OBJECT_INSPECTION: - target_vx, target_vy = 0.0, 0.0 - time.sleep(2) - self.change_state(FlightState.EXPLORING) - - elif self.state == FlightState.BLACK_OBJECT_INSPECTION: - target_vx, target_vy = 0.0, 0.0 - time.sleep(2) - self.change_state(FlightState.EXPLORING) - - elif self.state == FlightState.EMERGENCY: - target_vx, target_vy, target_yaw = 0, 0, 0 - target_z = max(pos.z_val, -20) - - elif self.state == FlightState.PLANNING: - target_vx, target_vy = 0, 0 - target_z = perception.recommended_height - - height_error = target_z - pos.z_val - height_adjustment = self.height_pid.update(height_error) - target_z += height_adjustment - - target_z = max(self.max_altitude, min(self.min_altitude, target_z)) - - decision_time = time.time() - decision_start - self.last_decision_info['total_decision_time'] = decision_time - - return target_vx, target_vy, target_z, target_yaw - - except Exception as e: - self.logger.error(f"❌ 决策过程异常: {e}") - if self.data_logger: - self.data_logger.record_event('decision_exception', {'error': str(e)}) - return 0.0, 0.0, self.base_height, 0.0 - - def _calculate_adaptive_speed(self, perception: PerceptionResult, vector_magnitude: float) -> float: - if not config.INTELLIGENT_DECISION['ADAPTIVE_SPEED_ENABLED']: - return 1.0 - - open_factor = min(1.0, perception.open_space_score * 1.2) - - if perception.obstacle_distance < self.depth_threshold_near * 2: - obs_factor = max(0.3, perception.obstacle_distance / (self.depth_threshold_near * 2)) - else: - obs_factor = 1.0 - - vector_factor = min(1.0, vector_magnitude * 2) - - red_factor = 0.8 if perception.red_objects_count > 0 else 1.0 - blue_factor = 0.8 if perception.blue_objects_count > 0 else 1.0 - black_factor = 0.8 if perception.black_objects_count > 0 else 1.0 - color_factor = min(red_factor, blue_factor, black_factor) - - speed_factor = open_factor * obs_factor * vector_factor * color_factor * 0.7 - - speed_factor = max( - config.INTELLIGENT_DECISION['MIN_SPEED_FACTOR'], - min(config.INTELLIGENT_DECISION['MAX_SPEED_FACTOR'], speed_factor) - ) - - return speed_factor - - def _execute_control_decision(self, decision): - try: - target_vx, target_vy, target_z, target_yaw = decision - - if self.state in [FlightState.EXPLORING, FlightState.AVOIDING, FlightState.PLANNING, - FlightState.RED_OBJECT_INSPECTION, FlightState.BLUE_OBJECT_INSPECTION, - FlightState.BLACK_OBJECT_INSPECTION]: - self.client.moveByVelocityZAsync( - target_vx, target_vy, target_z, 0.5, - drivetrain=airsim.DrivetrainType.MaxDegreeOfFreedom, - yaw_mode=airsim.YawMode(is_rate=False, yaw_or_rate=target_yaw), - vehicle_name=self.drone_name - ) - else: - self.client.moveToPositionAsync( - 0, 0, target_z, 2, - vehicle_name=self.drone_name - ) - - state = self.client.getMultirotorState(vehicle_name=self.drone_name) - pos = state.kinematics_estimated.position - self.visited_positions.append((pos.x_val, pos.y_val, pos.z_val)) - - except Exception as e: - self.logger.warning(f"⚠️ 控制指令执行失败: {e}") - if self.data_logger: - self.data_logger.record_event('control_exception', {'error': str(e)}) - try: - self.client.hoverAsync(vehicle_name=self.drone_name).join() - except: - pass - - def _report_status(self, exploration_start, perception): - elapsed = time.time() - exploration_start - try: - state = self.client.getMultirotorState(vehicle_name=self.drone_name) - pos = state.kinematics_estimated.position - - self.logger.info(f"\n📊 系统状态报告 [循环#{self.loop_count}]") - self.logger.info(f" 运行时间: {elapsed:.1f}s / {self.exploration_time}s") - self.logger.info(f" 飞行状态: {self.state.value}") - self.logger.info(f" 当前位置: ({pos.x_val:.1f}, {pos.y_val:.1f}, {-pos.z_val:.1f}m)") - self.logger.info(f" 环境感知: 障碍{'有' if perception.has_obstacle else '无'} " - f"| 距离={perception.obstacle_distance:.1f}m " - f"| 开阔度={perception.open_space_score:.2f}") - self.logger.info(f" 红色物体: 检测到{perception.red_objects_count}个 " - f"| 已访问{self.stats['red_objects_visited']}个") - self.logger.info(f" 蓝色物体: 检测到{perception.blue_objects_count}个 " - f"| 已访问{self.stats['blue_objects_visited']}个") - self.logger.info(f" 黑色物体: 检测到{perception.black_objects_count}个 " - f"| 已访问{self.stats['black_objects_visited']}个") - self.logger.info(f" 智能决策: 向量场{self.stats['vector_field_updates']}次 " - f"| 网格更新{self.stats['grid_updates']}次") - self.logger.info(f" 探索网格: 前沿{len(self.exploration_grid.frontier_cells)}个") - self.logger.info(f" 系统统计: 异常{self.stats['exceptions_caught']}次 " - f"| 状态切换{self.stats['state_changes']}次") - self.logger.info(f" 数据记录: {self.stats['data_points_recorded']}个数据点") - self.logger.info(f" 性能统计: 平均循环{self.stats['average_loop_time']*1000:.1f}ms " - f"| 最大{self.stats['max_loop_time']*1000:.1f}ms " - f"| 最小{self.stats['min_loop_time']*1000:.1f}ms") - if self.stats['manual_control_time'] > 0: - self.logger.info(f" 手动控制: {self.stats['manual_control_time']:.1f}秒") - except: - self.logger.info("状态报告: 无法获取无人机状态") - - def _finish_mission(self): - self.logger.info("=" * 60) - self.logger.info("探索任务完成，开始返航程序") - self.logger.info("=" * 60) - - self.change_state(FlightState.RETURNING) - - try: - self.logger.info("↩️ 返回起始区域...") - self.client.moveToPositionAsync(0, 0, -10, 4, vehicle_name=self.drone_name).join() - time.sleep(2) - - self.logger.info("🛬 降落中...") - self.change_state(FlightState.LANDING) - self.client.landAsync(vehicle_name=self.drone_name).join() - time.sleep(3) - - except Exception as e: - self.logger.error(f"❌ 降落过程中出现异常: {e}") - - finally: - self._cleanup_system() - - self._generate_summary_report() - - def _cleanup_system(self): - self.logger.info("🧹 清理系统资源...") - - try: - self.client.armDisarm(False, vehicle_name=self.drone_name) - self.client.enableApiControl(False, vehicle_name=self.drone_name) - self.logger.info("✅ 无人机控制已安全释放") - except: - self.logger.warning("⚠️ 释放控制时出现异常") - - if self.front_window: - self.front_window.stop() - self.logger.info("✅ 前视窗口已关闭") - - if self.info_window: - self.info_window.stop() - self.logger.info("✅ 信息窗口已关闭") - - if self.data_logger: - self.logger.info("💾 正在保存飞行数据...") - self.data_logger.save_json_data() - - if config.PERFORMANCE['SAVE_PERFORMANCE_REPORT']: - performance_report = self.data_logger.generate_performance_report() - self.logger.info(performance_report) - - timestamp = datetime.now().strftime("%Y%m%d_%H%M%S") - report_filename = f"performance_report_{timestamp}.txt" - with open(report_filename, 'w', encoding='utf-8') as f: - f.write(performance_report) - self.logger.info(f"📄 性能报告已保存至: {report_filename}") - - def _generate_summary_report(self): - total_time = time.time() - self.start_time - - self.logger.info("\n" + "=" * 60) - self.logger.info("🏁 任务总结报告") - self.logger.info("=" * 60) - self.logger.info(f" 总运行时间: {total_time:.1f}秒") - self.logger.info(f" 总循环次数: {self.loop_count}") - if total_time > 0: - self.logger.info(f" 平均循环频率: {self.loop_count/total_time:.1f} Hz") - self.logger.info(f" 探索航点数量: {len(self.visited_positions)}") - self.logger.info(f" 状态切换次数: {self.stats['state_changes']}") - self.logger.info(f" 检测到障碍次数: {self.stats['obstacles_detected']}") - self.logger.info(f" 红色物体检测: {self.stats['red_objects_detected']}个") - self.logger.info(f" 红色物体访问: {self.stats['red_objects_visited']}个") - self.logger.info(f" 蓝色物体检测: {self.stats['blue_objects_detected']}个") - self.logger.info(f" 蓝色物体访问: {self.stats['blue_objects_visited']}个") - self.logger.info(f" 黑色物体检测: {self.stats['black_objects_detected']}个") - self.logger.info(f" 黑色物体访问: {self.stats['black_objects_visited']}个") - self.logger.info(f" 向量场计算次数: {self.stats['vector_field_updates']}") - self.logger.info(f" 网格更新次数: {self.stats['grid_updates']}") - self.logger.info(f" 探索前沿数量: {len(self.exploration_grid.frontier_cells)}") - self.logger.info(f" 前视图像更新次数: {self.stats['front_image_updates']}") - self.logger.info(f" 数据记录点数: {self.stats['data_points_recorded']}") - self.logger.info(f" 手动控制时间: {self.stats['manual_control_time']:.1f}秒") - self.logger.info(f" 捕获的异常数: {self.stats['exceptions_caught']}") - self.logger.info(f" 重连尝试次数: {self.reconnect_attempts}") - self.logger.info(f" 平均循环时间: {self.stats['average_loop_time']*1000:.1f}ms") - self.logger.info(f" 最大循环时间: {self.stats['max_loop_time']*1000:.1f}ms") - self.logger.info(f" 最小循环时间: {self.stats['min_loop_time']*1000:.1f}ms") - - try: - report_filename = f"mission_report_{datetime.now().strftime('%Y%m%d_%H%M%S')}.txt" - with open(report_filename, 'w', encoding='utf-8') as f: - f.write("AirSimNH 无人机任务报告 (智能决策增强版 - 双窗口双色物体检测版)\n") - f.write("=" * 50 + "\n") - f.write(f"生成时间: {datetime.now().strftime('%Y-%m-%d %H:%M:%S')}\n") - f.write(f"总运行时间: {total_time:.1f}秒\n") - f.write(f"总循环次数: {self.loop_count}\n") - f.write(f"探索航点数量: {len(self.visited_positions)}\n") - f.write(f"状态切换次数: {self.stats['state_changes']}\n") - f.write(f"向量场计算次数: {self.stats['vector_field_updates']}\n") - f.write(f"网格更新次数: {self.stats['grid_updates']}\n") - f.write(f"探索前沿数量: {len(self.exploration_grid.frontier_cells)}\n") - f.write(f"数据记录点数: {self.stats['data_points_recorded']}\n") - f.write(f"手动控制时间: {self.stats['manual_control_time']:.1f}秒\n") - f.write(f"红色物体检测总数: {self.stats['red_objects_detected']}个\n") - f.write(f"红色物体已访问数: {self.stats['red_objects_visited']}个\n") - f.write(f"蓝色物体检测总数: {self.stats['blue_objects_detected']}个\n") - f.write(f"蓝色物体已访问数: {self.stats['blue_objects_visited']}个\n") - f.write(f"黑色物体检测总数: {self.stats['black_objects_detected']}个\n") - f.write(f"黑色物体已访问数: {self.stats['black_objects_visited']}个\n") - f.write(f"异常捕获次数: {self.stats['exceptions_caught']}\n") - f.write(f"前视图像更新次数: {self.stats['front_image_updates']}\n") - f.write(f"平均循环时间: {self.stats['average_loop_time']*1000:.1f}ms\n") - f.write(f"最大循环时间: {self.stats['max_loop_time']*1000:.1f}ms\n") - f.write(f"最小循环时间: {self.stats['min_loop_time']*1000:.1f}ms\n") - f.write("=" * 50 + "\n") - f.write("智能决策配置:\n") - for key, value in config.INTELLIGENT_DECISION.items(): - f.write(f" {key}: {value}\n") - f.write("=" * 50 + "\n") - f.write("飞行航点记录:\n") - for i, pos in enumerate(self.visited_positions[:20]): - f.write(f" 航点{i+1}: ({pos[0]:.1f}, {pos[1]:.1f}, {pos[2]:.1f})\n") - if len(self.visited_positions) > 20: - f.write(f" ... 还有{len(self.visited_positions)-20}个航点\n") - f.write("=" * 50 + "\n") - f.write("数据记录信息:\n") - if self.data_logger and config.DATA_RECORDING['ENABLED']: - f.write(f" CSV文件: {self.data_logger.csv_filename}\n") - f.write(f" JSON文件: {self.data_logger.json_filename}\n") - else: - f.write(" 数据记录未启用\n") - self.logger.info(f"📄 详细报告已保存至: {report_filename}") - except Exception as e: - self.logger.warning(f"⚠️ 无法保存报告文件: {e}") - - def emergency_stop(self): - if self.emergency_flag: - return - - self.logger.error("\n🆘 紧急停止程序启动!") - self.emergency_flag = True - - self.change_state(FlightState.EMERGENCY) - - try: - self.client.hoverAsync(vehicle_name=self.drone_name).join() - time.sleep(1) - self.client.landAsync(vehicle_name=self.drone_name).join() - time.sleep(2) - self.logger.info("✅ 紧急降落指令已发送") - except Exception as e: - self.logger.error(f"⚠️ 紧急降落异常: {e}") - - if self.front_window: - self.front_window.stop() - - if self.info_window: - self.info_window.stop() - - self._cleanup_system() - - -# ==================== 主程序入口 ==================== - -def main(): - print("=" * 70) - print("AirSimNH 无人机感知探索系统 - 智能决策增强版（双窗口双色物体检测版）") - print(f"启动时间: {datetime.now().strftime('%Y-%m-%d %H:%M:%S')}") - print(f"配置状态: {'已加载' if CONFIG_LOADED else '使用默认配置'}") - print(f"日志级别: {config.SYSTEM['LOG_LEVEL']}") - print(f"探索时间: {config.EXPLORATION['TOTAL_TIME']}秒") - print("=" * 70) - print("智能决策特性:") - print(" • 向量场避障算法 (VFH)") - print(" • 基于网格的信息增益探索") - print(" • PID平滑飞行控制") - print(" • 自适应速度调整") - print(" • 性能监控与数据闭环") - print(" • 红色与蓝色物体检测与记录") - print("=" * 70) - print("显示系统:") - print(" • 双窗口模式: 前视窗口 + 信息窗口") - print(" • 前视窗口: 摄像头画面、手动控制") - print(" • 信息窗口: 系统状态、探索网格、物体统计") - print("=" * 70) - print("数据记录:") - print(f" • CSV格式: {config.DATA_RECORDING.get('SAVE_TO_CSV', False)}") - print(f" • JSON格式: {config.DATA_RECORDING.get('SAVE_TO_JSON', False)}") - print(f" • 性能监控: {config.DATA_RECORDING.get('PERFORMANCE_MONITORING', False)}") - print(f" • 红色物体记录: {config.DATA_RECORDING.get('RECORD_RED_OBJECTS', False)}") - print(f" • 蓝色物体记录: {config.DATA_RECORDING.get('RECORD_BLUE_OBJECTS', False)}") - print("=" * 70) - - print("\n请选择运行模式:") - print(" 1. 智能探索模式 (AI自主决策，包含双色物体检测)") - print(" 2. 手动控制模式 (键盘控制)") - print(" 3. 混合模式 (先自动探索，后可切换)") - print("=" * 50) - - mode_choice = input("请输入选择 (1/2/3): ").strip() - - explorer = None - try: - explorer = PerceptiveExplorer(drone_name="") - - def signal_handler(sig, frame): - print("\n⚠️ 用户中断，正在安全停止...") - if explorer: - explorer.emergency_stop() - sys.exit(0) - - signal.signal(signal.SIGINT, signal_handler) - - if mode_choice == '1': - print("\n" + "="*50) - print("启动智能探索模式（含双色物体检测）") - print("="*50) - print("注意：将打开两个窗口:") - print(" 1. 前视窗口 - 显示摄像头画面") - print(" 2. 信息窗口 - 显示系统状态和探索信息") - print("="*50) - explorer.run_perception_loop() - - elif mode_choice == '2': - print("\n" + "="*50) - print("启动手动控制模式") - print("="*50) - - print("正在起飞...") - explorer.client.takeoffAsync(vehicle_name="").join() - time.sleep(2) - explorer.client.moveToZAsync(-10, 3, vehicle_name="").join() - time.sleep(2) - print("起飞完成，可以开始手动控制") - print("请切换到无人机前视窗口，使用WSAD键控制") - - explorer.run_manual_control() - - elif mode_choice == '3': - print("\n" + "="*50) - print("启动混合模式") - print("="*50) - - explorer.logger.info("🔍 开始智能探索（含双色物体检测）...") - original_time = config.EXPLORATION['TOTAL_TIME'] - explorer.exploration_time = min(60, original_time) - - explorer.run_perception_loop() - - if not explorer.emergency_flag: - print("\n" + "="*50) - print("智能探索阶段结束") - print(f"检测到红色物体: {explorer.stats['red_objects_detected']}个") - print(f"检测到蓝色物体: {explorer.stats['blue_objects_detected']}个") - print(f"检测到黑色物体: {explorer.stats['black_objects_detected']}个") - print("请选择下一步:") - print(" 1. 进入手动控制模式") - print(" 2. 继续智能探索") - print(" 3. 结束任务返航") - print("="*50) - - next_choice = input("请输入选择 (1/2/3): ").strip() - - if next_choice == '1': - explorer.run_manual_control() - elif next_choice == '2': - explorer.exploration_time = original_time - 60 - if explorer.exploration_time > 10: - explorer.run_perception_loop() - else: - explorer.logger.info("⏰ 剩余时间不足，开始返航") - explorer._finish_mission() - else: - explorer._finish_mission() - - else: - print("❌ 无效的选择，程序退出") - if explorer: - explorer._cleanup_system() - - except Exception as e: - print(f"\n❌ 程序启动异常: {e}") - traceback.print_exc() - - try: - if explorer and explorer.client: - explorer.client.landAsync().join() - explorer.client.armDisarm(False) - explorer.client.enableApiControl(False) - except: - pass - - -if __name__ == "__main__": - main() \ No newline at end of file From 049aad1c77db6787fdef1edf8a574b9f519bc252 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?=E5=AE=9E=E9=AA=8C604=E9=BB=84=E5=85=89=E8=BE=BE?= <2732514173@qq.com> Date: Sat, 27 Dec 2025 09:43:18 +0800 Subject: [PATCH 14/14] Add files via upload --- src/drone_perception/requirements.txt | 31 +++++++++++++++++++++++++++ 1 file changed, 31 insertions(+) create mode 100644 src/drone_perception/requirements.txt diff --git a/src/drone_perception/requirements.txt b/src/drone_perception/requirements.txt new file mode 100644 index 0000000000..695a72fd2b --- /dev/null +++ b/src/drone_perception/requirements.txt @@ -0,0 +1,31 @@ +# 核心深度学习框架 +torch>=1.9.0 +torchvision>=0.10.0 + +# 图像处理 +opencv-python>=4.5.0 +Pillow>=8.3.0 +numpy>=1.19.0 +scipy>=1.7.0 + +# 机器学习和数据处理 +scikit-learn>=0.24.0 +matplotlib>=3.3.0 + +# 3D可视化和交互 +plotly>=5.0.0 +pandas>=1.3.0 + +# 开发工具和辅助 +tqdm>=4.62.0 +seaborn>=0.11.0 +jupyter>=1.0.0 +ipywidgets>=7.6.0 + +# 文件处理 +pyyaml>=5.4.0 +chardet>=4.0.0 + +# 系统工具 +psutil>=5.8.0 +GPUtil>=1.4.0 \ No newline at end of file