From beae83dffd7a378be41f38fc3717f1d88469536a Mon Sep 17 00:00:00 2001 From: rhcgg <3136924319@qq.com> Date: Thu, 18 Dec 2025 19:26:51 +0800 Subject: [PATCH 01/23] =?UTF-8?q?=E5=B0=9D=E8=AF=95=E4=BF=AE=E6=94=B9?= =?UTF-8?q?=E5=86=B2=E7=AA=81=EF=BC=8C=E6=8F=90=E4=BA=A4?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit 尝试修改冲突，提交 --- src/drone_perception/drone_perception_fusion.py | 1 + 1 file changed, 1 insertion(+) diff --git a/src/drone_perception/drone_perception_fusion.py b/src/drone_perception/drone_perception_fusion.py index 7629fb32b7..66e213f684 100644 --- a/src/drone_perception/drone_perception_fusion.py +++ b/src/drone_perception/drone_perception_fusion.py @@ -961,6 +961,7 @@ def run_improved_demo(): else: print("\n⚠️ 传感器融合测试完成，但未检测到融合物体") + # 运行改进演示 run_improved_demo() From bbe755b9fc4432747ad9236f736bdec87f5edaad Mon Sep 17 00:00:00 2001 From: rhcgg <3136924319@qq.com> Date: Thu, 18 Dec 2025 20:08:32 +0800 Subject: [PATCH 02/23] perceptive_drone_explorer.py MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit 重置仓库后提交 --- .../drone_perception_fusion.py | 2 - .../perceptive_drone_explorer.py | 436 ++++++++++++++++++ 2 files changed, 436 insertions(+), 2 deletions(-) create mode 100644 src/drone_perception/perceptive_drone_explorer.py diff --git a/src/drone_perception/drone_perception_fusion.py b/src/drone_perception/drone_perception_fusion.py index 66e213f684..70be9cdb13 100644 --- a/src/drone_perception/drone_perception_fusion.py +++ b/src/drone_perception/drone_perception_fusion.py @@ -960,8 +960,6 @@ def run_improved_demo(): print("\n✅ 传感器融合测试成功!") else: print("\n⚠️ 传感器融合测试完成，但未检测到融合物体") - - # 运行改进演示 run_improved_demo() diff --git a/src/drone_perception/perceptive_drone_explorer.py b/src/drone_perception/perceptive_drone_explorer.py new file mode 100644 index 0000000000..0cda60eea3 --- /dev/null +++ b/src/drone_perception/perceptive_drone_explorer.py @@ -0,0 +1,436 @@ +""" +AirSimNH 感知驱动自主探索无人机 +核心：视觉感知 → 语义理解 → 智能决策 → 安全执行 +""" + +import airsim +import time +import numpy as np +import cv2 +import math +from collections import deque +from dataclasses import dataclass +from enum import Enum +import threading +from typing import Tuple, List, Optional + + +class FlightState(Enum): + """无人机状态枚举""" + TAKEOFF = "起飞" + HOVERING = "悬停观测" + EXPLORING = "主动探索" + AVOIDING = "避障机动" + LANDING = "降落" + EMERGENCY = "紧急状态" + + +@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 # 开阔度评分 (0-1) + recommended_height: float = -15.0 # 推荐飞行高度 + safe_directions: List[float] = None # 安全方向列表 + + def __post_init__(self): + if self.safe_directions is None: + self.safe_directions = [] + + +class PerceptiveExplorer: + """基于感知的自主探索无人机""" + + def __init__(self, drone_name=""): + print("=" * 60) + print("AirSimNH 感知驱动自主探索系统") + print("=" * 60) + + # 初始化AirSim连接 + self.client = airsim.MultirotorClient() + self.client.confirmConnection() + self.drone_name = drone_name + + # 启用API控制 + self.client.enableApiControl(True, vehicle_name=drone_name) + self.client.armDisarm(True, vehicle_name=drone_name) + + # 状态管理 + self.state = FlightState.TAKEOFF + self.state_history = deque(maxlen=20) + self.emergency_flag = False + + # 感知参数 + self.depth_threshold_near = 5.0 # 近距离警报阈值(米) + self.depth_threshold_safe = 10.0 # 安全距离阈值(米) + self.min_ground_clearance = 2.0 # 最小离地间隙(米) + self.max_pitch_angle = math.radians(15) # 最大允许俯仰角 + + # 探索参数 + self.exploration_time = 180 # 总探索时间(秒) + self.preferred_speed = 3.0 # 优选速度(m/s) + self.max_altitude = -30 # 最大海拔(米) + self.min_altitude = -8 # 最小海拔(米) + + # 记忆系统 + self.visited_positions = deque(maxlen=100) + self.obstacle_map = {} # 障碍物位置记忆 + self.traversability_map = {} # 地形可通行性记忆 + + # 性能监控 + self.perception_fps = 0 + self.decision_fps = 0 + self.start_time = time.time() + + print("✅ 系统初始化完成") + print(f" 开始时间: {time.strftime('%H:%M:%S')}") + print(f" 预计探索时长: {self.exploration_time}秒") + + def get_depth_perception(self) -> PerceptionResult: + """获取并分析深度图像，理解环境""" + result = PerceptionResult() + + try: + # 请求深度图像（使用正确的DepthPlanar类型） + responses = self.client.simGetImages([ + airsim.ImageRequest( + "0", + airsim.ImageType.DepthPlanar, + pixels_as_float=True, + compress=False + ) + ]) + + if not responses or not responses[0]: + print("⚠ 深度图像获取失败") + return result + + # 转换深度数据为numpy数组 + depth_img = responses[0] + 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 + + # 1. 前方近距离区域（紧急避障） + 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 + + # 2. 多方向扇形扫描 + directions = [] + scan_angles = [-45, -30, -15, 0, 15, 30, 45] # 度 + + for angle_deg in 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) # 使用25%分位数（较保守） + directions.append((angle_rad, dir_distance)) + + if dir_distance > self.depth_threshold_safe: + result.safe_directions.append(angle_rad) + + # 3. 地形分析（通过深度梯度估计坡度） + 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 + + # 4. 开阔度评分（基于有效距离像素比例） + 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 directions: + # 找出最近障碍物的方向 + closest_dir = min(directions, key=lambda x: x[1]) + result.obstacle_direction = closest_dir[0] + + # 根据感知动态调整推荐高度 + if result.terrain_slope > 5: + result.recommended_height = -20 # 陡峭地形飞高些 + elif result.open_space_score > 0.7: + result.recommended_height = -12 # 开阔地带可以飞低些 + else: + result.recommended_height = -15 # 默认高度 + + # 更新感知FPS + self.perception_fps = 1 / (time.time() - self.perception_start) + + except Exception as e: + print(f"❌ 深度感知异常: {e}") + + return result + + def get_visual_perception(self): + """获取视觉图像用于高级感知（可选）""" + try: + responses = self.client.simGetImages([ + airsim.ImageRequest("0", airsim.ImageType.Scene, False, False) + ]) + + if responses and responses[0]: + # 转换为OpenCV格式 + img_data = responses[0].image_data_uint8 + img_array = np.frombuffer(img_data, dtype=np.uint8) + img = img_array.reshape(responses[0].height, responses[0].width, 3) + + # 简单颜色分析（示例：寻找绿色植被区域） + hsv = cv2.cvtColor(img, cv2.COLOR_RGB2HSV) + green_mask = cv2.inRange(hsv, (40, 40, 40), (80, 255, 255)) + green_ratio = np.sum(green_mask > 0) / green_mask.size + + return img, green_ratio + except: + pass + + return None, 0 + + def make_intelligent_decision(self, perception: PerceptionResult) -> Tuple[float, float, float, float]: + """基于感知结果做出智能决策""" + + # 获取当前位置和状态 + 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 = -10 # 起飞到10米 + if pos.z_val < -9.5: + self.change_state(FlightState.HOVERING) + + elif self.state == FlightState.HOVERING: + # 悬停观察，缓慢旋转扫描 + target_yaw = (time.time() % 10) * 0.2 # 缓慢旋转 + + if len(perception.safe_directions) > 0: + 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, -vel.y_val + else: + # 选择最佳探索方向 + if perception.safe_directions: + # 优先选择与当前航向相差45-90度的新方向（避免来回摆动） + current_yaw = airsim.to_eularian_angles( + state.kinematics_estimated.orientation + )[2] + + # 过滤出与当前方向不同的安全方向 + diverse_dirs = [ + d for d in perception.safe_directions + if abs(d - current_yaw) > math.radians(45) + ] + + if diverse_dirs: + best_dir = diverse_dirs[0] + else: + best_dir = perception.safe_directions[0] + + # 设置前进速度 + speed_factor = min(1.0, perception.open_space_score * 1.5) + target_vx = self.preferred_speed * speed_factor * math.cos(best_dir) + target_vy = self.preferred_speed * speed_factor * math.sin(best_dir) + else: + # 没有安全方向，爬升 + target_z = pos.z_val - 5 + self.change_state(FlightState.AVOIDING) + + elif self.state == FlightState.AVOIDING: + # 避障机动 + if perception.has_obstacle: + # 根据障碍物方向决定避障策略 + if abs(perception.obstacle_direction) < math.radians(30): + # 前方障碍物：爬升 + target_z = pos.z_val - 3 + target_vx, target_vy = 0, 0 + else: + # 侧方障碍物：向反方向平移 + avoid_dir = perception.obstacle_direction + math.pi + target_vx = 1.5 * math.cos(avoid_dir) + target_vy = 1.5 * math.sin(avoid_dir) + else: + # 障碍物清除，返回探索 + self.change_state(FlightState.HOVERING) + time.sleep(1) # 避障后暂停观察 + + elif self.state == FlightState.EMERGENCY: + # 紧急状态：悬停并准备降落 + target_vx, target_vy, target_yaw = 0, 0, 0 + target_z = max(pos.z_val, -20) # 限制爬升 + + # 确保高度在安全范围内 + target_z = max(self.max_altitude, min(self.min_altitude, target_z)) + + return target_vx, target_vy, target_z, target_yaw + + def change_state(self, new_state: FlightState): + """状态转换""" + if self.state != new_state: + print(f"🔄 状态转换: {self.state.value} → {new_state.value}") + self.state = new_state + self.state_history.append((time.time(), new_state)) + + def run_perception_loop(self): + """主感知-决策-控制循环""" + print("\n" + "=" * 60) + print("启动感知-决策-控制循环") + print("=" * 60) + + # 起飞 + print("🚀 起飞中...") + self.client.takeoffAsync(vehicle_name=self.drone_name).join() + time.sleep(3) + self.change_state(FlightState.HOVERING) + + # 主循环 + loop_count = 0 + exploration_start = time.time() + + while time.time() - exploration_start < self.exploration_time and not self.emergency_flag: + loop_start = time.time() + loop_count += 1 + + # 1. 感知阶段 + self.perception_start = time.time() + perception = self.get_depth_perception() + # visual_img, green_ratio = self.get_visual_perception() # 可选 + + # 2. 决策阶段 + decision = self.make_intelligent_decision(perception) + + # 3. 控制执行阶段 + target_vx, target_vy, target_z, target_yaw = decision + + # 使用速度控制（更灵活）或位置控制（更精确） + use_velocity_control = self.state in [FlightState.EXPLORING, FlightState.AVOIDING] + + if use_velocity_control: + self.client.moveByVelocityZAsync( + target_vx, target_vy, target_z, 0.5, # 持续时间0.5秒 + 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)) + + # 性能监控输出 + if loop_count % 20 == 0: + elapsed = time.time() - exploration_start + print(f"\n📊 循环{loop_count} | 已运行{elapsed:.1f}s | 状态:{self.state.value}") + print(f" 感知FPS:{self.perception_fps:.1f} | 障碍:{perception.has_obstacle}") + print(f" 最近障碍:{perception.obstacle_distance:.1f}m | 开阔度:{perception.open_space_score:.2f}") + print(f" 位置:({pos.x_val:.1f}, {pos.y_val:.1f}, {-pos.z_val:.1f}m)") + print(f" 安全方向数:{len(perception.safe_directions)}") + + # 循环频率控制（10Hz） + loop_time = time.time() - loop_start + if loop_time < 0.1: + time.sleep(0.1 - loop_time) + + # 探索结束，准备降落 + print("\n" + "=" * 60) + print("探索完成，开始返航降落") + print("=" * 60) + + self.change_state(FlightState.LANDING) + self.return_to_start() + + def return_to_start(self): + """返回起始点附近并降落""" + try: + # 回到起点附近（简单实现：回到原点） + print("↩️ 返回起始区域...") + self.client.moveToPositionAsync(0, 0, -10, 5, vehicle_name=self.drone_name).join() + time.sleep(2) + + # 降落 + print("🛬 降落中...") + self.client.landAsync(vehicle_name=self.drone_name).join() + time.sleep(3) + + # 断开控制 + self.client.armDisarm(False, vehicle_name=self.drone_name) + self.client.enableApiControl(False, vehicle_name=self.drone_name) + + print("✅ 任务完成，系统安全关闭") + + except Exception as e: + print(f"❌ 降落异常: {e}") + print("⚠ 尝试紧急降落...") + try: + self.client.landAsync(vehicle_name=self.drone_name).join() + except: + pass + + def emergency_stop(self): + """紧急停止""" + print("🆘 紧急停止触发！") + self.emergency_flag = True + self.change_state(FlightState.EMERGENCY) + self.client.hoverAsync(vehicle_name=self.drone_name).join() + + +def main(): + """主程序入口""" + try: + # 创建感知探索器 + explorer = PerceptiveExplorer(drone_name="") + + # 设置键盘中断处理 + import signal + def signal_handler(sig, frame): + print("\n⚠ 用户中断，正在安全停止...") + explorer.emergency_stop() + exit(0) + + signal.signal(signal.SIGINT, signal_handler) + + # 运行主循环 + explorer.run_perception_loop() + + except Exception as e: + print(f"\n❌ 系统异常: {e}") + import traceback + traceback.print_exc() + + # 尝试安全降落 + try: + client = airsim.MultirotorClient() + client.landAsync().join() + client.armDisarm(False) + client.enableApiControl(False) + except: + pass + + +if __name__ == "__main__": + main() \ No newline at end of file From 7ceb6bfb1865e950ec1604a233cac8e765eb6960 Mon Sep 17 00:00:00 2001 From: rhcgg <3136924319@qq.com> Date: Thu, 18 Dec 2025 20:21:03 +0800 Subject: [PATCH 03/23] Remove unnecessary comment lines Remove unnecessary comment lines --- src/drone_perception/drone_perception_fusion.py | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/src/drone_perception/drone_perception_fusion.py b/src/drone_perception/drone_perception_fusion.py index 70be9cdb13..f44a9d0bf9 100644 --- a/src/drone_perception/drone_perception_fusion.py +++ b/src/drone_perception/drone_perception_fusion.py @@ -3,7 +3,6 @@ 无人机感知模块 - 多传感器融合版本 修复版本 """ - import cv2 import numpy as np from typing import Tuple, Optional, List, Dict, Any @@ -968,4 +967,4 @@ def run_improved_demo(): except Exception as e: print(f"程序运行出错: {e}") import traceback - traceback.print_exc() \ No newline at end of file + traceback.print_exc() From 9daf9aad7e34c19aea2a9d188ea3ae39c4e3a999 Mon Sep 17 00:00:00 2001 From: rhcgg <3136924319@qq.com> Date: Thu, 18 Dec 2025 20:26:13 +0800 Subject: [PATCH 04/23] =?UTF-8?q?=E5=88=A0=E9=99=A4=EF=BC=8C=E4=B8=8D?= =?UTF-8?q?=E5=BF=85=E8=A6=81=E4=BF=AE=E6=94=B9?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit 删除，不必要修改 --- src/drone_perception/drone_perception_fusion.py | 1 + 1 file changed, 1 insertion(+) diff --git a/src/drone_perception/drone_perception_fusion.py b/src/drone_perception/drone_perception_fusion.py index 70be9cdb13..7629fb32b7 100644 --- a/src/drone_perception/drone_perception_fusion.py +++ b/src/drone_perception/drone_perception_fusion.py @@ -960,6 +960,7 @@ def run_improved_demo(): print("\n✅ 传感器融合测试成功!") else: print("\n⚠️ 传感器融合测试完成，但未检测到融合物体") + # 运行改进演示 run_improved_demo() From 6d4f124e030e45c61be4dfea3f83805f2c87a427 Mon Sep 17 00:00:00 2001 From: rhcgg <3136924319@qq.com> Date: Thu, 18 Dec 2025 20:43:20 +0800 Subject: [PATCH 05/23] perceptive_drone_explorer.py MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit 添加了一个前视镜，完善的交互控制 --- .../perceptive_drone_explorer.py | 233 +++++++++++++++++- 1 file changed, 229 insertions(+), 4 deletions(-) diff --git a/src/drone_perception/perceptive_drone_explorer.py b/src/drone_perception/perceptive_drone_explorer.py index 0cda60eea3..d22453b7d6 100644 --- a/src/drone_perception/perceptive_drone_explorer.py +++ b/src/drone_perception/perceptive_drone_explorer.py @@ -1,6 +1,7 @@ """ AirSimNH 感知驱动自主探索无人机 核心：视觉感知 → 语义理解 → 智能决策 → 安全执行 +集成前视窗口版本 - 支持实时视觉监控 """ import airsim @@ -14,6 +15,9 @@ import threading from typing import Tuple, List, Optional +# ============== 新增：导入队列模块 ============== +import queue + class FlightState(Enum): """无人机状态枚举""" @@ -35,6 +39,8 @@ class PerceptionResult: open_space_score: float = 0.0 # 开阔度评分 (0-1) recommended_height: float = -15.0 # 推荐飞行高度 safe_directions: List[float] = None # 安全方向列表 + # ========== 新增：前视图像字段 ========== + front_image: Optional[np.ndarray] = None # 前视图像 def __post_init__(self): if self.safe_directions is None: @@ -85,6 +91,12 @@ def __init__(self, drone_name=""): self.decision_fps = 0 self.start_time = time.time() + # ========== 新增：前视窗口初始化 ========== + self.front_display = FrontViewDisplay( + window_name=f"无人机前视 - {drone_name or 'AirSimNH'}" + ) + print("🎥 前视窗口已初始化") + print("✅ 系统初始化完成") print(f" 开始时间: {time.strftime('%H:%M:%S')}") print(f" 预计探索时长: {self.exploration_time}秒") @@ -94,21 +106,28 @@ def get_depth_perception(self) -> PerceptionResult: result = PerceptionResult() try: - # 请求深度图像（使用正确的DepthPlanar类型） + # ========== 修改：同时获取深度图像和前视图像 ========== responses = self.client.simGetImages([ airsim.ImageRequest( "0", airsim.ImageType.DepthPlanar, pixels_as_float=True, compress=False + ), + # 新增：获取前视RGB图像 + airsim.ImageRequest( + "0", + airsim.ImageType.Scene, + False, + False ) ]) - if not responses or not responses[0]: - print("⚠ 深度图像获取失败") + if not responses or len(responses) < 2: + print("⚠ 图像获取失败") return result - # 转换深度数据为numpy数组 + # 处理深度图像（原逻辑） depth_img = responses[0] depth_array = np.array(depth_img.image_data_float, dtype=np.float32) depth_array = depth_array.reshape(depth_img.height, depth_img.width) @@ -167,6 +186,27 @@ def get_depth_perception(self) -> PerceptionResult: else: result.recommended_height = -15 # 默认高度 + # ========== 新增：处理前视图像 ========== + front_response = responses[1] + if front_response and front_response.image_data_uint8: + # 转换图像格式 + img_array = np.frombuffer(front_response.image_data_uint8, dtype=np.uint8) + img_rgb = img_array.reshape(front_response.height, front_response.width, 3) + img_bgr = cv2.cvtColor(img_rgb, cv2.COLOR_RGB2BGR) + result.front_image = img_bgr + + # 准备显示信息 + state = self.client.getMultirotorState(vehicle_name=self.drone_name) + pos = state.kinematics_estimated.position + display_info = { + 'state': self.state.value, + 'obstacle_distance': result.obstacle_distance, + 'position': (pos.x_val, pos.y_val, pos.z_val) + } + + # 更新前视窗口 + self.front_display.update_image(img_bgr, display_info) + # 更新感知FPS self.perception_fps = 1 / (time.time() - self.perception_start) @@ -175,6 +215,7 @@ def get_depth_perception(self) -> PerceptionResult: return result + # 注意：get_visual_perception 方法现在可能多余，但为了兼容性保留 def get_visual_perception(self): """获取视觉图像用于高级感知（可选）""" try: @@ -381,6 +422,9 @@ def return_to_start(self): self.client.armDisarm(False, vehicle_name=self.drone_name) self.client.enableApiControl(False, vehicle_name=self.drone_name) + # ========== 新增：关闭前视窗口 ========== + self.front_display.stop() + print("✅ 任务完成，系统安全关闭") except Exception as e: @@ -398,6 +442,9 @@ def emergency_stop(self): self.change_state(FlightState.EMERGENCY) self.client.hoverAsync(vehicle_name=self.drone_name).join() + # ========== 新增：关闭前视窗口 ========== + self.front_display.stop() + def main(): """主程序入口""" @@ -432,5 +479,183 @@ def signal_handler(sig, frame): pass +# ============== 新增：前视窗口显示类 ============== +class FrontViewDisplay: + """前视画面显示管理器""" + + def __init__(self, window_name="无人机前视画面", width=640, height=480): + self.window_name = window_name + self.window_width = width + self.window_height = height + + # 图像队列（线程安全） + self.image_queue = queue.Queue(maxsize=2) + self.display_active = True + self.display_thread = None + + # 显示状态 + self.paused = False + self.show_info = True + self.enable_sharpening = True # 启用锐化改善模糊 + + # 启动显示线程 + self.start() + + def start(self): + """启动显示线程""" + self.display_thread = threading.Thread( + target=self._display_loop, + daemon=True, + name="FrontViewDisplay" + ) + self.display_thread.start() + + def stop(self): + """停止显示线程""" + self.display_active = False + if self.display_thread: + self.display_thread.join(timeout=2.0) + + def update_image(self, image_data: np.ndarray, info: dict): + """更新要显示的图像""" + 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: + 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 + + display_packet = { + 'image': image_data.copy(), + 'info': info.copy() if info else {}, + 'timestamp': time.time() + } + + self.image_queue.put_nowait(display_packet) + + 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) + + print("💡 前视窗口控制:") + print(" - 按 'Q': 关闭窗口 | 'S': 保存截图") + print(" - 按 'P': 暂停/继续 | 'I': 切换信息显示") + print(" - 按 'H': 切换锐化效果") + + while self.display_active: + display_image = None + info = {} + + try: + # 获取最新图像 + if not self.image_queue.empty(): + packet = self.image_queue.get_nowait() + display_image = packet['image'] + info = packet['info'] + + # 清空队列中的旧帧 + while not self.image_queue.empty(): + self.image_queue.get_nowait() + except queue.Empty: + pass + + # 显示图像 + if display_image is not None: + # 添加信息叠加 + if self.show_info: + display_image = self._add_info_overlay(display_image, info) + + cv2.imshow(self.window_name, display_image) + + # 键盘事件处理 + key = cv2.waitKey(30) & 0xFF + + if key == ord('q') or key == ord('Q'): + print("🔄 用户关闭显示窗口") + self.display_active = False + break + elif key == ord('s') or key == ord('S'): + self._save_screenshot(display_image) + elif key == ord('p') or key == ord('P'): + self.paused = not self.paused + status = "已暂停" if self.paused else "已恢复" + print(f"⏸️ 视频流{status}") + elif key == ord('i') or key == ord('I'): + self.show_info = not self.show_info + status = "开启" if self.show_info else "关闭" + print(f"📊 信息叠加层{status}") + elif key == ord('h') or key == ord('H'): + self.enable_sharpening = not self.enable_sharpening + status = "开启" if self.enable_sharpening else "关闭" + print(f"🔍 图像锐化{status}") + + cv2.destroyWindow(self.window_name) + + def _add_info_overlay(self, image: np.ndarray, info: dict) -> np.ndarray: + """在图像上叠加状态信息""" + try: + height, width = image.shape[:2] + + # 创建半透明信息栏 + info_height = 80 + overlay = image.copy() + cv2.rectangle(overlay, (0, 0), (width, info_height), (0, 0, 0), -1) + cv2.addWeighted(overlay, 0.7, image, 0.3, 0, image) + + # 飞行状态 + state = info.get('state', 'UNKNOWN') + state_color = (0, 255, 0) if '探索' in state else (0, 255, 255) if '悬停' in state else (0, 0, 255) + cv2.putText(image, f"状态: {state}", (10, 25), + cv2.FONT_HERSHEY_SIMPLEX, 0.6, state_color, 2) + + # 位置信息 + pos = info.get('position', (0, 0, 0)) + cv2.putText(image, f"位置: ({pos[0]:.1f}, {pos[1]:.1f}, {-pos[2]:.1f}m)", (10, 50), + cv2.FONT_HERSHEY_SIMPLEX, 0.5, (255, 255, 255), 1) + + # 障碍物信息 + obs_dist = info.get('obstacle_distance', 0.0) + if obs_dist < 100: + obs_color = (0, 0, 255) if obs_dist < 5.0 else (0, 165, 255) if obs_dist < 10.0 else (0, 255, 0) + cv2.putText(image, f"障碍: {obs_dist:.1f}m", (width - 120, 30), + cv2.FONT_HERSHEY_SIMPLEX, 0.6, obs_color, 1) + + # 清晰度提示 + if height < 200: + cv2.putText(image, "提示: 修改settings.json可提高分辨率", (10, height-10), + cv2.FONT_HERSHEY_SIMPLEX, 0.4, (200, 200, 200), 1) + + return image + except Exception: + 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}") + + if __name__ == "__main__": + print("=" * 70) + print("AirSimNH 无人机感知探索系统 - 集成前视窗口版") + print("注意: 默认分辨率较低(256x144)，如需高清画面请修改settings.json") + print("=" * 70) main() \ No newline at end of file From 76c312d6fff4b48ac55d886e874c4f50b81c3f84 Mon Sep 17 00:00:00 2001 From: rhcgg <3136924319@qq.com> Date: Fri, 19 Dec 2025 17:53:28 +0800 Subject: [PATCH 06/23] =?UTF-8?q?=20=E8=BF=9B=E4=B8=80=E6=AD=A5=E5=AE=8C?= =?UTF-8?q?=E5=96=84=E5=8C=96=E7=A0=81=E4=BD=BF=E5=85=B6=E5=81=A5=E5=A3=AE?= =?UTF-8?q?=E4=B8=8E=E5=B7=A5=E7=A8=8B=E5=8C=96?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit 配置集中管理：所有参数在 config.py 中统一管理配置调整 修改 config.py 中的参数即可调整所有设置 无需修改主程序代码 重要参数都有中文注释说明 结构化日志系统：彩色控制台输出 + 文件日志，支持不同日志级别 运行统计与报告：自动生成详细的任务报告 --- src/drone_perception/config.py | 72 + .../perceptive_drone_explorer.py | 1283 +++++++++++------ 2 files changed, 888 insertions(+), 467 deletions(-) create mode 100644 src/drone_perception/config.py diff --git a/src/drone_perception/config.py b/src/drone_perception/config.py new file mode 100644 index 0000000000..e02a8330af --- /dev/null +++ b/src/drone_perception/config.py @@ -0,0 +1,72 @@ +# config.py +""" +AirSimNH 无人机项目配置文件 +所有可调参数集中在此管理 +""" + +# ==================== 飞行与探索参数 ==================== +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, # 开阔度阈值，大于此值认为开阔 + } +} + +# ==================== 前视窗口参数 ==================== +DISPLAY = { + 'WINDOW_WIDTH': 640, # 窗口宽度 + 'WINDOW_HEIGHT': 480, # 窗口高度 + 'ENABLE_SHARPENING': True, # 启用图像锐化，改善模糊 + 'SHOW_INFO_OVERLAY': True, # 显示信息叠加层 + 'REFRESH_RATE_MS': 30, # 刷新率 (毫秒)，建议30-50 +} + +# ==================== 系统与安全参数 ==================== +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, # 健康检查间隔 (循环次数) +} + +# ==================== 相机配置 ==================== +# 注意：这里的相机名称需要与AirSim环境中的相机名称匹配 +CAMERA = { + 'DEFAULT_NAME': "0", # 默认相机名称 + # 如果"0"无效，可以尝试以下名称: + # 'POSSIBLE_NAMES': ["0", "1", "front_center", "front", "CameraActor_0"] +} + +# ==================== 调试参数 ==================== +DEBUG = { + 'SAVE_PERCEPTION_IMAGES': False, # 是否保存感知图像用于调试 + 'IMAGE_SAVE_INTERVAL': 50, # 图像保存间隔 (循环次数) + 'LOG_DECISION_DETAILS': False, # 是否记录详细决策信息 +} \ No newline at end of file diff --git a/src/drone_perception/perceptive_drone_explorer.py b/src/drone_perception/perceptive_drone_explorer.py index d22453b7d6..61e783259b 100644 --- a/src/drone_perception/perceptive_drone_explorer.py +++ b/src/drone_perception/perceptive_drone_explorer.py @@ -1,7 +1,8 @@ """ -AirSimNH 感知驱动自主探索无人机 +AirSimNH 感知驱动自主探索无人机 - 工程优化版 核心：视觉感知 → 语义理解 → 智能决策 → 安全执行 -集成前视窗口版本 - 支持实时视觉监控 +集成：配置管理、日志系统、异常恢复、前视窗口显示 +版本: 2.0 (工程优化版) """ import airsim @@ -13,18 +14,48 @@ from dataclasses import dataclass from enum import Enum import threading -from typing import Tuple, List, Optional - -# ============== 新增：导入队列模块 ============== import queue +import signal +import sys +from typing import Tuple, List, Optional, Dict +import traceback +import logging +from datetime import datetime + +# ============ 导入配置文件 ============ +try: + import config + CONFIG_LOADED = True +except ImportError as e: + print(f"❌ 无法加载配置文件 config.py: {e}") + print("正在使用默认配置...") + CONFIG_LOADED = False + # 临时默认配置 + class DefaultConfig: + EXPLORATION = {'TOTAL_TIME': 180, 'PREFERRED_SPEED': 3.0, 'BASE_HEIGHT': -15.0, + 'MAX_ALTITUDE': -30.0, 'MIN_ALTITUDE': -8.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': [-45, -30, -15, 0, 15, 30, 45], + 'HEIGHT_STRATEGY': {'STEEP_SLOPE': -20.0, 'OPEN_SPACE': -12.0, + 'DEFAULT': -15.0, 'SLOPE_THRESHOLD': 5.0, + 'OPENNESS_THRESHOLD': 0.7}} + DISPLAY = {'WINDOW_WIDTH': 640, 'WINDOW_HEIGHT': 480, 'ENABLE_SHARPENING': True, + 'SHOW_INFO_OVERLAY': True, 'REFRESH_RATE_MS': 30} + 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': 10} + CAMERA = {'DEFAULT_NAME': "0"} + config = DefaultConfig() class FlightState(Enum): - """无人机状态枚举""" + """无人机飞行状态枚举""" TAKEOFF = "起飞" HOVERING = "悬停观测" EXPLORING = "主动探索" AVOIDING = "避障机动" + RETURNING = "返航中" LANDING = "降落" EMERGENCY = "紧急状态" @@ -37,9 +68,8 @@ class PerceptionResult: obstacle_direction: float = 0.0 # 障碍物相对方向（弧度） terrain_slope: float = 0.0 # 地形坡度 open_space_score: float = 0.0 # 开阔度评分 (0-1) - recommended_height: float = -15.0 # 推荐飞行高度 + recommended_height: float = config.PERCEPTION['HEIGHT_STRATEGY']['DEFAULT'] safe_directions: List[float] = None # 安全方向列表 - # ========== 新增：前视图像字段 ========== front_image: Optional[np.ndarray] = None # 前视图像 def __post_init__(self): @@ -47,462 +77,46 @@ def __post_init__(self): self.safe_directions = [] -class PerceptiveExplorer: - """基于感知的自主探索无人机""" - - def __init__(self, drone_name=""): - print("=" * 60) - print("AirSimNH 感知驱动自主探索系统") - print("=" * 60) - - # 初始化AirSim连接 - self.client = airsim.MultirotorClient() - self.client.confirmConnection() - self.drone_name = drone_name - - # 启用API控制 - self.client.enableApiControl(True, vehicle_name=drone_name) - self.client.armDisarm(True, vehicle_name=drone_name) - - # 状态管理 - self.state = FlightState.TAKEOFF - self.state_history = deque(maxlen=20) - self.emergency_flag = False - - # 感知参数 - self.depth_threshold_near = 5.0 # 近距离警报阈值(米) - self.depth_threshold_safe = 10.0 # 安全距离阈值(米) - self.min_ground_clearance = 2.0 # 最小离地间隙(米) - self.max_pitch_angle = math.radians(15) # 最大允许俯仰角 - - # 探索参数 - self.exploration_time = 180 # 总探索时间(秒) - self.preferred_speed = 3.0 # 优选速度(m/s) - self.max_altitude = -30 # 最大海拔(米) - self.min_altitude = -8 # 最小海拔(米) - - # 记忆系统 - self.visited_positions = deque(maxlen=100) - self.obstacle_map = {} # 障碍物位置记忆 - self.traversability_map = {} # 地形可通行性记忆 - - # 性能监控 - self.perception_fps = 0 - self.decision_fps = 0 - self.start_time = time.time() - - # ========== 新增：前视窗口初始化 ========== - self.front_display = FrontViewDisplay( - window_name=f"无人机前视 - {drone_name or 'AirSimNH'}" - ) - print("🎥 前视窗口已初始化") - - print("✅ 系统初始化完成") - print(f" 开始时间: {time.strftime('%H:%M:%S')}") - print(f" 预计探索时长: {self.exploration_time}秒") - - def get_depth_perception(self) -> PerceptionResult: - """获取并分析深度图像，理解环境""" - result = PerceptionResult() - - try: - # ========== 修改：同时获取深度图像和前视图像 ========== - responses = self.client.simGetImages([ - airsim.ImageRequest( - "0", - airsim.ImageType.DepthPlanar, - pixels_as_float=True, - compress=False - ), - # 新增：获取前视RGB图像 - airsim.ImageRequest( - "0", - airsim.ImageType.Scene, - False, - False - ) - ]) - - if not responses or len(responses) < 2: - print("⚠ 图像获取失败") - return result - - # 处理深度图像（原逻辑） - depth_img = responses[0] - 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 - - # 1. 前方近距离区域（紧急避障） - 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 - - # 2. 多方向扇形扫描 - directions = [] - scan_angles = [-45, -30, -15, 0, 15, 30, 45] # 度 - - for angle_deg in 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) # 使用25%分位数（较保守） - directions.append((angle_rad, dir_distance)) - - if dir_distance > self.depth_threshold_safe: - result.safe_directions.append(angle_rad) - - # 3. 地形分析（通过深度梯度估计坡度） - 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 - - # 4. 开阔度评分（基于有效距离像素比例） - 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 directions: - # 找出最近障碍物的方向 - closest_dir = min(directions, key=lambda x: x[1]) - result.obstacle_direction = closest_dir[0] - - # 根据感知动态调整推荐高度 - if result.terrain_slope > 5: - result.recommended_height = -20 # 陡峭地形飞高些 - elif result.open_space_score > 0.7: - result.recommended_height = -12 # 开阔地带可以飞低些 - else: - result.recommended_height = -15 # 默认高度 - - # ========== 新增：处理前视图像 ========== - front_response = responses[1] - if front_response and front_response.image_data_uint8: - # 转换图像格式 - img_array = np.frombuffer(front_response.image_data_uint8, dtype=np.uint8) - img_rgb = img_array.reshape(front_response.height, front_response.width, 3) - img_bgr = cv2.cvtColor(img_rgb, cv2.COLOR_RGB2BGR) - result.front_image = img_bgr - - # 准备显示信息 - state = self.client.getMultirotorState(vehicle_name=self.drone_name) - pos = state.kinematics_estimated.position - display_info = { - 'state': self.state.value, - 'obstacle_distance': result.obstacle_distance, - 'position': (pos.x_val, pos.y_val, pos.z_val) - } - - # 更新前视窗口 - self.front_display.update_image(img_bgr, display_info) - - # 更新感知FPS - self.perception_fps = 1 / (time.time() - self.perception_start) - - except Exception as e: - print(f"❌ 深度感知异常: {e}") - - return result - - # 注意：get_visual_perception 方法现在可能多余，但为了兼容性保留 - def get_visual_perception(self): - """获取视觉图像用于高级感知（可选）""" - try: - responses = self.client.simGetImages([ - airsim.ImageRequest("0", airsim.ImageType.Scene, False, False) - ]) - - if responses and responses[0]: - # 转换为OpenCV格式 - img_data = responses[0].image_data_uint8 - img_array = np.frombuffer(img_data, dtype=np.uint8) - img = img_array.reshape(responses[0].height, responses[0].width, 3) - - # 简单颜色分析（示例：寻找绿色植被区域） - hsv = cv2.cvtColor(img, cv2.COLOR_RGB2HSV) - green_mask = cv2.inRange(hsv, (40, 40, 40), (80, 255, 255)) - green_ratio = np.sum(green_mask > 0) / green_mask.size - - return img, green_ratio - except: - pass - - return None, 0 - - def make_intelligent_decision(self, perception: PerceptionResult) -> Tuple[float, float, float, float]: - """基于感知结果做出智能决策""" - - # 获取当前位置和状态 - 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 = -10 # 起飞到10米 - if pos.z_val < -9.5: - self.change_state(FlightState.HOVERING) - - elif self.state == FlightState.HOVERING: - # 悬停观察，缓慢旋转扫描 - target_yaw = (time.time() % 10) * 0.2 # 缓慢旋转 - - if len(perception.safe_directions) > 0: - 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, -vel.y_val - else: - # 选择最佳探索方向 - if perception.safe_directions: - # 优先选择与当前航向相差45-90度的新方向（避免来回摆动） - current_yaw = airsim.to_eularian_angles( - state.kinematics_estimated.orientation - )[2] - - # 过滤出与当前方向不同的安全方向 - diverse_dirs = [ - d for d in perception.safe_directions - if abs(d - current_yaw) > math.radians(45) - ] - - if diverse_dirs: - best_dir = diverse_dirs[0] - else: - best_dir = perception.safe_directions[0] - - # 设置前进速度 - speed_factor = min(1.0, perception.open_space_score * 1.5) - target_vx = self.preferred_speed * speed_factor * math.cos(best_dir) - target_vy = self.preferred_speed * speed_factor * math.sin(best_dir) - else: - # 没有安全方向，爬升 - target_z = pos.z_val - 5 - self.change_state(FlightState.AVOIDING) - - elif self.state == FlightState.AVOIDING: - # 避障机动 - if perception.has_obstacle: - # 根据障碍物方向决定避障策略 - if abs(perception.obstacle_direction) < math.radians(30): - # 前方障碍物：爬升 - target_z = pos.z_val - 3 - target_vx, target_vy = 0, 0 - else: - # 侧方障碍物：向反方向平移 - avoid_dir = perception.obstacle_direction + math.pi - target_vx = 1.5 * math.cos(avoid_dir) - target_vy = 1.5 * math.sin(avoid_dir) - else: - # 障碍物清除，返回探索 - self.change_state(FlightState.HOVERING) - time.sleep(1) # 避障后暂停观察 - - elif self.state == FlightState.EMERGENCY: - # 紧急状态：悬停并准备降落 - target_vx, target_vy, target_yaw = 0, 0, 0 - target_z = max(pos.z_val, -20) # 限制爬升 - - # 确保高度在安全范围内 - target_z = max(self.max_altitude, min(self.min_altitude, target_z)) - - return target_vx, target_vy, target_z, target_yaw - - def change_state(self, new_state: FlightState): - """状态转换""" - if self.state != new_state: - print(f"🔄 状态转换: {self.state.value} → {new_state.value}") - self.state = new_state - self.state_history.append((time.time(), new_state)) - - def run_perception_loop(self): - """主感知-决策-控制循环""" - print("\n" + "=" * 60) - print("启动感知-决策-控制循环") - print("=" * 60) - - # 起飞 - print("🚀 起飞中...") - self.client.takeoffAsync(vehicle_name=self.drone_name).join() - time.sleep(3) - self.change_state(FlightState.HOVERING) - - # 主循环 - loop_count = 0 - exploration_start = time.time() - - while time.time() - exploration_start < self.exploration_time and not self.emergency_flag: - loop_start = time.time() - loop_count += 1 +# ==================== 前视显示窗口类 ==================== - # 1. 感知阶段 - self.perception_start = time.time() - perception = self.get_depth_perception() - # visual_img, green_ratio = self.get_visual_perception() # 可选 - - # 2. 决策阶段 - decision = self.make_intelligent_decision(perception) - - # 3. 控制执行阶段 - target_vx, target_vy, target_z, target_yaw = decision - - # 使用速度控制（更灵活）或位置控制（更精确） - use_velocity_control = self.state in [FlightState.EXPLORING, FlightState.AVOIDING] - - if use_velocity_control: - self.client.moveByVelocityZAsync( - target_vx, target_vy, target_z, 0.5, # 持续时间0.5秒 - 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)) - - # 性能监控输出 - if loop_count % 20 == 0: - elapsed = time.time() - exploration_start - print(f"\n📊 循环{loop_count} | 已运行{elapsed:.1f}s | 状态:{self.state.value}") - print(f" 感知FPS:{self.perception_fps:.1f} | 障碍:{perception.has_obstacle}") - print(f" 最近障碍:{perception.obstacle_distance:.1f}m | 开阔度:{perception.open_space_score:.2f}") - print(f" 位置:({pos.x_val:.1f}, {pos.y_val:.1f}, {-pos.z_val:.1f}m)") - print(f" 安全方向数:{len(perception.safe_directions)}") - - # 循环频率控制（10Hz） - loop_time = time.time() - loop_start - if loop_time < 0.1: - time.sleep(0.1 - loop_time) - - # 探索结束，准备降落 - print("\n" + "=" * 60) - print("探索完成，开始返航降落") - print("=" * 60) - - self.change_state(FlightState.LANDING) - self.return_to_start() - - def return_to_start(self): - """返回起始点附近并降落""" - try: - # 回到起点附近（简单实现：回到原点） - print("↩️ 返回起始区域...") - self.client.moveToPositionAsync(0, 0, -10, 5, vehicle_name=self.drone_name).join() - time.sleep(2) - - # 降落 - print("🛬 降落中...") - self.client.landAsync(vehicle_name=self.drone_name).join() - time.sleep(3) - - # 断开控制 - self.client.armDisarm(False, vehicle_name=self.drone_name) - self.client.enableApiControl(False, vehicle_name=self.drone_name) - - # ========== 新增：关闭前视窗口 ========== - self.front_display.stop() - - print("✅ 任务完成，系统安全关闭") - - except Exception as e: - print(f"❌ 降落异常: {e}") - print("⚠ 尝试紧急降落...") - try: - self.client.landAsync(vehicle_name=self.drone_name).join() - except: - pass - - def emergency_stop(self): - """紧急停止""" - print("🆘 紧急停止触发！") - self.emergency_flag = True - self.change_state(FlightState.EMERGENCY) - self.client.hoverAsync(vehicle_name=self.drone_name).join() - - # ========== 新增：关闭前视窗口 ========== - self.front_display.stop() - - -def main(): - """主程序入口""" - try: - # 创建感知探索器 - explorer = PerceptiveExplorer(drone_name="") - - # 设置键盘中断处理 - import signal - def signal_handler(sig, frame): - print("\n⚠ 用户中断，正在安全停止...") - explorer.emergency_stop() - exit(0) - - signal.signal(signal.SIGINT, signal_handler) - - # 运行主循环 - explorer.run_perception_loop() - - except Exception as e: - print(f"\n❌ 系统异常: {e}") - import traceback - traceback.print_exc() - - # 尝试安全降落 - try: - client = airsim.MultirotorClient() - client.landAsync().join() - client.armDisarm(False) - client.enableApiControl(False) - except: - pass - - -# ============== 新增：前视窗口显示类 ============== class FrontViewDisplay: """前视画面显示管理器""" - def __init__(self, window_name="无人机前视画面", width=640, height=480): + def __init__(self, window_name="无人机前视画面", width=None, height=None, + enable_sharpening=None, show_info=None): + # 使用配置参数或传入参数 self.window_name = window_name - self.window_width = width - self.window_height = height + self.window_width = width if width is not None else config.DISPLAY['WINDOW_WIDTH'] + self.window_height = height if height is not None else config.DISPLAY['WINDOW_HEIGHT'] + self.enable_sharpening = (enable_sharpening if enable_sharpening is not None + else config.DISPLAY['ENABLE_SHARPENING']) + self.show_info = (show_info if show_info is not None + else config.DISPLAY['SHOW_INFO_OVERLAY']) # 图像队列（线程安全） - self.image_queue = queue.Queue(maxsize=2) + self.image_queue = queue.Queue(maxsize=3) self.display_active = True self.display_thread = None # 显示状态 self.paused = False - self.show_info = True - self.enable_sharpening = True # 启用锐化改善模糊 + + # 显示统计 + 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, @@ -516,14 +130,14 @@ def stop(self): if self.display_thread: self.display_thread.join(timeout=2.0) - def update_image(self, image_data: np.ndarray, info: dict): + def update_image(self, image_data: np.ndarray, info: Optional[Dict] = 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: + # 图像增强处理 + 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]]) @@ -552,6 +166,13 @@ 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((300, 400, 3), dtype=np.uint8) + cv2.putText(wait_img, "等待无人机图像...", (50, 150), + cv2.FONT_HERSHEY_SIMPLEX, 0.8, (255, 255, 255), 2) + cv2.imshow(self.window_name, wait_img) + cv2.waitKey(100) # 关键：给窗口时间初始化 + print("💡 前视窗口控制:") print(" - 按 'Q': 关闭窗口 | 'S': 保存截图") print(" - 按 'P': 暂停/继续 | 'I': 切换信息显示") @@ -568,6 +189,9 @@ def _display_loop(self): display_image = packet['image'] info = packet['info'] + # 更新统计 + self._update_stats() + # 清空队列中的旧帧 while not self.image_queue.empty(): self.image_queue.get_nowait() @@ -581,9 +205,15 @@ def _display_loop(self): display_image = self._add_info_overlay(display_image, info) cv2.imshow(self.window_name, display_image) + elif self.paused: + # 暂停时显示提示 + blank = np.zeros((300, 400, 3), dtype=np.uint8) + cv2.putText(blank, "PAUSED", (120, 150), + cv2.FONT_HERSHEY_SIMPLEX, 1.5, (0, 0, 255), 3) + cv2.imshow(self.window_name, blank) # 键盘事件处理 - key = cv2.waitKey(30) & 0xFF + key = cv2.waitKey(config.DISPLAY.get('REFRESH_RATE_MS', 30)) & 0xFF if key == ord('q') or key == ord('Q'): print("🔄 用户关闭显示窗口") @@ -605,43 +235,62 @@ def _display_loop(self): print(f"🔍 图像锐化{status}") cv2.destroyWindow(self.window_name) + cv2.waitKey(1) + + def _update_stats(self): + """更新显示统计信息""" + now = time.time() + self.display_stats['frame_count'] += 1 - def _add_info_overlay(self, image: np.ndarray, info: dict) -> np.ndarray: + 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) -> np.ndarray: """在图像上叠加状态信息""" + if image is None or image.size == 0: + return image + try: + overlay = image.copy() height, width = image.shape[:2] # 创建半透明信息栏 - info_height = 80 - overlay = image.copy() + info_height = 100 cv2.rectangle(overlay, (0, 0), (width, info_height), (0, 0, 0), -1) cv2.addWeighted(overlay, 0.7, image, 0.3, 0, image) # 飞行状态 state = info.get('state', 'UNKNOWN') state_color = (0, 255, 0) if '探索' in state else (0, 255, 255) if '悬停' in state else (0, 0, 255) - cv2.putText(image, f"状态: {state}", (10, 25), - cv2.FONT_HERSHEY_SIMPLEX, 0.6, state_color, 2) + cv2.putText(image, f"状态: {state}", (10, 30), + cv2.FONT_HERSHEY_SIMPLEX, 0.7, state_color, 2) # 位置信息 pos = info.get('position', (0, 0, 0)) - cv2.putText(image, f"位置: ({pos[0]:.1f}, {pos[1]:.1f}, {-pos[2]:.1f}m)", (10, 50), - cv2.FONT_HERSHEY_SIMPLEX, 0.5, (255, 255, 255), 1) + cv2.putText(image, f"位置: ({pos[0]:.1f}, {pos[1]:.1f}, {-pos[2]:.1f}m)", (10, 60), + cv2.FONT_HERSHEY_SIMPLEX, 0.6, (255, 255, 255), 1) # 障碍物信息 obs_dist = info.get('obstacle_distance', 0.0) - if obs_dist < 100: - obs_color = (0, 0, 255) if obs_dist < 5.0 else (0, 165, 255) if obs_dist < 10.0 else (0, 255, 0) - cv2.putText(image, f"障碍: {obs_dist:.1f}m", (width - 120, 30), - cv2.FONT_HERSHEY_SIMPLEX, 0.6, obs_color, 1) + obs_color = (0, 0, 255) if obs_dist < 5.0 else (0, 165, 255) if obs_dist < 10.0 else (0, 255, 0) + cv2.putText(image, f"障碍: {obs_dist:.1f}m", (10, 90), + cv2.FONT_HERSHEY_SIMPLEX, 0.7, obs_color, 2) + + # 显示统计信息 + fps_text = f"FPS: {self.display_stats['fps']:.1f}" + cv2.putText(image, fps_text, (width - 120, 30), + cv2.FONT_HERSHEY_SIMPLEX, 0.6, (255, 255, 0), 1) - # 清晰度提示 + # 清晰度提示（低分辨率时显示） if height < 200: - cv2.putText(image, "提示: 修改settings.json可提高分辨率", (10, height-10), + cv2.putText(image, "提示: 修改config.py可提高分辨率", (10, height-10), cv2.FONT_HERSHEY_SIMPLEX, 0.4, (200, 200, 200), 1) return image - except Exception: + except Exception as e: + print(f"⚠️ 添加信息叠加层出错: {e}") return image def _save_screenshot(self, image: Optional[np.ndarray]): @@ -651,11 +300,711 @@ def _save_screenshot(self, image: Optional[np.ndarray]): filename = f"drone_snapshot_{timestamp}.png" cv2.imwrite(filename, image) print(f"📸 截图已保存: {filename}") + else: + print("⚠️ 无法保存截图：无有效图像数据") -if __name__ == "__main__": +# ==================== 主探索器类 ==================== + +class PerceptiveExplorer: + """基于感知的自主探索无人机 - 工程优化版""" + + def __init__(self, drone_name=""): + # 初始化日志系统 + self._setup_logging() + self.logger.info("=" * 60) + self.logger.info("AirSimNH 感知驱动自主探索系统 - 工程优化版 初始化") + self.logger.info("=" * 60) + + # 初始化AirSim连接 + self.client = None + self.drone_name = drone_name + self._connect_to_airsim() + + # 启用API控制 + 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.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.stats = { + 'perception_cycles': 0, + 'decision_cycles': 0, + 'exceptions_caught': 0, + 'obstacles_detected': 0, + 'state_changes': 0, + 'front_image_updates': 0, + } + + # 前视窗口 + self.front_display = None + self._setup_front_display() + + self.logger.info("✅ 系统初始化完成") + self.logger.info(f" 开始时间: {datetime.now().strftime('%H:%M:%S')}") + self.logger.info(f" 预计探索时长: {self.exploration_time}秒") + self.logger.info(f" 日志级别: {config.SYSTEM['LOG_LEVEL']}") + + 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 _connect_to_airsim(self): + """连接到AirSim，支持重试机制""" + 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 _setup_front_display(self): + """初始化前视显示窗口""" + try: + self.front_display = FrontViewDisplay( + window_name=f"无人机前视 - {self.drone_name or 'AirSimNH'}", + width=config.DISPLAY['WINDOW_WIDTH'], + height=config.DISPLAY['WINDOW_HEIGHT'], + enable_sharpening=config.DISPLAY['ENABLE_SHARPENING'], + show_info=config.DISPLAY['SHOW_INFO_OVERLAY'] + ) + self.logger.info("🎥 前视窗口已初始化") + except Exception as e: + self.logger.error(f"❌ 前视窗口初始化失败: {e}") + self.front_display = None + + 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] + 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) + + # 地形分析 + 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}") + return result + except Exception as e: + self.logger.error(f"❌ 深度图像处理异常: {e}") + return result + + # 处理前视图像 + front_response = responses[1] + if front_response and hasattr(front_response, 'image_data_uint8'): + try: + # 转换为OpenCV格式 + 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) + result.front_image = img_bgr + + # 准备显示信息 + display_info = self._prepare_display_info(result) + + # 更新前视窗口 + if self.front_display: + self.front_display.update_image(img_bgr, display_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: + self.logger.debug(f"感知结果: 障碍={result.has_obstacle}, 距离={result.obstacle_distance:.1f}m, " + f"开阔度={result.open_space_score:.2f}") + + except airsim.ClientException as e: + self.logger.error(f"❌ AirSim客户端异常: {e}") + self.stats['exceptions_caught'] += 1 + self._handle_airsim_exception(e) + except ConnectionError as e: + self.logger.error(f"❌ 网络连接异常: {e}") + self.stats['exceptions_caught'] += 1 + self._handle_connection_error() + except Exception as e: + self.logger.error(f"❌ 感知过程中发生未知异常: {e}") + self.logger.debug(f"异常详情: {traceback.format_exc()}") + self.stats['exceptions_caught'] += 1 + + return result + + def _prepare_display_info(self, perception: PerceptionResult) -> Dict: + """准备显示信息""" + try: + state = self.client.getMultirotorState(vehicle_name=self.drone_name) + pos = state.kinematics_estimated.position + return { + 'state': self.state.value, + 'obstacle_distance': perception.obstacle_distance, + 'position': (pos.x_val, pos.y_val, pos.z_val), + 'loop_count': self.loop_count, + } + except: + return {} + + def _check_connection_health(self): + """检查连接健康状态""" + try: + # 简单的ping测试 + self.client.ping() + self.logger.info("✅ 连接健康检查通过") + return True + except Exception as e: + self.logger.warning(f"⚠️ 连接健康检查失败: {e}") + if self.reconnect_attempts < config.SYSTEM['MAX_RECONNECT_ATTEMPTS']: + self.logger.info("尝试重新连接...") + self._connect_to_airsim() + self.reconnect_attempts += 1 + return False + + def _handle_airsim_exception(self, exception): + """处理AirSim特定异常""" + error_msg = str(exception) + if "timeout" in error_msg.lower(): + self.logger.warning("⏰ AirSim响应超时，等待后继续...") + time.sleep(1.0) + elif "not connected" in error_msg.lower(): + self.logger.error("🔌 与AirSim连接断开") + self._handle_connection_error() + + def _handle_connection_error(self): + """处理连接错误""" + if self.emergency_flag: + return + + self.logger.warning("🔄 尝试恢复连接...") + success = self._check_connection_health() + if not success and not self.emergency_flag: + self.logger.error("❌ 无法恢复连接，启动紧急降落程序") + self.emergency_stop() + + def get_visual_perception(self): + """获取视觉图像用于高级感知（可选）""" + try: + responses = self.client.simGetImages([ + airsim.ImageRequest("0", airsim.ImageType.Scene, False, False) + ]) + + if responses and responses[0]: + img_data = responses[0].image_data_uint8 + img_array = np.frombuffer(img_data, dtype=np.uint8) + img = img_array.reshape(responses[0].height, responses[0].width, 3) + + hsv = cv2.cvtColor(img, cv2.COLOR_RGB2HSV) + green_mask = cv2.inRange(hsv, (40, 40, 40), (80, 255, 255)) + green_ratio = np.sum(green_mask > 0) / green_mask.size + + return img, green_ratio + except: + pass + + return None, 0 + + def make_intelligent_decision(self, perception: PerceptionResult) -> Tuple[float, float, float, float]: + """基于感知结果做出智能决策""" + self.stats['decision_cycles'] += 1 + + 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 + + if len(perception.safe_directions) > 0: + 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, -vel.y_val + else: + if perception.safe_directions: + current_yaw = airsim.to_eularian_angles( + state.kinematics_estimated.orientation + )[2] + + diverse_dirs = [ + d for d in perception.safe_directions + if abs(d - current_yaw) > math.radians(45) + ] + + if diverse_dirs: + best_dir = diverse_dirs[0] + else: + best_dir = perception.safe_directions[0] + + speed_factor = min(1.0, perception.open_space_score * 1.5) + target_vx = self.preferred_speed * speed_factor * math.cos(best_dir) + target_vy = self.preferred_speed * speed_factor * math.sin(best_dir) + else: + target_z = pos.z_val - 5 + self.change_state(FlightState.AVOIDING) + + elif self.state == FlightState.AVOIDING: + if perception.has_obstacle: + if abs(perception.obstacle_direction) < math.radians(30): + target_z = pos.z_val - 3 + target_vx, target_vy = 0, 0 + else: + avoid_dir = perception.obstacle_direction + math.pi + target_vx = 1.5 * math.cos(avoid_dir) + target_vy = 1.5 * math.sin(avoid_dir) + else: + self.change_state(FlightState.HOVERING) + time.sleep(1) + + elif self.state == FlightState.EMERGENCY: + target_vx, target_vy, target_yaw = 0, 0, 0 + target_z = max(pos.z_val, -20) + + # 高度安全限制 + target_z = max(self.max_altitude, min(self.min_altitude, target_z)) + + return target_vx, target_vy, target_z, target_yaw + + except Exception as e: + self.logger.error(f"❌ 决策过程异常: {e}") + return 0.0, 0.0, self.base_height, 0.0 + + def change_state(self, new_state: FlightState): + """状态转换""" + if self.state != new_state: + self.logger.info(f"🔄 状态转换: {self.state.value} → {new_state.value}") + self.state = new_state + self.state_history.append((time.time(), new_state)) + self.stats['state_changes'] += 1 + + def _execute_control_decision(self, decision): + """执行控制决策，增强异常处理""" + try: + target_vx, target_vy, target_z, target_yaw = decision + + if self.state in [FlightState.EXPLORING, FlightState.AVOIDING]: + self.client.moveByVelocityZAsync( + target_vx, target_vy, target_z, 0.5, + 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}") + 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" 系统统计: 异常{self.stats['exceptions_caught']}次 " + f"| 状态切换{self.stats['state_changes']}次") + except: + self.logger.info("状态报告: 无法获取无人机状态") + + 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() + + # 1. 感知阶段 + perception = self.get_depth_perception() + + # 2. 决策阶段 + decision = self.make_intelligent_decision(perception) + + # 3. 控制执行阶段 + self._execute_control_decision(decision) + + # 定期状态报告 + if self.loop_count % config.SYSTEM.get('HEALTH_CHECK_INTERVAL', 20) == 0: + self._report_status(exploration_start, perception) + + # 循环频率控制 + 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 _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_display: + self.front_display.stop() + self.logger.info("✅ 前视窗口已关闭") + + 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['front_image_updates']}") + self.logger.info(f" 捕获的异常数: {self.stats['exceptions_caught']}") + self.logger.info(f" 重连尝试次数: {self.reconnect_attempts}") + + # 保存报告到文件 + 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['exceptions_caught']}\n") + f.write(f"前视图像更新次数: {self.stats['front_image_updates']}\n") + f.write("=" * 50 + "\n") + f.write("飞行航点记录:\n") + for i, pos in enumerate(self.visited_positions[:20]): # 只记录前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") + 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_display: + self.front_display.stop() + + self._cleanup_system() + + +# ==================== 主程序入口 ==================== + +def main(): + """主程序入口""" + # 显示启动信息 print("=" * 70) - print("AirSimNH 无人机感知探索系统 - 集成前视窗口版") - print("注意: 默认分辨率较低(256x144)，如需高清画面请修改settings.json") + 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) + + # 用户确认 + response = input("按 Enter 键开始运行，或输入 'q' 退出: ") + if response.lower() == 'q': + print("程序退出") + return + + 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) + + # 运行主循环 + explorer.run_perception_loop() + + 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 51ff05e8aae5894c9efd4ecc95f12924115bdcad Mon Sep 17 00:00:00 2001 From: rhcgg <3136924319@qq.com> Date: Sat, 20 Dec 2025 00:49:15 +0800 Subject: [PATCH 07/23] =?UTF-8?q?=20=E6=B7=BB=E5=8A=A0=E4=BA=86=E4=B8=80?= =?UTF-8?q?=E4=B8=AA=E6=89=8B=E5=8A=A8=E6=8E=A7=E5=88=B6=EF=BC=8C=E9=80=9A?= =?UTF-8?q?=E8=BF=87=E5=89=8D=E8=A7=86=E7=AA=97=E5=8F=A3=E6=8E=A7=E5=88=B6?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit 窗口控制键（通用模式，非手动控制时生效）： Q：关闭窗口 S：保存截图 P：暂停/继续 I：切换信息显示 H：切换锐化效果 2. 手动控制键（只在手动控制模式生效）： W/S：前进/后退 A/D：左移/右移 Q/E：上升/下降 Z/X：左转/右转 空格：悬停 ESC：退出手动模式（不关闭窗口） --- src/drone_perception/config.py | 13 + .../perceptive_drone_explorer.py | 810 ++++++++++++++---- 2 files changed, 641 insertions(+), 182 deletions(-) diff --git a/src/drone_perception/config.py b/src/drone_perception/config.py index e02a8330af..2c20f8cd25 100644 --- a/src/drone_perception/config.py +++ b/src/drone_perception/config.py @@ -34,6 +34,19 @@ } } +# ==================== 手动控制参数 ==================== +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 = { 'WINDOW_WIDTH': 640, # 窗口宽度 diff --git a/src/drone_perception/perceptive_drone_explorer.py b/src/drone_perception/perceptive_drone_explorer.py index 61e783259b..c88534a5e0 100644 --- a/src/drone_perception/perceptive_drone_explorer.py +++ b/src/drone_perception/perceptive_drone_explorer.py @@ -2,7 +2,8 @@ AirSimNH 感知驱动自主探索无人机 - 工程优化版 核心：视觉感知 → 语义理解 → 智能决策 → 安全执行 集成：配置管理、日志系统、异常恢复、前视窗口显示 -版本: 2.0 (工程优化版) +新增：修复按键冲突，改进手动控制 +版本: 2.5 (修复按键冲突改进版) """ import airsim @@ -30,13 +31,12 @@ print(f"❌ 无法加载配置文件 config.py: {e}") print("正在使用默认配置...") CONFIG_LOADED = False - # 临时默认配置 class DefaultConfig: - EXPLORATION = {'TOTAL_TIME': 180, 'PREFERRED_SPEED': 3.0, 'BASE_HEIGHT': -15.0, - 'MAX_ALTITUDE': -30.0, 'MIN_ALTITUDE': -8.0, 'TAKEOFF_HEIGHT': -10.0} + 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': [-45, -30, -15, 0, 15, 30, 45], + '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}} @@ -44,8 +44,19 @@ class DefaultConfig: 'SHOW_INFO_OVERLAY': True, 'REFRESH_RATE_MS': 30} 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': 10} + 'ENABLE_HEALTH_CHECK': True, 'HEALTH_CHECK_INTERVAL': 20} CAMERA = {'DEFAULT_NAME': "0"} + 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 + } config = DefaultConfig() @@ -58,6 +69,7 @@ class FlightState(Enum): RETURNING = "返航中" LANDING = "降落" EMERGENCY = "紧急状态" + MANUAL = "手动控制" @dataclass @@ -65,26 +77,23 @@ 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 # 开阔度评分 (0-1) + 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 # 前视图像 + safe_directions: List[float] = None + front_image: Optional[np.ndarray] = None def __post_init__(self): if self.safe_directions is None: self.safe_directions = [] -# ==================== 前视显示窗口类 ==================== - class FrontViewDisplay: - """前视画面显示管理器""" + """前视画面显示管理器 - 修复按键冲突改进版""" def __init__(self, window_name="无人机前视画面", width=None, height=None, enable_sharpening=None, show_info=None): - # 使用配置参数或传入参数 self.window_name = window_name self.window_width = width if width is not None else config.DISPLAY['WINDOW_WIDTH'] self.window_height = height if height is not None else config.DISPLAY['WINDOW_HEIGHT'] @@ -93,14 +102,21 @@ def __init__(self, window_name="无人机前视画面", width=None, height=None, self.show_info = (show_info if show_info is not None else config.DISPLAY['SHOW_INFO_OVERLAY']) - # 图像队列（线程安全） + # 图像队列 self.image_queue = queue.Queue(maxsize=3) 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, @@ -127,10 +143,12 @@ def start(self): 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): + 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 @@ -153,6 +171,7 @@ def update_image(self, image_data: np.ndarray, info: Optional[Dict] = None): display_packet = { 'image': image_data.copy(), 'info': info.copy() if info else {}, + 'manual_info': manual_info.copy() if manual_info else [], 'timestamp': time.time() } @@ -161,8 +180,24 @@ def update_image(self, image_data: np.ndarray, info: Optional[Dict] = None): 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) @@ -171,16 +206,21 @@ def _display_loop(self): cv2.putText(wait_img, "等待无人机图像...", (50, 150), cv2.FONT_HERSHEY_SIMPLEX, 0.8, (255, 255, 255), 2) cv2.imshow(self.window_name, wait_img) - cv2.waitKey(100) # 关键：给窗口时间初始化 + cv2.waitKey(100) print("💡 前视窗口控制:") - print(" - 按 'Q': 关闭窗口 | 'S': 保存截图") - print(" - 按 'P': 暂停/继续 | 'I': 切换信息显示") - print(" - 按 'H': 切换锐化效果") - - while self.display_active: + 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: # 获取最新图像 @@ -188,13 +228,17 @@ def _display_loop(self): 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(): - self.image_queue.get_nowait() + try: + self.image_queue.get_nowait() + except queue.Empty: + break except queue.Empty: pass @@ -202,7 +246,7 @@ def _display_loop(self): if display_image is not None: # 添加信息叠加 if self.show_info: - display_image = self._add_info_overlay(display_image, info) + display_image = self._add_info_overlay(display_image, info, manual_info) cv2.imshow(self.window_name, display_image) elif self.paused: @@ -212,31 +256,99 @@ def _display_loop(self): cv2.FONT_HERSHEY_SIMPLEX, 1.5, (0, 0, 255), 3) cv2.imshow(self.window_name, blank) - # 键盘事件处理 + # 处理键盘输入（非阻塞） key = cv2.waitKey(config.DISPLAY.get('REFRESH_RATE_MS', 30)) & 0xFF - if key == ord('q') or key == ord('Q'): - print("🔄 用户关闭显示窗口") + # 记录当前按键 + 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 - elif key == ord('s') or key == ord('S'): - self._save_screenshot(display_image) - elif key == ord('p') or key == ord('P'): - self.paused = not self.paused - status = "已暂停" if self.paused else "已恢复" - print(f"⏸️ 视频流{status}") - elif key == ord('i') or key == ord('I'): - self.show_info = not self.show_info - status = "开启" if self.show_info else "关闭" - print(f"📊 信息叠加层{status}") - elif key == ord('h') or key == ord('H'): - self.enable_sharpening = not self.enable_sharpening - status = "开启" if self.enable_sharpening else "关闭" - print(f"🔍 图像锐化{status}") - - cv2.destroyWindow(self.window_name) + + # 清理窗口 + try: + cv2.destroyWindow(self.window_name) + except: + pass cv2.waitKey(1) + def _handle_manual_mode_key(self, key): + """处理手动控制模式下的按键""" + key_char = chr(key).lower() if 0 <= key <= 255 else '' + + # ESC键：退出手动模式 + if key == 27: # ESC + print("收到退出手动模式指令") + self.exit_manual_flag = True + return + + # 记录手动控制按键 + self.key_states[key] = True + + # 特别处理空格键（悬停） + if key == 32: # 空格 + print("⏸️ 悬停指令") + + # 其他手动控制键已经在key_states中记录，会在主循环中处理 + + 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() @@ -247,7 +359,7 @@ def _update_stats(self): self.display_stats['frame_count'] = 0 self.display_stats['last_update'] = now - def _add_info_overlay(self, image: np.ndarray, info: Dict) -> np.ndarray: + def _add_info_overlay(self, image: np.ndarray, info: Dict, manual_info: List[str] = None) -> np.ndarray: """在图像上叠加状态信息""" if image is None or image.size == 0: return image @@ -256,14 +368,19 @@ def _add_info_overlay(self, image: np.ndarray, info: Dict) -> np.ndarray: overlay = image.copy() height, width = image.shape[:2] + # 判断是否为手动模式 + is_manual = info.get('state', '') == "手动控制" + + # 信息栏高度（手动模式需要更多空间显示控制说明） + info_height = 140 if (is_manual and manual_info) else 100 + # 创建半透明信息栏 - info_height = 100 cv2.rectangle(overlay, (0, 0), (width, info_height), (0, 0, 0), -1) cv2.addWeighted(overlay, 0.7, image, 0.3, 0, image) # 飞行状态 state = info.get('state', 'UNKNOWN') - state_color = (0, 255, 0) if '探索' in state else (0, 255, 255) if '悬停' in state else (0, 0, 255) + state_color = (0, 255, 0) if '探索' in state else (0, 255, 255) if '悬停' in state else (255, 255, 0) if '手动' in state else (0, 0, 255) cv2.putText(image, f"状态: {state}", (10, 30), cv2.FONT_HERSHEY_SIMPLEX, 0.7, state_color, 2) @@ -272,22 +389,28 @@ def _add_info_overlay(self, image: np.ndarray, info: Dict) -> np.ndarray: cv2.putText(image, f"位置: ({pos[0]:.1f}, {pos[1]:.1f}, {-pos[2]:.1f}m)", (10, 60), cv2.FONT_HERSHEY_SIMPLEX, 0.6, (255, 255, 255), 1) - # 障碍物信息 - obs_dist = info.get('obstacle_distance', 0.0) - obs_color = (0, 0, 255) if obs_dist < 5.0 else (0, 165, 255) if obs_dist < 10.0 else (0, 255, 0) - cv2.putText(image, f"障碍: {obs_dist:.1f}m", (10, 90), - cv2.FONT_HERSHEY_SIMPLEX, 0.7, obs_color, 2) + # 手动控制信息 + if is_manual and manual_info: + for i, line in enumerate(manual_info): + y_pos = 90 + i * 20 + cv2.putText(image, line, (10, y_pos), + cv2.FONT_HERSHEY_SIMPLEX, 0.5, (200, 255, 200), 1) + + # 显示手动控制提示 + cv2.putText(image, "手动控制中...", (width - 150, 60), + cv2.FONT_HERSHEY_SIMPLEX, 0.6, (255, 255, 0), 1) + elif not is_manual: + # 障碍物信息（如果不是手动模式） + obs_dist = info.get('obstacle_distance', 0.0) + obs_color = (0, 0, 255) if obs_dist < 5.0 else (0, 165, 255) if obs_dist < 10.0 else (0, 255, 0) + cv2.putText(image, f"障碍: {obs_dist:.1f}m", (10, 90), + cv2.FONT_HERSHEY_SIMPLEX, 0.7, obs_color, 2) # 显示统计信息 fps_text = f"FPS: {self.display_stats['fps']:.1f}" cv2.putText(image, fps_text, (width - 120, 30), cv2.FONT_HERSHEY_SIMPLEX, 0.6, (255, 255, 0), 1) - # 清晰度提示（低分辨率时显示） - if height < 200: - cv2.putText(image, "提示: 修改config.py可提高分辨率", (10, height-10), - cv2.FONT_HERSHEY_SIMPLEX, 0.4, (200, 200, 200), 1) - return image except Exception as e: print(f"⚠️ 添加信息叠加层出错: {e}") @@ -304,16 +427,14 @@ def _save_screenshot(self, image: Optional[np.ndarray]): print("⚠️ 无法保存截图：无有效图像数据") -# ==================== 主探索器类 ==================== - class PerceptiveExplorer: - """基于感知的自主探索无人机 - 工程优化版""" + """基于感知的自主探索无人机 - 修复按键冲突改进版""" def __init__(self, drone_name=""): # 初始化日志系统 self._setup_logging() self.logger.info("=" * 60) - self.logger.info("AirSimNH 感知驱动自主探索系统 - 工程优化版 初始化") + self.logger.info("AirSimNH 感知驱动自主探索系统 - 修复按键冲突改进版") self.logger.info("=" * 60) # 初始化AirSim连接 @@ -368,12 +489,17 @@ def __init__(self, drone_name=""): 'obstacles_detected': 0, 'state_changes': 0, 'front_image_updates': 0, + 'manual_control_time': 0.0, } # 前视窗口 self.front_display = None self._setup_front_display() + # 手动控制状态 + 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}秒") @@ -558,7 +684,11 @@ def get_depth_perception(self) -> PerceptionResult: # 更新前视窗口 if self.front_display: - self.front_display.update_image(img_bgr, display_info) + manual_info = None + if self.state == FlightState.MANUAL: + manual_info = self._get_manual_control_info() + + self.front_display.update_image(img_bgr, display_info, manual_info) self.stats['front_image_updates'] += 1 except Exception as e: @@ -600,63 +730,366 @@ def _prepare_display_info(self, perception: PerceptionResult) -> Dict: except: return {} - def _check_connection_health(self): - """检查连接健康状态""" + 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.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: - # 简单的ping测试 - self.client.ping() - self.logger.info("✅ 连接健康检查通过") - return True + # 获取当前无人机状态 + 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'] # AirSim中Z轴向下为正 + 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}") - if self.reconnect_attempts < config.SYSTEM['MAX_RECONNECT_ATTEMPTS']: - self.logger.info("尝试重新连接...") - self._connect_to_airsim() - self.reconnect_attempts += 1 - return False - - def _handle_airsim_exception(self, exception): - """处理AirSim特定异常""" - error_msg = str(exception) - if "timeout" in error_msg.lower(): - self.logger.warning("⏰ AirSim响应超时，等待后继续...") - time.sleep(1.0) - elif "not connected" in error_msg.lower(): - self.logger.error("🔌 与AirSim连接断开") - self._handle_connection_error() + self.logger.warning(f"⚠️ 手动控制应用失败: {e}") - def _handle_connection_error(self): - """处理连接错误""" - if self.emergency_flag: + def change_state(self, new_state: FlightState): + """状态转换""" + if self.state != new_state: + self.logger.info(f"🔄 状态转换: {self.state.value} → {new_state.value}") + self.state = new_state + self.state_history.append((time.time(), new_state)) + self.stats['state_changes'] += 1 + + def run_manual_control(self): + """改进的手动控制模式 - 修复按键冲突""" + self.logger.info("=" * 60) + self.logger.info("启动手动控制模式") + self.logger.info("=" * 60) + + if not self.front_display: + self.logger.error("❌ 前视窗口未初始化") return - self.logger.warning("🔄 尝试恢复连接...") - success = self._check_connection_health() - if not success and not self.emergency_flag: - self.logger.error("❌ 无法恢复连接，启动紧急降落程序") + try: + # 切换到手动控制状态 + self.change_state(FlightState.MANUAL) + self.manual_control_start = time.time() + + # 设置前视窗口为手动模式 + self.front_display.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_display.should_exit_manual(): + self.logger.info("收到退出手动模式指令") + manual_active = False + break + + # 获取前视窗口的按键状态 + if self.front_display: + window_keys = self.front_display.get_control_inputs() + self.control_keys = window_keys.copy() + + # 检查前视窗口是否还在运行 + if not self.front_display.display_active: + self.logger.info("前视窗口已关闭，退出手动模式") + manual_active = False + break + + # 应用手动控制（限制频率） + current_time = time.time() + if current_time - last_control_time >= 0.05: # 20Hz + self.apply_manual_control() + last_control_time = current_time + + # 定期获取并显示图像（降低频率，避免冲突） + if current_time - last_image_time >= 0.1: # 10Hz + 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_display: + manual_info = self._get_manual_control_info() + self.front_display.update_image(img_bgr, display_info, manual_info) + last_image_time = current_time + except Exception as img_error: + # 忽略图像获取错误，继续控制 + 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_display: + self.front_display.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("="*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: # 至少还有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 get_visual_perception(self): - """获取视觉图像用于高级感知（可选）""" + def run_perception_loop(self): + """主感知-决策-控制循环""" + self.logger.info("=" * 60) + self.logger.info("启动感知-决策-控制主循环") + self.logger.info("=" * 60) + try: - responses = self.client.simGetImages([ - airsim.ImageRequest("0", airsim.ImageType.Scene, False, False) - ]) + # 起飞 + 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) - if responses and responses[0]: - img_data = responses[0].image_data_uint8 - img_array = np.frombuffer(img_data, dtype=np.uint8) - img = img_array.reshape(responses[0].height, responses[0].width, 3) + # 主循环 + exploration_start = time.time() - hsv = cv2.cvtColor(img, cv2.COLOR_RGB2HSV) - green_mask = cv2.inRange(hsv, (40, 40, 40), (80, 255, 255)) - green_ratio = np.sum(green_mask > 0) / green_mask.size + while (time.time() - exploration_start < self.exploration_time and + not self.emergency_flag): - return img, green_ratio - except: - pass + self.loop_count += 1 + loop_start = time.time() + + # 1. 感知阶段 + perception = self.get_depth_perception() - return None, 0 + # 2. 决策阶段 + decision = self.make_intelligent_decision(perception) + + # 3. 控制执行阶段 + self._execute_control_decision(decision) + + # 定期状态报告 + if self.loop_count % config.SYSTEM.get('HEALTH_CHECK_INTERVAL', 20) == 0: + self._report_status(exploration_start, perception) + + # 循环频率控制 + 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 make_intelligent_decision(self, perception: PerceptionResult) -> Tuple[float, float, float, float]: """基于感知结果做出智能决策""" @@ -733,14 +1166,6 @@ def make_intelligent_decision(self, perception: PerceptionResult) -> Tuple[float self.logger.error(f"❌ 决策过程异常: {e}") return 0.0, 0.0, self.base_height, 0.0 - def change_state(self, new_state: FlightState): - """状态转换""" - if self.state != new_state: - self.logger.info(f"🔄 状态转换: {self.state.value} → {new_state.value}") - self.state = new_state - self.state_history.append((time.time(), new_state)) - self.stats['state_changes'] += 1 - def _execute_control_decision(self, decision): """执行控制决策，增强异常处理""" try: @@ -785,65 +1210,11 @@ def _report_status(self, exploration_start, perception): f"| 开阔度={perception.open_space_score:.2f}") self.logger.info(f" 系统统计: 异常{self.stats['exceptions_caught']}次 " f"| 状态切换{self.stats['state_changes']}次") + if self.stats['manual_control_time'] > 0: + self.logger.info(f" 手动控制: {self.stats['manual_control_time']:.1f}秒") except: self.logger.info("状态报告: 无法获取无人机状态") - 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() - - # 1. 感知阶段 - perception = self.get_depth_perception() - - # 2. 决策阶段 - decision = self.make_intelligent_decision(perception) - - # 3. 控制执行阶段 - self._execute_control_decision(decision) - - # 定期状态报告 - if self.loop_count % config.SYSTEM.get('HEALTH_CHECK_INTERVAL', 20) == 0: - self._report_status(exploration_start, perception) - - # 循环频率控制 - 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 _finish_mission(self): """完成任务并生成总结报告""" self.logger.info("=" * 60) @@ -905,6 +1276,7 @@ def _generate_summary_report(self): self.logger.info(f" 状态切换次数: {self.stats['state_changes']}") self.logger.info(f" 检测到障碍次数: {self.stats['obstacles_detected']}") self.logger.info(f" 前视图像更新次数: {self.stats['front_image_updates']}") + 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}") @@ -919,6 +1291,7 @@ def _generate_summary_report(self): 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['manual_control_time']:.1f}秒\n") f.write(f"异常捕获次数: {self.stats['exceptions_caught']}\n") f.write(f"前视图像更新次数: {self.stats['front_image_updates']}\n") f.write("=" * 50 + "\n") @@ -938,9 +1311,12 @@ def emergency_stop(self): 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() @@ -962,18 +1338,21 @@ def main(): """主程序入口""" # 显示启动信息 print("=" * 70) - print("AirSimNH 无人机感知探索系统 - 工程优化版") + 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) - # 用户确认 - response = input("按 Enter 键开始运行，或输入 'q' 退出: ") - if response.lower() == 'q': - print("程序退出") - return + # 用户选择模式 + print("\n请选择运行模式:") + print(" 1. 自动探索模式 (AI自主决策)") + print(" 2. 手动控制模式 (键盘控制)") + print(" 3. 混合模式 (先自动探索，后可切换)") + print("=" * 50) + + mode_choice = input("请输入选择 (1/2/3): ").strip() explorer = None try: @@ -989,8 +1368,75 @@ def signal_handler(sig, frame): signal.signal(signal.SIGINT, signal_handler) - # 运行主循环 - explorer.run_perception_loop() + # 根据选择运行相应模式 + if mode_choice == '1': + # 自动探索模式 + print("\n" + "="*50) + print("启动自动探索模式") + 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) # 最多自动探索60秒 + + # 运行自动探索 + explorer.run_perception_loop() + + # 如果自动探索正常结束（非紧急停止） + if not explorer.emergency_flag: + print("\n" + "="*50) + print("自动探索阶段结束") + 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}") From 98557f8e03dfa2427527dded400d6e4728f65801 Mon Sep 17 00:00:00 2001 From: rhcgg <3136924319@qq.com> Date: Sat, 20 Dec 2025 01:20:54 +0800 Subject: [PATCH 08/23] =?UTF-8?q?=E6=B7=BB=E5=8A=A0=E4=BA=86=E4=B8=80?= =?UTF-8?q?=E4=B8=AA=E6=89=8B=E5=8A=A8=E6=8E=A7=E5=88=B6=EF=BC=8C=E9=80=9A?= =?UTF-8?q?=E8=BF=87=E5=89=8D=E8=A7=86=E7=AA=97=E5=8F=A3=E6=8E=A7=E5=88=B6?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit 窗口控制键（通用模式，非手动控制时生效）： Q：关闭窗口 S：保存截图 P：暂停/继续 I：切换信息显示 H：切换锐化效果 2. 手动控制键（只在手动控制模式生效）： W/S：前进/后退 A/D：左移/右移 Q/E：上升/下降 Z/X：左转/右转 空格：悬停 ESC：退出手动模式（不关闭窗口） --- src/drone_perception/config.py | 13 + .../perceptive_drone_explorer.py | 810 ++++++++++++++---- 2 files changed, 641 insertions(+), 182 deletions(-) diff --git a/src/drone_perception/config.py b/src/drone_perception/config.py index e02a8330af..2c20f8cd25 100644 --- a/src/drone_perception/config.py +++ b/src/drone_perception/config.py @@ -34,6 +34,19 @@ } } +# ==================== 手动控制参数 ==================== +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 = { 'WINDOW_WIDTH': 640, # 窗口宽度 diff --git a/src/drone_perception/perceptive_drone_explorer.py b/src/drone_perception/perceptive_drone_explorer.py index 61e783259b..c88534a5e0 100644 --- a/src/drone_perception/perceptive_drone_explorer.py +++ b/src/drone_perception/perceptive_drone_explorer.py @@ -2,7 +2,8 @@ AirSimNH 感知驱动自主探索无人机 - 工程优化版 核心：视觉感知 → 语义理解 → 智能决策 → 安全执行 集成：配置管理、日志系统、异常恢复、前视窗口显示 -版本: 2.0 (工程优化版) +新增：修复按键冲突，改进手动控制 +版本: 2.5 (修复按键冲突改进版) """ import airsim @@ -30,13 +31,12 @@ print(f"❌ 无法加载配置文件 config.py: {e}") print("正在使用默认配置...") CONFIG_LOADED = False - # 临时默认配置 class DefaultConfig: - EXPLORATION = {'TOTAL_TIME': 180, 'PREFERRED_SPEED': 3.0, 'BASE_HEIGHT': -15.0, - 'MAX_ALTITUDE': -30.0, 'MIN_ALTITUDE': -8.0, 'TAKEOFF_HEIGHT': -10.0} + 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': [-45, -30, -15, 0, 15, 30, 45], + '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}} @@ -44,8 +44,19 @@ class DefaultConfig: 'SHOW_INFO_OVERLAY': True, 'REFRESH_RATE_MS': 30} 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': 10} + 'ENABLE_HEALTH_CHECK': True, 'HEALTH_CHECK_INTERVAL': 20} CAMERA = {'DEFAULT_NAME': "0"} + 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 + } config = DefaultConfig() @@ -58,6 +69,7 @@ class FlightState(Enum): RETURNING = "返航中" LANDING = "降落" EMERGENCY = "紧急状态" + MANUAL = "手动控制" @dataclass @@ -65,26 +77,23 @@ 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 # 开阔度评分 (0-1) + 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 # 前视图像 + safe_directions: List[float] = None + front_image: Optional[np.ndarray] = None def __post_init__(self): if self.safe_directions is None: self.safe_directions = [] -# ==================== 前视显示窗口类 ==================== - class FrontViewDisplay: - """前视画面显示管理器""" + """前视画面显示管理器 - 修复按键冲突改进版""" def __init__(self, window_name="无人机前视画面", width=None, height=None, enable_sharpening=None, show_info=None): - # 使用配置参数或传入参数 self.window_name = window_name self.window_width = width if width is not None else config.DISPLAY['WINDOW_WIDTH'] self.window_height = height if height is not None else config.DISPLAY['WINDOW_HEIGHT'] @@ -93,14 +102,21 @@ def __init__(self, window_name="无人机前视画面", width=None, height=None, self.show_info = (show_info if show_info is not None else config.DISPLAY['SHOW_INFO_OVERLAY']) - # 图像队列（线程安全） + # 图像队列 self.image_queue = queue.Queue(maxsize=3) 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, @@ -127,10 +143,12 @@ def start(self): 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): + 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 @@ -153,6 +171,7 @@ def update_image(self, image_data: np.ndarray, info: Optional[Dict] = None): display_packet = { 'image': image_data.copy(), 'info': info.copy() if info else {}, + 'manual_info': manual_info.copy() if manual_info else [], 'timestamp': time.time() } @@ -161,8 +180,24 @@ def update_image(self, image_data: np.ndarray, info: Optional[Dict] = None): 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) @@ -171,16 +206,21 @@ def _display_loop(self): cv2.putText(wait_img, "等待无人机图像...", (50, 150), cv2.FONT_HERSHEY_SIMPLEX, 0.8, (255, 255, 255), 2) cv2.imshow(self.window_name, wait_img) - cv2.waitKey(100) # 关键：给窗口时间初始化 + cv2.waitKey(100) print("💡 前视窗口控制:") - print(" - 按 'Q': 关闭窗口 | 'S': 保存截图") - print(" - 按 'P': 暂停/继续 | 'I': 切换信息显示") - print(" - 按 'H': 切换锐化效果") - - while self.display_active: + 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: # 获取最新图像 @@ -188,13 +228,17 @@ def _display_loop(self): 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(): - self.image_queue.get_nowait() + try: + self.image_queue.get_nowait() + except queue.Empty: + break except queue.Empty: pass @@ -202,7 +246,7 @@ def _display_loop(self): if display_image is not None: # 添加信息叠加 if self.show_info: - display_image = self._add_info_overlay(display_image, info) + display_image = self._add_info_overlay(display_image, info, manual_info) cv2.imshow(self.window_name, display_image) elif self.paused: @@ -212,31 +256,99 @@ def _display_loop(self): cv2.FONT_HERSHEY_SIMPLEX, 1.5, (0, 0, 255), 3) cv2.imshow(self.window_name, blank) - # 键盘事件处理 + # 处理键盘输入（非阻塞） key = cv2.waitKey(config.DISPLAY.get('REFRESH_RATE_MS', 30)) & 0xFF - if key == ord('q') or key == ord('Q'): - print("🔄 用户关闭显示窗口") + # 记录当前按键 + 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 - elif key == ord('s') or key == ord('S'): - self._save_screenshot(display_image) - elif key == ord('p') or key == ord('P'): - self.paused = not self.paused - status = "已暂停" if self.paused else "已恢复" - print(f"⏸️ 视频流{status}") - elif key == ord('i') or key == ord('I'): - self.show_info = not self.show_info - status = "开启" if self.show_info else "关闭" - print(f"📊 信息叠加层{status}") - elif key == ord('h') or key == ord('H'): - self.enable_sharpening = not self.enable_sharpening - status = "开启" if self.enable_sharpening else "关闭" - print(f"🔍 图像锐化{status}") - - cv2.destroyWindow(self.window_name) + + # 清理窗口 + try: + cv2.destroyWindow(self.window_name) + except: + pass cv2.waitKey(1) + def _handle_manual_mode_key(self, key): + """处理手动控制模式下的按键""" + key_char = chr(key).lower() if 0 <= key <= 255 else '' + + # ESC键：退出手动模式 + if key == 27: # ESC + print("收到退出手动模式指令") + self.exit_manual_flag = True + return + + # 记录手动控制按键 + self.key_states[key] = True + + # 特别处理空格键（悬停） + if key == 32: # 空格 + print("⏸️ 悬停指令") + + # 其他手动控制键已经在key_states中记录，会在主循环中处理 + + 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() @@ -247,7 +359,7 @@ def _update_stats(self): self.display_stats['frame_count'] = 0 self.display_stats['last_update'] = now - def _add_info_overlay(self, image: np.ndarray, info: Dict) -> np.ndarray: + def _add_info_overlay(self, image: np.ndarray, info: Dict, manual_info: List[str] = None) -> np.ndarray: """在图像上叠加状态信息""" if image is None or image.size == 0: return image @@ -256,14 +368,19 @@ def _add_info_overlay(self, image: np.ndarray, info: Dict) -> np.ndarray: overlay = image.copy() height, width = image.shape[:2] + # 判断是否为手动模式 + is_manual = info.get('state', '') == "手动控制" + + # 信息栏高度（手动模式需要更多空间显示控制说明） + info_height = 140 if (is_manual and manual_info) else 100 + # 创建半透明信息栏 - info_height = 100 cv2.rectangle(overlay, (0, 0), (width, info_height), (0, 0, 0), -1) cv2.addWeighted(overlay, 0.7, image, 0.3, 0, image) # 飞行状态 state = info.get('state', 'UNKNOWN') - state_color = (0, 255, 0) if '探索' in state else (0, 255, 255) if '悬停' in state else (0, 0, 255) + state_color = (0, 255, 0) if '探索' in state else (0, 255, 255) if '悬停' in state else (255, 255, 0) if '手动' in state else (0, 0, 255) cv2.putText(image, f"状态: {state}", (10, 30), cv2.FONT_HERSHEY_SIMPLEX, 0.7, state_color, 2) @@ -272,22 +389,28 @@ def _add_info_overlay(self, image: np.ndarray, info: Dict) -> np.ndarray: cv2.putText(image, f"位置: ({pos[0]:.1f}, {pos[1]:.1f}, {-pos[2]:.1f}m)", (10, 60), cv2.FONT_HERSHEY_SIMPLEX, 0.6, (255, 255, 255), 1) - # 障碍物信息 - obs_dist = info.get('obstacle_distance', 0.0) - obs_color = (0, 0, 255) if obs_dist < 5.0 else (0, 165, 255) if obs_dist < 10.0 else (0, 255, 0) - cv2.putText(image, f"障碍: {obs_dist:.1f}m", (10, 90), - cv2.FONT_HERSHEY_SIMPLEX, 0.7, obs_color, 2) + # 手动控制信息 + if is_manual and manual_info: + for i, line in enumerate(manual_info): + y_pos = 90 + i * 20 + cv2.putText(image, line, (10, y_pos), + cv2.FONT_HERSHEY_SIMPLEX, 0.5, (200, 255, 200), 1) + + # 显示手动控制提示 + cv2.putText(image, "手动控制中...", (width - 150, 60), + cv2.FONT_HERSHEY_SIMPLEX, 0.6, (255, 255, 0), 1) + elif not is_manual: + # 障碍物信息（如果不是手动模式） + obs_dist = info.get('obstacle_distance', 0.0) + obs_color = (0, 0, 255) if obs_dist < 5.0 else (0, 165, 255) if obs_dist < 10.0 else (0, 255, 0) + cv2.putText(image, f"障碍: {obs_dist:.1f}m", (10, 90), + cv2.FONT_HERSHEY_SIMPLEX, 0.7, obs_color, 2) # 显示统计信息 fps_text = f"FPS: {self.display_stats['fps']:.1f}" cv2.putText(image, fps_text, (width - 120, 30), cv2.FONT_HERSHEY_SIMPLEX, 0.6, (255, 255, 0), 1) - # 清晰度提示（低分辨率时显示） - if height < 200: - cv2.putText(image, "提示: 修改config.py可提高分辨率", (10, height-10), - cv2.FONT_HERSHEY_SIMPLEX, 0.4, (200, 200, 200), 1) - return image except Exception as e: print(f"⚠️ 添加信息叠加层出错: {e}") @@ -304,16 +427,14 @@ def _save_screenshot(self, image: Optional[np.ndarray]): print("⚠️ 无法保存截图：无有效图像数据") -# ==================== 主探索器类 ==================== - class PerceptiveExplorer: - """基于感知的自主探索无人机 - 工程优化版""" + """基于感知的自主探索无人机 - 修复按键冲突改进版""" def __init__(self, drone_name=""): # 初始化日志系统 self._setup_logging() self.logger.info("=" * 60) - self.logger.info("AirSimNH 感知驱动自主探索系统 - 工程优化版 初始化") + self.logger.info("AirSimNH 感知驱动自主探索系统 - 修复按键冲突改进版") self.logger.info("=" * 60) # 初始化AirSim连接 @@ -368,12 +489,17 @@ def __init__(self, drone_name=""): 'obstacles_detected': 0, 'state_changes': 0, 'front_image_updates': 0, + 'manual_control_time': 0.0, } # 前视窗口 self.front_display = None self._setup_front_display() + # 手动控制状态 + 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}秒") @@ -558,7 +684,11 @@ def get_depth_perception(self) -> PerceptionResult: # 更新前视窗口 if self.front_display: - self.front_display.update_image(img_bgr, display_info) + manual_info = None + if self.state == FlightState.MANUAL: + manual_info = self._get_manual_control_info() + + self.front_display.update_image(img_bgr, display_info, manual_info) self.stats['front_image_updates'] += 1 except Exception as e: @@ -600,63 +730,366 @@ def _prepare_display_info(self, perception: PerceptionResult) -> Dict: except: return {} - def _check_connection_health(self): - """检查连接健康状态""" + 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.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: - # 简单的ping测试 - self.client.ping() - self.logger.info("✅ 连接健康检查通过") - return True + # 获取当前无人机状态 + 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'] # AirSim中Z轴向下为正 + 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}") - if self.reconnect_attempts < config.SYSTEM['MAX_RECONNECT_ATTEMPTS']: - self.logger.info("尝试重新连接...") - self._connect_to_airsim() - self.reconnect_attempts += 1 - return False - - def _handle_airsim_exception(self, exception): - """处理AirSim特定异常""" - error_msg = str(exception) - if "timeout" in error_msg.lower(): - self.logger.warning("⏰ AirSim响应超时，等待后继续...") - time.sleep(1.0) - elif "not connected" in error_msg.lower(): - self.logger.error("🔌 与AirSim连接断开") - self._handle_connection_error() + self.logger.warning(f"⚠️ 手动控制应用失败: {e}") - def _handle_connection_error(self): - """处理连接错误""" - if self.emergency_flag: + def change_state(self, new_state: FlightState): + """状态转换""" + if self.state != new_state: + self.logger.info(f"🔄 状态转换: {self.state.value} → {new_state.value}") + self.state = new_state + self.state_history.append((time.time(), new_state)) + self.stats['state_changes'] += 1 + + def run_manual_control(self): + """改进的手动控制模式 - 修复按键冲突""" + self.logger.info("=" * 60) + self.logger.info("启动手动控制模式") + self.logger.info("=" * 60) + + if not self.front_display: + self.logger.error("❌ 前视窗口未初始化") return - self.logger.warning("🔄 尝试恢复连接...") - success = self._check_connection_health() - if not success and not self.emergency_flag: - self.logger.error("❌ 无法恢复连接，启动紧急降落程序") + try: + # 切换到手动控制状态 + self.change_state(FlightState.MANUAL) + self.manual_control_start = time.time() + + # 设置前视窗口为手动模式 + self.front_display.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_display.should_exit_manual(): + self.logger.info("收到退出手动模式指令") + manual_active = False + break + + # 获取前视窗口的按键状态 + if self.front_display: + window_keys = self.front_display.get_control_inputs() + self.control_keys = window_keys.copy() + + # 检查前视窗口是否还在运行 + if not self.front_display.display_active: + self.logger.info("前视窗口已关闭，退出手动模式") + manual_active = False + break + + # 应用手动控制（限制频率） + current_time = time.time() + if current_time - last_control_time >= 0.05: # 20Hz + self.apply_manual_control() + last_control_time = current_time + + # 定期获取并显示图像（降低频率，避免冲突） + if current_time - last_image_time >= 0.1: # 10Hz + 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_display: + manual_info = self._get_manual_control_info() + self.front_display.update_image(img_bgr, display_info, manual_info) + last_image_time = current_time + except Exception as img_error: + # 忽略图像获取错误，继续控制 + 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_display: + self.front_display.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("="*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: # 至少还有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 get_visual_perception(self): - """获取视觉图像用于高级感知（可选）""" + def run_perception_loop(self): + """主感知-决策-控制循环""" + self.logger.info("=" * 60) + self.logger.info("启动感知-决策-控制主循环") + self.logger.info("=" * 60) + try: - responses = self.client.simGetImages([ - airsim.ImageRequest("0", airsim.ImageType.Scene, False, False) - ]) + # 起飞 + 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) - if responses and responses[0]: - img_data = responses[0].image_data_uint8 - img_array = np.frombuffer(img_data, dtype=np.uint8) - img = img_array.reshape(responses[0].height, responses[0].width, 3) + # 主循环 + exploration_start = time.time() - hsv = cv2.cvtColor(img, cv2.COLOR_RGB2HSV) - green_mask = cv2.inRange(hsv, (40, 40, 40), (80, 255, 255)) - green_ratio = np.sum(green_mask > 0) / green_mask.size + while (time.time() - exploration_start < self.exploration_time and + not self.emergency_flag): - return img, green_ratio - except: - pass + self.loop_count += 1 + loop_start = time.time() + + # 1. 感知阶段 + perception = self.get_depth_perception() - return None, 0 + # 2. 决策阶段 + decision = self.make_intelligent_decision(perception) + + # 3. 控制执行阶段 + self._execute_control_decision(decision) + + # 定期状态报告 + if self.loop_count % config.SYSTEM.get('HEALTH_CHECK_INTERVAL', 20) == 0: + self._report_status(exploration_start, perception) + + # 循环频率控制 + 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 make_intelligent_decision(self, perception: PerceptionResult) -> Tuple[float, float, float, float]: """基于感知结果做出智能决策""" @@ -733,14 +1166,6 @@ def make_intelligent_decision(self, perception: PerceptionResult) -> Tuple[float self.logger.error(f"❌ 决策过程异常: {e}") return 0.0, 0.0, self.base_height, 0.0 - def change_state(self, new_state: FlightState): - """状态转换""" - if self.state != new_state: - self.logger.info(f"🔄 状态转换: {self.state.value} → {new_state.value}") - self.state = new_state - self.state_history.append((time.time(), new_state)) - self.stats['state_changes'] += 1 - def _execute_control_decision(self, decision): """执行控制决策，增强异常处理""" try: @@ -785,65 +1210,11 @@ def _report_status(self, exploration_start, perception): f"| 开阔度={perception.open_space_score:.2f}") self.logger.info(f" 系统统计: 异常{self.stats['exceptions_caught']}次 " f"| 状态切换{self.stats['state_changes']}次") + if self.stats['manual_control_time'] > 0: + self.logger.info(f" 手动控制: {self.stats['manual_control_time']:.1f}秒") except: self.logger.info("状态报告: 无法获取无人机状态") - 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() - - # 1. 感知阶段 - perception = self.get_depth_perception() - - # 2. 决策阶段 - decision = self.make_intelligent_decision(perception) - - # 3. 控制执行阶段 - self._execute_control_decision(decision) - - # 定期状态报告 - if self.loop_count % config.SYSTEM.get('HEALTH_CHECK_INTERVAL', 20) == 0: - self._report_status(exploration_start, perception) - - # 循环频率控制 - 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 _finish_mission(self): """完成任务并生成总结报告""" self.logger.info("=" * 60) @@ -905,6 +1276,7 @@ def _generate_summary_report(self): self.logger.info(f" 状态切换次数: {self.stats['state_changes']}") self.logger.info(f" 检测到障碍次数: {self.stats['obstacles_detected']}") self.logger.info(f" 前视图像更新次数: {self.stats['front_image_updates']}") + 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}") @@ -919,6 +1291,7 @@ def _generate_summary_report(self): 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['manual_control_time']:.1f}秒\n") f.write(f"异常捕获次数: {self.stats['exceptions_caught']}\n") f.write(f"前视图像更新次数: {self.stats['front_image_updates']}\n") f.write("=" * 50 + "\n") @@ -938,9 +1311,12 @@ def emergency_stop(self): 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() @@ -962,18 +1338,21 @@ def main(): """主程序入口""" # 显示启动信息 print("=" * 70) - print("AirSimNH 无人机感知探索系统 - 工程优化版") + 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) - # 用户确认 - response = input("按 Enter 键开始运行，或输入 'q' 退出: ") - if response.lower() == 'q': - print("程序退出") - return + # 用户选择模式 + print("\n请选择运行模式:") + print(" 1. 自动探索模式 (AI自主决策)") + print(" 2. 手动控制模式 (键盘控制)") + print(" 3. 混合模式 (先自动探索，后可切换)") + print("=" * 50) + + mode_choice = input("请输入选择 (1/2/3): ").strip() explorer = None try: @@ -989,8 +1368,75 @@ def signal_handler(sig, frame): signal.signal(signal.SIGINT, signal_handler) - # 运行主循环 - explorer.run_perception_loop() + # 根据选择运行相应模式 + if mode_choice == '1': + # 自动探索模式 + print("\n" + "="*50) + print("启动自动探索模式") + 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) # 最多自动探索60秒 + + # 运行自动探索 + explorer.run_perception_loop() + + # 如果自动探索正常结束（非紧急停止） + if not explorer.emergency_flag: + print("\n" + "="*50) + print("自动探索阶段结束") + 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}") From 175c234c1c5086f11f2e5c891d1089d2392adaae Mon Sep 17 00:00:00 2001 From: rhcgg <3136924319@qq.com> Date: Sat, 20 Dec 2025 21:36:10 +0800 Subject: [PATCH 09/23] =?UTF-8?q?=E6=99=BA=E8=83=BD=E5=86=B3=E7=AD=96?= =?UTF-8?q?=E7=AE=97=E6=B3=95=E5=A2=9E=E5=BC=BA?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit 重点：改进避障（向量场算法），优化探索策略（基于网格信息增益），平滑飞行控制。 --- src/drone_perception/config.py | 58 +- .../perceptive_drone_explorer.py | 877 ++++++++++++++++-- 2 files changed, 833 insertions(+), 102 deletions(-) diff --git a/src/drone_perception/config.py b/src/drone_perception/config.py index 2c20f8cd25..a6ec61ff74 100644 --- a/src/drone_perception/config.py +++ b/src/drone_perception/config.py @@ -4,6 +4,8 @@ 所有可调参数集中在此管理 """ +import math + # ==================== 飞行与探索参数 ==================== EXPLORATION = { 'TOTAL_TIME': 120, # 总探索时间 (秒)，建议120-180 @@ -34,6 +36,42 @@ } } +# ==================== 智能决策参数 ==================== +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, # 目标到达判定距离 (米) +} + # ==================== 手动控制参数 ==================== MANUAL = { 'CONTROL_SPEED': 3.0, # 水平移动速度 (米/秒) @@ -41,7 +79,7 @@ 'YAW_SPEED': 45.0, # 偏航角速度 (度/秒) 'ENABLE_AUTO_HOVER': True, # 松开按键时自动悬停 'DISPLAY_CONTROLS': True, # 在画面显示控制说明 - 'SAFETY_ENABLED': True, # 启用安全限制（高度、速度限制） + 'SAFETY_ENABLED': True, # 启用安全限制 (高度、速度限制) 'MAX_MANUAL_SPEED': 5.0, # 最大手动控制速度 'MIN_ALTITUDE_LIMIT': -5.0, # 最低飞行高度限制 'MAX_ALTITUDE_LIMIT': -30.0, # 最高飞行高度限制 @@ -49,11 +87,12 @@ # ==================== 前视窗口参数 ==================== DISPLAY = { - 'WINDOW_WIDTH': 640, # 窗口宽度 - 'WINDOW_HEIGHT': 480, # 窗口高度 + 'WINDOW_WIDTH': 640, # 窗口宽度 (像素) + 'WINDOW_HEIGHT': 480, # 窗口高度 (像素) 'ENABLE_SHARPENING': True, # 启用图像锐化，改善模糊 'SHOW_INFO_OVERLAY': True, # 显示信息叠加层 'REFRESH_RATE_MS': 30, # 刷新率 (毫秒)，建议30-50 + 'GRID_DISPLAY_SIZE': 150, # 探索网格显示大小 (像素) } # ==================== 系统与安全参数 ==================== @@ -67,6 +106,8 @@ 'ENABLE_HEALTH_CHECK': True, # 启用健康检查 'HEALTH_CHECK_INTERVAL': 20, # 健康检查间隔 (循环次数) + + 'EMERGENCY_RESPONSE_TIME': 10.0, # 紧急响应超时时间 (秒) } # ==================== 相机配置 ==================== @@ -74,12 +115,19 @@ CAMERA = { 'DEFAULT_NAME': "0", # 默认相机名称 # 如果"0"无效，可以尝试以下名称: - # 'POSSIBLE_NAMES': ["0", "1", "front_center", "front", "CameraActor_0"] + 'POSSIBLE_NAMES': ["0", "1", "front_center", "front", "CameraActor_0"], + + # 深度相机参数 + 'DEPTH_FOV_DEG': 90, # 深度相机视野 (度) + 'MAX_DEPTH_RANGE': 100.0, # 最大深度范围 (米) } # ==================== 调试参数 ==================== DEBUG = { 'SAVE_PERCEPTION_IMAGES': False, # 是否保存感知图像用于调试 'IMAGE_SAVE_INTERVAL': 50, # 图像保存间隔 (循环次数) - 'LOG_DECISION_DETAILS': False, # 是否记录详细决策信息 + 'LOG_DECISION_DETAILS': True, # 是否记录详细决策信息 + 'SAVE_GRID_VISUALIZATION': True, # 是否保存网格可视化 + 'LOG_VECTOR_FIELD': False, # 是否记录向量场详细信息 + 'PERFORMANCE_PROFILING': False, # 是否启用性能分析 } \ No newline at end of file diff --git a/src/drone_perception/perceptive_drone_explorer.py b/src/drone_perception/perceptive_drone_explorer.py index c88534a5e0..a099153357 100644 --- a/src/drone_perception/perceptive_drone_explorer.py +++ b/src/drone_perception/perceptive_drone_explorer.py @@ -1,9 +1,9 @@ """ -AirSimNH 感知驱动自主探索无人机 - 工程优化版 +AirSimNH 感知驱动自主探索无人机 - 智能决策增强版（修复版） 核心：视觉感知 → 语义理解 → 智能决策 → 安全执行 集成：配置管理、日志系统、异常恢复、前视窗口显示 -新增：修复按键冲突，改进手动控制 -版本: 2.5 (修复按键冲突改进版) +新增：向量场避障算法、基于网格的信息增益探索、平滑飞行控制 +版本: 3.1 (修复配置和健康检查问题) """ import airsim @@ -18,10 +18,11 @@ import queue import signal import sys -from typing import Tuple, List, Optional, Dict +from typing import Tuple, List, Optional, Dict, Set import traceback import logging from datetime import datetime +import random # ============ 导入配置文件 ============ try: @@ -57,6 +58,40 @@ class DefaultConfig: '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, # 目标到达判定距离（米） + } + DEBUG = { + 'SAVE_PERCEPTION_IMAGES': False, + 'IMAGE_SAVE_INTERVAL': 50, + 'LOG_DECISION_DETAILS': False + } config = DefaultConfig() @@ -70,6 +105,268 @@ class FlightState(Enum): LANDING = "降落" EMERGENCY = "紧急状态" MANUAL = "手动控制" + PLANNING = "路径规划" + + +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 + + # 初始化网格：0=未知，1=已探索，0.x=部分探索 + 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.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 get_best_exploration_target(self, current_pos): + """获取最佳探索目标""" + if not self.frontier_cells: + # 如果没有前沿，返回随机方向 + angle = random.uniform(0, 2 * math.pi) + distance = 10.0 # 10米外 + 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) # 1分钟内衰减 + + # 综合得分 + score = ( + config.INTELLIGENT_DECISION['CURIOUSITY_WEIGHT'] * info_gain + + (1 - config.INTELLIGENT_DECISION['MEMORY_WEIGHT'] * time_factor) - + distance_cost * 0.3 + ) + + 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.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 @dataclass @@ -83,14 +380,134 @@ class PerceptionResult: 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 # 新增：障碍物位置列表 def __post_init__(self): if self.safe_directions is None: self.safe_directions = [] + if self.obstacle_positions is None: + self.obstacle_positions = [] + + +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.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): + """计算合成向量""" + # 目标吸引力 + attraction_vector = self._compute_attraction(current_pos, goal_pos) + + # 障碍物排斥力 + repulsion_vector = self._compute_repulsion(current_pos, obstacles) + + # 合成向量 + combined_vector = attraction_vector + repulsion_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 _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) class FrontViewDisplay: - """前视画面显示管理器 - 修复按键冲突改进版""" + """前视画面显示管理器""" def __init__(self, window_name="无人机前视画面", width=None, height=None, enable_sharpening=None, show_info=None): @@ -110,8 +527,8 @@ def __init__(self, window_name="无人机前视画面", width=None, height=None, # 手动控制状态 self.manual_mode = False - self.key_states = {} # 当前按下的键 - self.last_keys = {} # 上次按下的键（用于检测释放） + self.key_states = {} + self.last_keys = {} # 控制退出标志 self.exit_manual_flag = False @@ -148,7 +565,8 @@ def stop(self): 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): + manual_info: Optional[List[str]] = None, + additional_images: Optional[Dict] = None): """更新要显示的图像""" if not self.display_active or self.paused or image_data is None: return @@ -172,6 +590,7 @@ def update_image(self, image_data: np.ndarray, info: Optional[Dict] = None, 'image': image_data.copy(), 'info': info.copy() if info else {}, 'manual_info': manual_info.copy() if manual_info else [], + 'additional_images': additional_images.copy() if additional_images else {}, 'timestamp': time.time() } @@ -197,7 +616,7 @@ 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) @@ -221,6 +640,7 @@ def _display_loop(self): display_image = None info = {} manual_info = [] + additional_images = {} try: # 获取最新图像 @@ -229,6 +649,7 @@ def _display_loop(self): display_image = packet['image'] info = packet['info'] manual_info = packet['manual_info'] + additional_images = packet.get('additional_images', {}) # 更新统计 self._update_stats() @@ -246,7 +667,7 @@ def _display_loop(self): if display_image is not None: # 添加信息叠加 if self.show_info: - display_image = self._add_info_overlay(display_image, info, manual_info) + display_image = self._add_info_overlay(display_image, info, manual_info, additional_images) cv2.imshow(self.window_name, display_image) elif self.paused: @@ -266,13 +687,11 @@ def _display_loop(self): # 根据模式处理按键 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) # 检查窗口是否被关闭 @@ -282,7 +701,6 @@ def _display_loop(self): self.display_active = False break except: - # 窗口可能已被销毁 self.display_active = False break @@ -310,8 +728,6 @@ def _handle_manual_mode_key(self, key): if key == 32: # 空格 print("⏸️ 悬停指令") - # 其他手动控制键已经在key_states中记录，会在主循环中处理 - def _handle_window_control_key(self, key, display_image): """处理通用窗口控制按键""" key_char = chr(key).lower() if 0 <= key <= 255 else '' @@ -359,7 +775,8 @@ def _update_stats(self): 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: + def _add_info_overlay(self, image: np.ndarray, info: Dict, manual_info: List[str] = None, + additional_images: Dict = None) -> np.ndarray: """在图像上叠加状态信息""" if image is None or image.size == 0: return image @@ -371,8 +788,9 @@ def _add_info_overlay(self, image: np.ndarray, info: Dict, manual_info: List[str # 判断是否为手动模式 is_manual = info.get('state', '') == "手动控制" - # 信息栏高度（手动模式需要更多空间显示控制说明） - info_height = 140 if (is_manual and manual_info) else 100 + # 如果有附加图像（如网格图），调整信息栏高度 + grid_img = additional_images.get('grid') if additional_images else None + info_height = 180 if (is_manual and manual_info) or grid_img is not None else 100 # 创建半透明信息栏 cv2.rectangle(overlay, (0, 0), (width, info_height), (0, 0, 0), -1) @@ -389,18 +807,32 @@ def _add_info_overlay(self, image: np.ndarray, info: Dict, manual_info: List[str cv2.putText(image, f"位置: ({pos[0]:.1f}, {pos[1]:.1f}, {-pos[2]:.1f}m)", (10, 60), cv2.FONT_HERSHEY_SIMPLEX, 0.6, (255, 255, 255), 1) + # 智能决策信息 + decision_info = info.get('decision_info', {}) + if decision_info: + y_pos = 90 + for key, value in decision_info.items(): + if key == 'vector_angle': + cv2.putText(image, f"方向: {math.degrees(value):.0f}°", (10, y_pos), + cv2.FONT_HERSHEY_SIMPLEX, 0.5, (200, 255, 200), 1) + y_pos += 20 + elif key == 'grid_score': + cv2.putText(image, f"探索得分: {value:.2f}", (10, y_pos), + cv2.FONT_HERSHEY_SIMPLEX, 0.5, (200, 200, 255), 1) + y_pos += 20 + # 手动控制信息 if is_manual and manual_info: + y_start = 130 for i, line in enumerate(manual_info): - y_pos = 90 + i * 20 + y_pos = y_start + i * 20 cv2.putText(image, line, (10, y_pos), cv2.FONT_HERSHEY_SIMPLEX, 0.5, (200, 255, 200), 1) - # 显示手动控制提示 cv2.putText(image, "手动控制中...", (width - 150, 60), cv2.FONT_HERSHEY_SIMPLEX, 0.6, (255, 255, 0), 1) elif not is_manual: - # 障碍物信息（如果不是手动模式） + # 障碍物信息 obs_dist = info.get('obstacle_distance', 0.0) obs_color = (0, 0, 255) if obs_dist < 5.0 else (0, 165, 255) if obs_dist < 10.0 else (0, 255, 0) cv2.putText(image, f"障碍: {obs_dist:.1f}m", (10, 90), @@ -411,6 +843,27 @@ def _add_info_overlay(self, image: np.ndarray, info: Dict, manual_info: List[str cv2.putText(image, fps_text, (width - 120, 30), cv2.FONT_HERSHEY_SIMPLEX, 0.6, (255, 255, 0), 1) + # 如果有网格图，显示在右上角 + if grid_img is not None and grid_img.size > 0: + grid_size = 150 + grid_resized = cv2.resize(grid_img, (grid_size, grid_size)) + + # 在图像右上角放置网格图 + x_offset = width - grid_size - 10 + y_offset = info_height + 10 + + if y_offset + grid_size < height: + # 创建网格图的背景 + cv2.rectangle(image, (x_offset-2, y_offset-2), + (x_offset+grid_size+2, y_offset+grid_size+2), + (255, 255, 255), 1) + + # 将网格图放入指定位置 + image[y_offset:y_offset+grid_size, x_offset:x_offset+grid_size] = grid_resized + + cv2.putText(image, "探索网格", (x_offset, y_offset-5), + cv2.FONT_HERSHEY_SIMPLEX, 0.5, (255, 255, 255), 1) + return image except Exception as e: print(f"⚠️ 添加信息叠加层出错: {e}") @@ -428,13 +881,13 @@ def _save_screenshot(self, image: Optional[np.ndarray]): class PerceptiveExplorer: - """基于感知的自主探索无人机 - 修复按键冲突改进版""" + """基于感知的自主探索无人机 - 智能决策增强版（修复版）""" def __init__(self, drone_name=""): # 初始化日志系统 self._setup_logging() self.logger.info("=" * 60) - self.logger.info("AirSimNH 感知驱动自主探索系统 - 修复按键冲突改进版") + self.logger.info("AirSimNH 感知驱动自主探索系统 - 智能决策增强版（修复版）") self.logger.info("=" * 60) # 初始化AirSim连接 @@ -471,6 +924,28 @@ def __init__(self, drone_name=""): 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'] + ) + + # PID控制器 + 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.visited_positions = deque(maxlen=100) @@ -490,6 +965,8 @@ def __init__(self, drone_name=""): 'state_changes': 0, 'front_image_updates': 0, 'manual_control_time': 0.0, + 'vector_field_updates': 0, + 'grid_updates': 0, } # 前视窗口 @@ -503,7 +980,7 @@ def __init__(self, drone_name=""): self.logger.info("✅ 系统初始化完成") self.logger.info(f" 开始时间: {datetime.now().strftime('%H:%M:%S')}") self.logger.info(f" 预计探索时长: {self.exploration_time}秒") - self.logger.info(f" 日志级别: {config.SYSTEM['LOG_LEVEL']}") + self.logger.info(f" 智能决策: 向量场避障 + 网格探索") def _setup_logging(self): """配置日志系统""" @@ -555,6 +1032,22 @@ def _connect_to_airsim(self): 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_front_display(self): """初始化前视显示窗口""" try: @@ -571,12 +1064,12 @@ def _setup_front_display(self): self.front_display = None 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: @@ -633,6 +1126,9 @@ def get_depth_perception(self) -> PerceptionResult: 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: @@ -682,13 +1178,20 @@ def get_depth_perception(self) -> PerceptionResult: # 准备显示信息 display_info = self._prepare_display_info(result) + # 更新探索网格 + self._update_exploration_grid(result) + # 更新前视窗口 if self.front_display: manual_info = None if self.state == FlightState.MANUAL: manual_info = self._get_manual_control_info() - self.front_display.update_image(img_bgr, display_info, manual_info) + # 获取网格可视化图像 + grid_img = self.exploration_grid.visualize_grid(size=150) + additional_images = {'grid': grid_img} if grid_img is not None else {} + + self.front_display.update_image(img_bgr, display_info, manual_info, additional_images) self.stats['front_image_updates'] += 1 except Exception as e: @@ -697,36 +1200,108 @@ def get_depth_perception(self) -> PerceptionResult: self.last_successful_loop = time.time() # 详细日志 - if self.loop_count % 50 == 0: + 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}") - - except airsim.ClientException as e: - self.logger.error(f"❌ AirSim客户端异常: {e}") - self.stats['exceptions_caught'] += 1 - self._handle_airsim_exception(e) - except ConnectionError as e: - self.logger.error(f"❌ 网络连接异常: {e}") - self.stats['exceptions_caught'] += 1 - self._handle_connection_error() - except Exception as e: - self.logger.error(f"❌ 感知过程中发生未知异常: {e}") - self.logger.debug(f"异常详情: {traceback.format_exc()}") - self.stats['exceptions_caught'] += 1 + f"开阔度={result.open_space_score:.2f}, 障碍物数={len(result.obstacle_positions)}") + + 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 + # 尝试重新连接 + self._check_connection_health() + else: + self.logger.error(f"❌ 感知过程中发生未知异常: {e}") + self.logger.debug(f"异常详情: {traceback.format_exc()}") + self.stats['exceptions_caught'] += 1 return result + 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] + + # 将像素坐标转换为相对于相机的3D坐标 + # 简化模型：假设相机水平视角为90度 + 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) + + 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 - return { + + 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, } + + # 添加决策信息 + if hasattr(self, 'last_decision_info'): + info['decision_info'] = self.last_decision_info + + return info except: return {} @@ -864,7 +1439,7 @@ def change_state(self, new_state: FlightState): self.stats['state_changes'] += 1 def run_manual_control(self): - """改进的手动控制模式 - 修复按键冲突""" + """手动控制模式""" self.logger.info("=" * 60) self.logger.info("启动手动控制模式") self.logger.info("=" * 60) @@ -927,10 +1502,9 @@ def run_manual_control(self): self.apply_manual_control() last_control_time = current_time - # 定期获取并显示图像（降低频率，避免冲突） + # 定期获取并显示图像 if current_time - last_image_time >= 0.1: # 10Hz try: - # 只获取前视图像，避免深度图像获取冲突 camera_name = config.CAMERA['DEFAULT_NAME'] responses = self.client.simGetImages([ airsim.ImageRequest( @@ -965,7 +1539,6 @@ def run_manual_control(self): self.front_display.update_image(img_bgr, display_info, manual_info) last_image_time = current_time except Exception as img_error: - # 忽略图像获取错误，继续控制 pass # 短暂休眠 @@ -1015,9 +1588,8 @@ def run_manual_control(self): if choice == '1': self.logger.info("🔄 返回自动探索模式") - # 继续自动探索 remaining_time = self.exploration_time - (time.time() - self.start_time) - if remaining_time > 10: # 至少还有10秒才继续 + if remaining_time > 10: self.exploration_time = remaining_time self.run_perception_loop() else: @@ -1064,7 +1636,7 @@ def run_perception_loop(self): # 1. 感知阶段 perception = self.get_depth_perception() - # 2. 决策阶段 + # 2. 智能决策阶段 decision = self.make_intelligent_decision(perception) # 3. 控制执行阶段 @@ -1092,7 +1664,7 @@ def run_perception_loop(self): self.emergency_stop() def make_intelligent_decision(self, perception: PerceptionResult) -> Tuple[float, float, float, float]: - """基于感知结果做出智能决策""" + """基于感知结果做出智能决策 - 增强版（修复配置键名）""" self.stats['decision_cycles'] += 1 try: @@ -1108,55 +1680,117 @@ def make_intelligent_decision(self, perception: PerceptionResult) -> Tuple[float self.change_state(FlightState.HOVERING) elif self.state == FlightState.HOVERING: + # 扫描环境，选择探索目标 target_yaw = (time.time() % 10) * 0.2 - if len(perception.safe_directions) > 0: + # 检查是否需要更新探索目标 + 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)) + 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, -vel.y_val + # 紧急避障：后退 + target_vx, target_vy = -vel.x_val * 2, -vel.y_val * 2 else: - if perception.safe_directions: - current_yaw = airsim.to_eularian_angles( - state.kinematics_estimated.orientation - )[2] - - diverse_dirs = [ - d for d in perception.safe_directions - if abs(d - current_yaw) > math.radians(45) - ] - - if diverse_dirs: - best_dir = diverse_dirs[0] - else: - best_dir = perception.safe_directions[0] - - speed_factor = min(1.0, perception.open_space_score * 1.5) - target_vx = self.preferred_speed * speed_factor * math.cos(best_dir) - target_vy = self.preferred_speed * speed_factor * math.sin(best_dir) - else: - target_z = pos.z_val - 5 - self.change_state(FlightState.AVOIDING) + # 使用向量场算法计算最佳方向 + 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) + self.target_update_time = time.time() + + # 计算向量场 + vector = self.vector_planner.compute_vector( + current_pos, + self.exploration_target, + perception.obstacle_positions + ) + + # 自适应速度调整 + speed_factor = self._calculate_adaptive_speed(perception, vector.magnitude()) + + # 应用PID控制平滑速度 + 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 + } + + # 检查是否到达目标附近 + 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: - if abs(perception.obstacle_direction) < math.radians(30): - target_z = pos.z_val - 3 - target_vx, target_vy = 0, 0 - else: - avoid_dir = perception.obstacle_direction + math.pi - target_vx = 1.5 * math.cos(avoid_dir) - target_vy = 1.5 * math.sin(avoid_dir) + # 使用向量场进行避障 + current_pos = (pos.x_val, pos.y_val) + + # 计算远离障碍物的方向 + avoid_vector = self.vector_planner.compute_vector( + current_pos, + None, # 没有目标，只有排斥力 + perception.obstacle_positions + ) + + 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.HOVERING) + # 障碍物清除，回到探索状态 + self.change_state(FlightState.EXPLORING) time.sleep(1) 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 + + # 高度PID控制 + 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)) @@ -1166,14 +1800,45 @@ def make_intelligent_decision(self, perception: PerceptionResult) -> Tuple[float self.logger.error(f"❌ 决策过程异常: {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) + + # 综合速度因子 + speed_factor = open_factor * obs_factor * vector_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]: + if self.state in [FlightState.EXPLORING, FlightState.AVOIDING, FlightState.PLANNING]: + # 使用平滑的速度控制 self.client.moveByVelocityZAsync( - target_vx, target_vy, target_z, 0.5, + 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: @@ -1208,6 +1873,9 @@ def _report_status(self, exploration_start, perception): 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" 智能决策: 向量场{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']}次") if self.stats['manual_control_time'] > 0: @@ -1275,6 +1943,9 @@ def _generate_summary_report(self): 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['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['manual_control_time']:.1f}秒") self.logger.info(f" 捕获的异常数: {self.stats['exceptions_caught']}") @@ -1284,19 +1955,26 @@ def _generate_summary_report(self): 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("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['manual_control_time']:.1f}秒\n") f.write(f"异常捕获次数: {self.stats['exceptions_caught']}\n") f.write(f"前视图像更新次数: {self.stats['front_image_updates']}\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]): # 只记录前20个航点 + 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") @@ -1338,16 +2016,22 @@ def main(): """主程序入口""" # 显示启动信息 print("=" * 70) - print("AirSimNH 无人机感知探索系统 - 修复按键冲突改进版") + 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("=" * 70) # 用户选择模式 print("\n请选择运行模式:") - print(" 1. 自动探索模式 (AI自主决策)") + print(" 1. 智能探索模式 (AI自主决策)") print(" 2. 手动控制模式 (键盘控制)") print(" 3. 混合模式 (先自动探索，后可切换)") print("=" * 50) @@ -1372,7 +2056,7 @@ def signal_handler(sig, frame): if mode_choice == '1': # 自动探索模式 print("\n" + "="*50) - print("启动自动探索模式") + print("启动智能探索模式") print("="*50) explorer.run_perception_loop() @@ -1401,21 +2085,20 @@ def signal_handler(sig, frame): print("="*50) # 先运行一段时间的自动探索 - explorer.logger.info("🔍 开始自动探索...") + explorer.logger.info("🔍 开始智能探索...") original_time = config.EXPLORATION['TOTAL_TIME'] - # 设置较短的自动探索时间，然后询问 - explorer.exploration_time = min(60, original_time) # 最多自动探索60秒 + explorer.exploration_time = min(60, original_time) # 运行自动探索 explorer.run_perception_loop() - # 如果自动探索正常结束（非紧急停止） + # 如果自动探索正常结束 if not explorer.emergency_flag: print("\n" + "="*50) - print("自动探索阶段结束") + print("智能探索阶段结束") print("请选择下一步:") print(" 1. 进入手动控制模式") - print(" 2. 继续自动探索") + print(" 2. 继续智能探索") print(" 3. 结束任务返航") print("="*50) From 9318dd166cdd0e38876212e0940d1059c5191153 Mon Sep 17 00:00:00 2001 From: rhcgg <3136924319@qq.com> Date: Sun, 21 Dec 2025 16:11:34 +0800 Subject: [PATCH 10/23] =?UTF-8?q?=E6=80=A7=E8=83=BD=E7=9B=91=E6=8E=A7?= =?UTF-8?q?=E4=B8=8E=E6=95=B0=E6=8D=AE=E9=97=AD=E7=8E=AF?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit 新增 数据记录系统 (DataLogger类)，性能监控系统， 数据闭环功能 --- src/drone_perception/config.py | 22 + .../perceptive_drone_explorer.py | 569 +++++++++++++++++- 2 files changed, 585 insertions(+), 6 deletions(-) diff --git a/src/drone_perception/config.py b/src/drone_perception/config.py index a6ec61ff74..7f9bd9647c 100644 --- a/src/drone_perception/config.py +++ b/src/drone_perception/config.py @@ -130,4 +130,26 @@ 'SAVE_GRID_VISUALIZATION': True, # 是否保存网格可视化 'LOG_VECTOR_FIELD': False, # 是否记录向量场详细信息 'PERFORMANCE_PROFILING': 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, # 系统指标记录间隔（秒） +} + +# ==================== 性能监控参数 ==================== +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, # 性能报告间隔（秒） } \ No newline at end of file diff --git a/src/drone_perception/perceptive_drone_explorer.py b/src/drone_perception/perceptive_drone_explorer.py index a099153357..84d958fc41 100644 --- a/src/drone_perception/perceptive_drone_explorer.py +++ b/src/drone_perception/perceptive_drone_explorer.py @@ -3,7 +3,8 @@ 核心：视觉感知 → 语义理解 → 智能决策 → 安全执行 集成：配置管理、日志系统、异常恢复、前视窗口显示 新增：向量场避障算法、基于网格的信息增益探索、平滑飞行控制 -版本: 3.1 (修复配置和健康检查问题) +新增：性能监控与数据闭环系统 +版本: 3.2 (完整版) """ import airsim @@ -11,6 +12,8 @@ import numpy as np import cv2 import math +import json +import csv from collections import deque from dataclasses import dataclass from enum import Enum @@ -18,11 +21,13 @@ import queue import signal import sys -from typing import Tuple, List, Optional, Dict, Set +from typing import Tuple, List, Optional, Dict, Set, Any import traceback import logging from datetime import datetime import random +import psutil +import os # ============ 导入配置文件 ============ try: @@ -92,6 +97,26 @@ class DefaultConfig: 'IMAGE_SAVE_INTERVAL': 50, 'LOG_DECISION_DETAILS': 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, # 系统指标记录间隔 + } + # 新增：性能监控参数 + 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, # 性能报告间隔（秒） + } config = DefaultConfig() @@ -369,6 +394,279 @@ def visualize_grid(self, size=300): 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 + } + + # 定义CSV列名 + 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' + ] + + # 初始化CSV文件 + 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): + """初始化CSV文件，写入列名""" + 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() + + # 记录到CSV + 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) + + # 记录到JSON缓冲区 + if self.enable_json: + self.json_data['flight_data'].append(data_dict) + + # 更新性能指标 + self.performance_metrics['data_points'] += 1 + + # 收集系统性能数据 + if self.performance_metrics['data_points'] % 10 == 0: + self._collect_system_metrics() + + except Exception as e: + print(f"⚠️ 记录飞行数据时出错: {e}") + + def _collect_system_metrics(self): + """收集系统性能指标""" + try: + # CPU使用率 + 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 = 1000 + 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 = 1000 + 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'] = [] + + self.json_data['events'].append(event_record) + + except Exception as e: + print(f"⚠️ 记录事件时出错: {e}") + + def save_json_data(self): + """保存JSON数据到文件""" + if not self.enable_json: + return + + try: + # 计算性能统计 + self._calculate_performance_stats() + + # 保存到文件 + 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统计 + 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 + + # 添加到JSON数据 + 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" + + 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: """感知结果数据结构""" @@ -541,6 +839,13 @@ def __init__(self, window_name="无人机前视画面", width=None, height=None, 'frame_count': 0 } + # 性能监控显示 + self.performance_info = { + 'cpu_usage': 0.0, + 'memory_usage': 0.0, + 'update_time': time.time() + } + # 启动显示线程 self.start() @@ -599,6 +904,12 @@ def update_image(self, image_data: np.ndarray, info: Optional[Dict] = None, except Exception as e: print(f"⚠️ 更新图像时出错: {e}") + def update_performance_info(self, cpu_usage, memory_usage): + """更新性能信息""" + self.performance_info['cpu_usage'] = cpu_usage + self.performance_info['memory_usage'] = memory_usage + self.performance_info['update_time'] = time.time() + def set_manual_mode(self, manual_mode): """设置手动模式状态""" self.manual_mode = manual_mode @@ -843,6 +1154,19 @@ def _add_info_overlay(self, image: np.ndarray, info: Dict, manual_info: List[str cv2.putText(image, fps_text, (width - 120, 30), cv2.FONT_HERSHEY_SIMPLEX, 0.6, (255, 255, 0), 1) + # 性能信息 + if self.performance_info['cpu_usage'] > 0: + cpu_text = f"CPU: {self.performance_info['cpu_usage']:.1f}%" + mem_text = f"MEM: {self.performance_info['memory_usage']:.1f}%" + + cpu_color = (0, 200, 255) if self.performance_info['cpu_usage'] > 80 else (0, 255, 0) + mem_color = (0, 200, 255) if self.performance_info['memory_usage'] > 80 else (0, 255, 0) + + cv2.putText(image, cpu_text, (width - 120, 60), + cv2.FONT_HERSHEY_SIMPLEX, 0.5, cpu_color, 1) + cv2.putText(image, mem_text, (width - 120, 80), + cv2.FONT_HERSHEY_SIMPLEX, 0.5, mem_color, 1) + # 如果有网格图，显示在右上角 if grid_img is not None and grid_img.size > 0: grid_size = 150 @@ -956,6 +1280,17 @@ def __init__(self, drone_name=""): 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, @@ -967,6 +1302,10 @@ def __init__(self, drone_name=""): '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, } # 前视窗口 @@ -981,6 +1320,8 @@ def __init__(self, drone_name=""): self.logger.info(f" 开始时间: {datetime.now().strftime('%H:%M:%S')}") self.logger.info(f" 预计探索时长: {self.exploration_time}秒") self.logger.info(f" 智能决策: 向量场避障 + 网格探索") + if config.DATA_RECORDING['ENABLED']: + self.logger.info(f" 数据记录: CSV + JSON 格式") def _setup_logging(self): """配置日志系统""" @@ -1008,6 +1349,20 @@ def _setup_logging(self): 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): """连接到AirSim，支持重试机制""" max_attempts = config.SYSTEM['MAX_RECONNECT_ATTEMPTS'] @@ -1181,6 +1536,9 @@ def get_depth_perception(self) -> PerceptionResult: # 更新探索网格 self._update_exploration_grid(result) + # 记录数据 + self._record_flight_data(result) + # 更新前视窗口 if self.front_display: manual_info = None @@ -1191,6 +1549,12 @@ def get_depth_perception(self) -> PerceptionResult: grid_img = self.exploration_grid.visualize_grid(size=150) additional_images = {'grid': grid_img} if grid_img is not None else {} + # 更新性能信息 + if config.PERFORMANCE['ENABLE_REALTIME_METRICS']: + cpu_usage = psutil.cpu_percent(interval=0) + memory_usage = psutil.virtual_memory().percent + self.front_display.update_performance_info(cpu_usage, memory_usage) + self.front_display.update_image(img_bgr, display_info, manual_info, additional_images) self.stats['front_image_updates'] += 1 @@ -1208,15 +1572,81 @@ def get_depth_perception(self) -> PerceptionResult: 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 + + # 构建数据字典 + 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, + } + + # 记录数据 + 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 = [] @@ -1301,6 +1731,10 @@ def _prepare_display_info(self, perception: PerceptionResult) -> Dict: 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 {} @@ -1433,11 +1867,21 @@ def apply_manual_control(self): def change_state(self, new_state: FlightState): """状态转换""" if self.state != new_state: - self.logger.info(f"🔄 状态转换: {self.state.value} → {new_state.value}") + 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) @@ -1642,6 +2086,22 @@ def run_perception_loop(self): # 3. 控制执行阶段 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) @@ -1663,9 +2123,53 @@ def run_perception_loop(self): 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) @@ -1739,7 +2243,8 @@ def make_intelligent_decision(self, perception: PerceptionResult) -> Tuple[float '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 + 'speed_factor': speed_factor, + 'decision_time': time.time() - decision_start } # 检查是否到达目标附近 @@ -1794,10 +2299,17 @@ def make_intelligent_decision(self, perception: PerceptionResult) -> Tuple[float # 高度安全限制 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: @@ -1854,6 +2366,9 @@ def _execute_control_decision(self, decision): 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: @@ -1878,6 +2393,10 @@ def _report_status(self, exploration_start, perception): 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: @@ -1929,6 +2448,23 @@ def _cleanup_system(self): self.front_display.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 @@ -1947,15 +2483,19 @@ def _generate_summary_report(self): 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("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") @@ -1965,9 +2505,13 @@ def _generate_summary_report(self): 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['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(): @@ -1978,6 +2522,13 @@ def _generate_summary_report(self): 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}") @@ -2016,7 +2567,7 @@ def main(): """主程序入口""" # 显示启动信息 print("=" * 70) - print("AirSimNH 无人机感知探索系统 - 智能决策增强版（修复版）") + 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']}") @@ -2027,6 +2578,12 @@ def main(): print(" • 基于网格的信息增益探索") print(" • PID平滑飞行控制") 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("=" * 70) # 用户选择模式 From c7df8a43f0071537ac75d80fcc8f416a2f11d030 Mon Sep 17 00:00:00 2001 From: rhcgg <3136924319@qq.com> Date: Mon, 22 Dec 2025 00:27:31 +0800 Subject: [PATCH 11/23] =?UTF-8?q?=E5=8A=9F=E8=83=BD=E5=AE=8C=E5=96=84?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit 添加了对探索过程中对红色物品的记录 --- src/drone_perception/config.py | 33 +- .../perceptive_drone_explorer.py | 920 +++++++++--------- 2 files changed, 509 insertions(+), 444 deletions(-) diff --git a/src/drone_perception/config.py b/src/drone_perception/config.py index 7f9bd9647c..3c950b3fc7 100644 --- a/src/drone_perception/config.py +++ b/src/drone_perception/config.py @@ -1,4 +1,3 @@ -# config.py """ AirSimNH 无人机项目配置文件 所有可调参数集中在此管理 @@ -33,6 +32,15 @@ '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, # 物体记忆时间（秒），避免重复计数 } } @@ -70,6 +78,14 @@ # 目标管理 '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, # 探索奖励分数 + } } # ==================== 手动控制参数 ==================== @@ -93,6 +109,7 @@ 'SHOW_INFO_OVERLAY': True, # 显示信息叠加层 'REFRESH_RATE_MS': 30, # 刷新率 (毫秒)，建议30-50 'GRID_DISPLAY_SIZE': 150, # 探索网格显示大小 (像素) + 'SHOW_RED_OBJECTS': True, # 在画面中标记红色物体 } # ==================== 系统与安全参数 ==================== @@ -111,15 +128,21 @@ } # ==================== 相机配置 ==================== -# 注意：这里的相机名称需要与AirSim环境中的相机名称匹配 CAMERA = { 'DEFAULT_NAME': "0", # 默认相机名称 - # 如果"0"无效，可以尝试以下名称: 'POSSIBLE_NAMES': ["0", "1", "front_center", "front", "CameraActor_0"], # 深度相机参数 'DEPTH_FOV_DEG': 90, # 深度相机视野 (度) - 'MAX_DEPTH_RANGE': 100.0, # 最大深度范围 (米) + '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 + } } # ==================== 调试参数 ==================== @@ -130,6 +153,7 @@ 'SAVE_GRID_VISUALIZATION': True, # 是否保存网格可视化 'LOG_VECTOR_FIELD': False, # 是否记录向量场详细信息 'PERFORMANCE_PROFILING': False, # 是否启用性能分析 + 'SAVE_RED_OBJECT_IMAGES': False, # 是否保存检测到红色物体的图像 } # ==================== 数据记录参数 ==================== @@ -142,6 +166,7 @@ 'JSON_FILENAME': 'flight_data.json', # JSON文件名 'PERFORMANCE_MONITORING': True, # 启用性能监控 'SYSTEM_METRICS_INTERVAL': 5.0, # 系统指标记录间隔（秒） + 'RECORD_RED_OBJECTS': True, # 记录红色物体信息 } # ==================== 性能监控参数 ==================== diff --git a/src/drone_perception/perceptive_drone_explorer.py b/src/drone_perception/perceptive_drone_explorer.py index 84d958fc41..a99c3c8bf9 100644 --- a/src/drone_perception/perceptive_drone_explorer.py +++ b/src/drone_perception/perceptive_drone_explorer.py @@ -1,10 +1,10 @@ """ -AirSimNH 感知驱动自主探索无人机 - 智能决策增强版（修复版） +AirSimNH 感知驱动自主探索无人机 - 智能决策增强版（红色物体检测版） 核心：视觉感知 → 语义理解 → 智能决策 → 安全执行 集成：配置管理、日志系统、异常恢复、前视窗口显示 新增：向量场避障算法、基于网格的信息增益探索、平滑飞行控制 -新增：性能监控与数据闭环系统 -版本: 3.2 (完整版) +新增：性能监控与数据闭环系统、红色物体检测与记录 +版本: 3.3 (红色物体检测版) """ import airsim @@ -45,13 +45,18 @@ class DefaultConfig: '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}} + 'OPENNESS_THRESHOLD': 0.7}, + 'RED_OBJECT_DETECTION': {'ENABLED': True, 'MIN_AREA': 50, + 'MAX_AREA': 10000, 'UPDATE_INTERVAL': 1.0, + 'MEMORY_TIME': 5.0}} DISPLAY = {'WINDOW_WIDTH': 640, 'WINDOW_HEIGHT': 480, 'ENABLE_SHARPENING': True, - 'SHOW_INFO_OVERLAY': True, 'REFRESH_RATE_MS': 30} + 'SHOW_INFO_OVERLAY': True, 'REFRESH_RATE_MS': 30, 'SHOW_RED_OBJECTS': True} 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"} + CAMERA = {'DEFAULT_NAME': "0", + 'RED_COLOR_RANGE': {'LOWER1': [0, 120, 70], 'UPPER1': [10, 255, 255], + 'LOWER2': [170, 120, 70], 'UPPER2': [180, 255, 255]}} MANUAL = { 'CONTROL_SPEED': 3.0, 'ALTITUDE_SPEED': 2.0, @@ -63,59 +68,55 @@ class DefaultConfig: '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, # 目标到达判定距离（米） + '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} } DEBUG = { 'SAVE_PERCEPTION_IMAGES': False, 'IMAGE_SAVE_INTERVAL': 50, - 'LOG_DECISION_DETAILS': False + 'LOG_DECISION_DETAILS': False, + 'SAVE_RED_OBJECT_IMAGES': False } - # 新增：数据记录参数 DATA_RECORDING = { 'ENABLED': True, - 'RECORD_INTERVAL': 0.2, # 记录间隔（秒） + '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, # 系统指标记录间隔 + 'PERFORMANCE_MONITORING': True, + 'SYSTEM_METRICS_INTERVAL': 5.0, + 'RECORD_RED_OBJECTS': True } - # 新增：性能监控参数 PERFORMANCE = { 'ENABLE_REALTIME_METRICS': True, - 'CPU_WARNING_THRESHOLD': 80.0, # CPU使用率警告阈值 - 'MEMORY_WARNING_THRESHOLD': 80.0, # 内存使用率警告阈值 - 'LOOP_TIME_WARNING_THRESHOLD': 0.2, # 循环时间警告阈值（秒） + 'CPU_WARNING_THRESHOLD': 80.0, + 'MEMORY_WARNING_THRESHOLD': 80.0, + 'LOOP_TIME_WARNING_THRESHOLD': 0.2, 'SAVE_PERFORMANCE_REPORT': True, - 'REPORT_INTERVAL': 30.0, # 性能报告间隔（秒） + 'REPORT_INTERVAL': 30.0, } config = DefaultConfig() @@ -131,6 +132,20 @@ class FlightState(Enum): EMERGENCY = "紧急状态" MANUAL = "手动控制" PLANNING = "路径规划" + RED_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 class Vector2D: @@ -161,7 +176,6 @@ def normalize(self): return Vector2D() def rotate(self, angle): - """旋转向量""" cos_a = math.cos(angle) sin_a = math.sin(angle) return Vector2D( @@ -196,15 +210,12 @@ def update(self, error, dt=None): 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)) @@ -216,50 +227,35 @@ def __init__(self, resolution=2.0, grid_size=50): self.grid_size = grid_size self.half_size = grid_size // 2 - # 初始化网格：0=未知，1=已探索，0.x=部分探索 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.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 # 探索半径（网格单元） + radius = 3 for dx in range(-radius, radius + 1): for dy in range(-radius, radius + 1): @@ -270,17 +266,14 @@ def update_position(self, world_x, world_y): 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: # 足够探索 + 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) @@ -289,7 +282,6 @@ def _update_frontiers(self): 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): @@ -301,53 +293,78 @@ def _update_frontiers(self): 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 # 障碍物区域不可探索 + 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 get_best_exploration_target(self, current_pos, red_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]) - def get_best_exploration_target(self, current_pos): - """获取最佳探索目标""" if not self.frontier_cells: - # 如果没有前沿，返回随机方向 angle = random.uniform(0, 2 * math.pi) - distance = 10.0 # 10米外 + 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) # 归一化 + 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) # 1分钟内衰减 + 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'] - # 综合得分 score = ( config.INTELLIGENT_DECISION['CURIOUSITY_WEIGHT'] * info_gain + (1 - config.INTELLIGENT_DECISION['MEMORY_WEIGHT'] * time_factor) - - distance_cost * 0.3 + distance_cost * 0.3 + + red_bonus ) if score > best_score: @@ -357,7 +374,6 @@ def get_best_exploration_target(self, current_pos): return best_target def visualize_grid(self, size=300): - """可视化网格""" if self.grid.size == 0: return None @@ -371,19 +387,19 @@ def visualize_grid(self, size=300): color = (0, 0, 0) if (x, y) == self.current_idx: - color = (0, 255, 0) # 当前位置：绿色 + color = (0, 255, 0) elif self.obstacle_grid[x, y]: - color = (0, 0, 255) # 障碍物：红色 + color = (0, 0, 255) + elif self.red_object_grid[x, y]: + color = (0, 100, 255) elif self.grid[x, y] > 0.7: - color = (200, 200, 200) # 已探索：灰色 + color = (200, 200, 200) elif self.grid[x, y] > 0.3: - color = (100, 100, 100) # 部分探索：深灰色 + 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))) # 绿到黄 + color = (0, int(255 * gain), int(255 * (1 - gain))) - # 绘制单元格 x1 = x * cell_size y1 = y * cell_size x2 = (x + 1) * cell_size @@ -395,16 +411,14 @@ def visualize_grid(self, size=300): 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: @@ -415,7 +429,6 @@ def __init__(self, enable_csv=True, enable_json=True, csv_filename=None, json_fi else: self.json_filename = f"flight_data_{timestamp}.json" - # 数据缓存 self.data_buffer = [] self.json_data = { "flight_info": { @@ -430,7 +443,6 @@ def __init__(self, enable_csv=True, enable_json=True, csv_filename=None, json_fi "flight_data": [] } - # 性能指标 self.performance_metrics = { "start_time": time.time(), "cpu_usage": [], @@ -439,7 +451,8 @@ def __init__(self, enable_csv=True, enable_json=True, csv_filename=None, json_fi "data_points": 0 } - # 定义CSV列名 + self.red_objects_detected = [] + self.csv_columns = [ 'timestamp', 'loop_count', 'state', 'pos_x', 'pos_y', 'pos_z', 'vel_x', 'vel_y', 'vel_z', 'yaw', 'pitch', 'roll', @@ -449,10 +462,10 @@ def __init__(self, enable_csv=True, enable_json=True, csv_filename=None, json_fi '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' + 'adaptive_speed_factor', 'decision_making_time', 'perception_time', + 'red_objects_count', 'red_objects_detected', 'red_objects_visited' ] - # 初始化CSV文件 if self.enable_csv: self._init_csv_file() @@ -461,7 +474,6 @@ def __init__(self, enable_csv=True, enable_json=True, csv_filename=None, json_fi print(f" JSON文件: {self.json_filename}") def _init_csv_file(self): - """初始化CSV文件，写入列名""" try: with open(self.csv_filename, 'w', newline='', encoding='utf-8') as f: writer = csv.DictWriter(f, fieldnames=self.csv_columns) @@ -471,50 +483,60 @@ def _init_csv_file(self): 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() - # 记录到CSV 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) - # 记录到JSON缓冲区 if self.enable_json: self.json_data['flight_data'].append(data_dict) - # 更新性能指标 self.performance_metrics['data_points'] += 1 - # 收集系统性能数据 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 + } + + self.red_objects_detected.append(red_object_data) + + if 'red_objects' not in self.json_data: + self.json_data['red_objects'] = [] + + self.json_data['red_objects'].append(red_object_data) + + except Exception as e: + print(f"⚠️ 记录红色物体时出错: {e}") + def _collect_system_metrics(self): - """收集系统性能指标""" try: - # CPU使用率 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 = 1000 if len(self.performance_metrics['cpu_usage']) > max_length: self.performance_metrics['cpu_usage'] = self.performance_metrics['cpu_usage'][-max_length:] @@ -525,16 +547,13 @@ def _collect_system_metrics(self): print(f"⚠️ 收集系统指标时出错: {e}") def record_loop_time(self, loop_time): - """记录循环时间""" self.performance_metrics['loop_times'].append(loop_time) - # 限制数据长度 max_length = 1000 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(), @@ -551,15 +570,21 @@ def record_event(self, event_type, event_data): print(f"⚠️ 记录事件时出错: {e}") def save_json_data(self): - """保存JSON数据到文件""" 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 + } + with open(self.json_filename, 'w', encoding='utf-8') as f: json.dump(self.json_data, f, indent=2, ensure_ascii=False) @@ -569,21 +594,17 @@ def save_json_data(self): print(f"❌ 保存JSON数据时出错: {e}") def _calculate_performance_stats(self): - """计算性能统计数据""" if not self.performance_metrics['cpu_usage']: return - # 计算CPU统计 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']) @@ -591,7 +612,6 @@ def _calculate_performance_stats(self): else: loop_avg = loop_max = loop_min = 0 - # 添加到JSON数据 self.json_data['performance_summary'] = { 'total_data_points': self.performance_metrics['data_points'], 'total_time_seconds': time.time() - self.performance_metrics['start_time'], @@ -613,12 +633,10 @@ def _calculate_performance_stats(self): } def generate_performance_report(self): - """生成性能报告""" try: if not self.performance_metrics['cpu_usage']: return "无性能数据可用" - # 计算统计 self._calculate_performance_stats() report = "\n" + "="*60 + "\n" @@ -643,9 +661,13 @@ def generate_performance_report(self): 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" + report += "="*60 + "\n" - # 检查警告 warnings = [] if cpu_avg > config.PERFORMANCE['CPU_WARNING_THRESHOLD']: warnings.append(f"⚠️ CPU使用率过高: {cpu_avg:.1f}%") @@ -678,13 +700,18 @@ class PerceptionResult: 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 # 新增：障碍物位置列表 + obstacle_positions: List[Tuple[float, float]] = None + red_objects: List[RedObject] = None + red_objects_count: int = 0 + red_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 = [] class VectorFieldPlanner: @@ -694,55 +721,43 @@ def __init__(self): 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.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): - """计算合成向量""" - # 目标吸引力 + def compute_vector(self, current_pos, goal_pos, obstacles, red_objects=None): attraction_vector = self._compute_attraction(current_pos, goal_pos) - - # 障碍物排斥力 repulsion_vector = self._compute_repulsion(current_pos, obstacles) + red_attraction_vector = Vector2D() + if red_objects: + red_attraction_vector = self._compute_red_attraction(current_pos, red_objects) - # 合成向量 - combined_vector = attraction_vector + repulsion_vector - - # 向量平滑 + combined_vector = attraction_vector + repulsion_vector + red_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: # 已到达目标 + 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: @@ -751,21 +766,34 @@ def _compute_repulsion(self, current_pos, obstacles): 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 _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 @@ -780,7 +808,6 @@ def _smooth_vector(self, new_vector): return smoothed def _limit_turn_angle(self, vector): - """限制转向角度""" if self.current_vector.magnitude() < 0.1: return vector @@ -788,16 +815,13 @@ def _limit_turn_angle(self, vector): 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 @@ -817,40 +841,33 @@ def __init__(self, window_name="无人机前视画面", width=None, height=None, self.show_info = (show_info if show_info is not None else config.DISPLAY['SHOW_INFO_OVERLAY']) - # 图像队列 self.image_queue = queue.Queue(maxsize=3) 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.performance_info = { 'cpu_usage': 0.0, 'memory_usage': 0.0, 'update_time': time.time() } - # 启动显示线程 self.start() def start(self): - """启动显示线程""" if self.display_thread and self.display_thread.is_alive(): return @@ -863,7 +880,6 @@ def start(self): self.display_thread.start() def stop(self): - """停止显示线程""" self.display_active = False self.exit_display_flag = True if self.display_thread: @@ -872,19 +888,16 @@ def stop(self): def update_image(self, image_data: np.ndarray, info: Optional[Dict] = None, manual_info: Optional[List[str]] = None, additional_images: Optional[Dict] = 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() @@ -905,13 +918,11 @@ def update_image(self, image_data: np.ndarray, info: Optional[Dict] = None, print(f"⚠️ 更新图像时出错: {e}") def update_performance_info(self, cpu_usage, memory_usage): - """更新性能信息""" self.performance_info['cpu_usage'] = cpu_usage self.performance_info['memory_usage'] = memory_usage self.performance_info['update_time'] = time.time() def set_manual_mode(self, manual_mode): - """设置手动模式状态""" self.manual_mode = manual_mode self.exit_manual_flag = False self.key_states = {} @@ -919,19 +930,15 @@ def set_manual_mode(self, manual_mode): 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((300, 400, 3), dtype=np.uint8) cv2.putText(wait_img, "等待无人机图像...", (50, 150), cv2.FONT_HERSHEY_SIMPLEX, 0.8, (255, 255, 255), 2) @@ -954,7 +961,6 @@ def _display_loop(self): additional_images = {} try: - # 获取最新图像 if not self.image_queue.empty(): packet = self.image_queue.get_nowait() display_image = packet['image'] @@ -962,10 +968,8 @@ def _display_loop(self): manual_info = packet['manual_info'] additional_images = packet.get('additional_images', {}) - # 更新统计 self._update_stats() - # 清空队列中的旧帧 while not self.image_queue.empty(): try: self.image_queue.get_nowait() @@ -974,38 +978,30 @@ def _display_loop(self): except queue.Empty: pass - # 显示图像 if display_image is not None: - # 添加信息叠加 if self.show_info: display_image = self._add_info_overlay(display_image, info, manual_info, additional_images) cv2.imshow(self.window_name, display_image) elif self.paused: - # 暂停时显示提示 blank = np.zeros((300, 400, 3), dtype=np.uint8) cv2.putText(blank, "PAUSED", (120, 150), cv2.FONT_HERSHEY_SIMPLEX, 1.5, (0, 0, 255), 3) cv2.imshow(self.window_name, blank) - # 处理键盘输入（非阻塞） key = cv2.waitKey(config.DISPLAY.get('REFRESH_RATE_MS', 30)) & 0xFF - # 记录当前按键 current_keys = {} - if key != 255: # 有按键按下 + 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("🔄 用户关闭了前视窗口") @@ -1015,7 +1011,6 @@ def _display_loop(self): self.display_active = False break - # 清理窗口 try: cv2.destroyWindow(self.window_name) except: @@ -1023,24 +1018,17 @@ def _display_loop(self): cv2.waitKey(1) def _handle_manual_mode_key(self, key): - """处理手动控制模式下的按键""" - key_char = chr(key).lower() if 0 <= key <= 255 else '' - - # ESC键：退出手动模式 - if key == 27: # ESC + if key == 27: print("收到退出手动模式指令") self.exit_manual_flag = True return - # 记录手动控制按键 self.key_states[key] = True - # 特别处理空格键（悬停） - if key == 32: # 空格 + 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': @@ -1062,22 +1050,17 @@ def _handle_window_control_key(self, key, display_image): 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 @@ -1088,7 +1071,6 @@ def _update_stats(self): def _add_info_overlay(self, image: np.ndarray, info: Dict, manual_info: List[str] = None, additional_images: Dict = None) -> np.ndarray: - """在图像上叠加状态信息""" if image is None or image.size == 0: return image @@ -1096,32 +1078,33 @@ def _add_info_overlay(self, image: np.ndarray, info: Dict, manual_info: List[str overlay = image.copy() height, width = image.shape[:2] - # 判断是否为手动模式 is_manual = info.get('state', '') == "手动控制" - # 如果有附加图像（如网格图），调整信息栏高度 grid_img = additional_images.get('grid') if additional_images else None info_height = 180 if (is_manual and manual_info) or grid_img is not None else 100 - # 创建半透明信息栏 cv2.rectangle(overlay, (0, 0), (width, info_height), (0, 0, 0), -1) cv2.addWeighted(overlay, 0.7, image, 0.3, 0, image) - # 飞行状态 state = info.get('state', 'UNKNOWN') state_color = (0, 255, 0) if '探索' in state else (0, 255, 255) if '悬停' in state else (255, 255, 0) if '手动' in state else (0, 0, 255) cv2.putText(image, f"状态: {state}", (10, 30), cv2.FONT_HERSHEY_SIMPLEX, 0.7, state_color, 2) - # 位置信息 pos = info.get('position', (0, 0, 0)) cv2.putText(image, f"位置: ({pos[0]:.1f}, {pos[1]:.1f}, {-pos[2]:.1f}m)", (10, 60), cv2.FONT_HERSHEY_SIMPLEX, 0.6, (255, 255, 255), 1) - # 智能决策信息 + red_objects_count = info.get('red_objects_count', 0) + red_objects_visited = info.get('red_objects_visited', 0) + if red_objects_count > 0: + red_text = f"红色物体: {red_objects_visited}/{red_objects_count}" + cv2.putText(image, red_text, (10, 90), + cv2.FONT_HERSHEY_SIMPLEX, 0.6, (0, 100, 255), 2) + decision_info = info.get('decision_info', {}) if decision_info: - y_pos = 90 + y_pos = 120 if red_objects_count > 0 else 90 for key, value in decision_info.items(): if key == 'vector_angle': cv2.putText(image, f"方向: {math.degrees(value):.0f}°", (10, y_pos), @@ -1132,9 +1115,8 @@ def _add_info_overlay(self, image: np.ndarray, info: Dict, manual_info: List[str cv2.FONT_HERSHEY_SIMPLEX, 0.5, (200, 200, 255), 1) y_pos += 20 - # 手动控制信息 if is_manual and manual_info: - y_start = 130 + y_start = 130 if red_objects_count > 0 else 100 for i, line in enumerate(manual_info): y_pos = y_start + i * 20 cv2.putText(image, line, (10, y_pos), @@ -1142,19 +1124,16 @@ def _add_info_overlay(self, image: np.ndarray, info: Dict, manual_info: List[str cv2.putText(image, "手动控制中...", (width - 150, 60), cv2.FONT_HERSHEY_SIMPLEX, 0.6, (255, 255, 0), 1) - elif not is_manual: - # 障碍物信息 + elif not is_manual and red_objects_count == 0: obs_dist = info.get('obstacle_distance', 0.0) obs_color = (0, 0, 255) if obs_dist < 5.0 else (0, 165, 255) if obs_dist < 10.0 else (0, 255, 0) cv2.putText(image, f"障碍: {obs_dist:.1f}m", (10, 90), cv2.FONT_HERSHEY_SIMPLEX, 0.7, obs_color, 2) - # 显示统计信息 fps_text = f"FPS: {self.display_stats['fps']:.1f}" cv2.putText(image, fps_text, (width - 120, 30), cv2.FONT_HERSHEY_SIMPLEX, 0.6, (255, 255, 0), 1) - # 性能信息 if self.performance_info['cpu_usage'] > 0: cpu_text = f"CPU: {self.performance_info['cpu_usage']:.1f}%" mem_text = f"MEM: {self.performance_info['memory_usage']:.1f}%" @@ -1167,22 +1146,18 @@ def _add_info_overlay(self, image: np.ndarray, info: Dict, manual_info: List[str cv2.putText(image, mem_text, (width - 120, 80), cv2.FONT_HERSHEY_SIMPLEX, 0.5, mem_color, 1) - # 如果有网格图，显示在右上角 if grid_img is not None and grid_img.size > 0: grid_size = 150 grid_resized = cv2.resize(grid_img, (grid_size, grid_size)) - # 在图像右上角放置网格图 x_offset = width - grid_size - 10 y_offset = info_height + 10 if y_offset + grid_size < height: - # 创建网格图的背景 cv2.rectangle(image, (x_offset-2, y_offset-2), (x_offset+grid_size+2, y_offset+grid_size+2), (255, 255, 255), 1) - # 将网格图放入指定位置 image[y_offset:y_offset+grid_size, x_offset:x_offset+grid_size] = grid_resized cv2.putText(image, "探索网格", (x_offset, y_offset-5), @@ -1194,7 +1169,6 @@ def _add_info_overlay(self, image: np.ndarray, info: Dict, manual_info: List[str 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" @@ -1205,21 +1179,18 @@ def _save_screenshot(self, image: Optional[np.ndarray]): class PerceptiveExplorer: - """基于感知的自主探索无人机 - 智能决策增强版（修复版）""" + """基于感知的自主探索无人机 - 智能决策增强版（红色物体检测版）""" def __init__(self, drone_name=""): - # 初始化日志系统 self._setup_logging() self.logger.info("=" * 60) - self.logger.info("AirSimNH 感知驱动自主探索系统 - 智能决策增强版（修复版）") + self.logger.info("AirSimNH 感知驱动自主探索系统 - 红色物体检测版") self.logger.info("=" * 60) - # 初始化AirSim连接 self.client = None self.drone_name = drone_name self._connect_to_airsim() - # 启用API控制 try: self.client.enableApiControl(True, vehicle_name=drone_name) self.client.armDisarm(True, vehicle_name=drone_name) @@ -1228,19 +1199,16 @@ def __init__(self, drone_name=""): 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'] @@ -1248,14 +1216,12 @@ def __init__(self, drone_name=""): 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'] ) - # PID控制器 self.velocity_pid = PIDController( config.INTELLIGENT_DECISION['PID_KP'], config.INTELLIGENT_DECISION['PID_KI'], @@ -1263,35 +1229,34 @@ def __init__(self, drone_name=""): ) 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.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, @@ -1306,39 +1271,37 @@ def __init__(self, drone_name=""): 'average_loop_time': 0.0, 'max_loop_time': 0.0, 'min_loop_time': 100.0, + 'red_objects_detected': 0, + 'red_objects_visited': 0, } - # 前视窗口 self.front_display = None self._setup_front_display() - # 手动控制状态 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" 红色物体检测: 已启用") 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') @@ -1350,7 +1313,6 @@ def _setup_logging(self): print(f"⚠️ 无法创建日志文件: {e}") def _setup_data_logger(self): - """初始化数据记录器""" try: self.data_logger = DataLogger( enable_csv=config.DATA_RECORDING['SAVE_TO_CSV'], @@ -1364,7 +1326,6 @@ def _setup_data_logger(self): self.data_logger = None def _connect_to_airsim(self): - """连接到AirSim，支持重试机制""" max_attempts = config.SYSTEM['MAX_RECONNECT_ATTEMPTS'] for attempt in range(1, max_attempts + 1): try: @@ -1388,15 +1349,12 @@ def _connect_to_airsim(self): 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 @@ -1404,7 +1362,6 @@ def _check_connection_health(self): return False def _setup_front_display(self): - """初始化前视显示窗口""" try: self.front_display = FrontViewDisplay( window_name=f"无人机前视 - {self.drone_name or 'AirSimNH'}", @@ -1418,20 +1375,194 @@ def _setup_front_display(self): self.logger.error(f"❌ 前视窗口初始化失败: {e}") self.front_display = None + 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"ID:{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 _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 _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 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( @@ -1452,21 +1583,18 @@ def get_depth_perception(self) -> PerceptionResult: 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) @@ -1481,21 +1609,17 @@ def get_depth_perception(self) -> PerceptionResult: 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: @@ -1505,7 +1629,6 @@ def get_depth_perception(self) -> PerceptionResult: 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']: @@ -1513,49 +1636,50 @@ def get_depth_perception(self) -> PerceptionResult: except ValueError as e: self.logger.error(f"❌ 深度图像数据转换错误: {e}") - return result except Exception as e: self.logger.error(f"❌ 深度图像处理异常: {e}") - return result - # 处理前视图像 front_response = responses[1] if front_response and hasattr(front_response, 'image_data_uint8'): try: - # 转换为OpenCV格式 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 + result.front_image = img_bgr - # 准备显示信息 display_info = self._prepare_display_info(result) - # 更新探索网格 self._update_exploration_grid(result) - # 记录数据 self._record_flight_data(result) - # 更新前视窗口 if self.front_display: manual_info = None if self.state == FlightState.MANUAL: manual_info = self._get_manual_control_info() - # 获取网格可视化图像 grid_img = self.exploration_grid.visualize_grid(size=150) additional_images = {'grid': grid_img} if grid_img is not None else {} - # 更新性能信息 + display_image = result.red_objects_image if result.red_objects_image is not None else img_bgr + if config.PERFORMANCE['ENABLE_REALTIME_METRICS']: cpu_usage = psutil.cpu_percent(interval=0) memory_usage = psutil.virtual_memory().percent self.front_display.update_performance_info(cpu_usage, memory_usage) - self.front_display.update_image(img_bgr, display_info, manual_info, additional_images) + self.front_display.update_image(display_image, display_info, manual_info, additional_images) self.stats['front_image_updates'] += 1 except Exception as e: @@ -1563,19 +1687,16 @@ def get_depth_perception(self) -> PerceptionResult: 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}, 障碍物数={len(result.obstacle_positions)}") + f"开阔度={result.open_space_score:.2f}, 红色物体={result.red_objects_count}个") - except Exception as e: # 修复：捕获通用异常 + 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}") @@ -1587,7 +1708,6 @@ def get_depth_perception(self) -> PerceptionResult: return result def _record_flight_data(self, perception: PerceptionResult): - """记录飞行数据""" if not config.DATA_RECORDING['ENABLED'] or not self.data_logger: return @@ -1596,20 +1716,19 @@ def _record_flight_data(self, perception: PerceptionResult): 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) + data_dict = { 'timestamp': datetime.now().isoformat(), 'loop_count': self.loop_count, @@ -1637,9 +1756,11 @@ def _record_flight_data(self, perception: PerceptionResult): '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, } - # 记录数据 self.data_logger.record_flight_data(data_dict) self.stats['data_points_recorded'] += 1 self.last_data_record_time = current_time @@ -1648,44 +1769,35 @@ def _record_flight_data(self, perception: PerceptionResult): 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 # 采样步长 + 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] - # 将像素坐标转换为相对于相机的3D坐标 - # 简化模型：假设相机水平视角为90度 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 # 深度方向 + 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) @@ -1696,26 +1808,24 @@ def _extract_obstacle_positions(self, depth_array, height, width): 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) + 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 @@ -1725,13 +1835,13 @@ def _prepare_display_info(self, perception: PerceptionResult) -> Dict: '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), } - # 添加决策信息 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'] @@ -1740,10 +1850,8 @@ def _prepare_display_info(self, perception: PerceptionResult) -> Dict: return {} def _get_manual_control_info(self): - """获取手动控制信息""" info_lines = [] - # 控制状态 if self.control_keys: key_names = [] for key in self.control_keys: @@ -1763,7 +1871,7 @@ def _get_manual_control_info(self): key_names.append("左转") elif key == ord('x'): key_names.append("右转") - elif key == 32: # 空格 + elif key == 32: key_names.append("悬停") if key_names: @@ -1771,38 +1879,34 @@ def _get_manual_control_info(self): 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.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) @@ -1810,7 +1914,6 @@ def apply_manual_control(self): 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) @@ -1818,54 +1921,45 @@ def apply_manual_control(self): 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'] # AirSim中Z轴向下为正 + 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: # 空格 + if key == 32: self.client.hoverAsync(vehicle_name=self.drone_name) - self.control_keys = {} # 清空控制键 + 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}") @@ -1873,7 +1967,6 @@ def change_state(self, new_state: FlightState): self.state_history.append((time.time(), new_state)) self.stats['state_changes'] += 1 - # 记录状态改变事件 if self.data_logger: event_data = { 'old_state': old_state, @@ -1883,7 +1976,6 @@ def change_state(self, new_state: FlightState): 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) @@ -1893,11 +1985,9 @@ def run_manual_control(self): return try: - # 切换到手动控制状态 self.change_state(FlightState.MANUAL) self.manual_control_start = time.time() - # 设置前视窗口为手动模式 self.front_display.set_manual_mode(True) self.logger.info("🕹️ 进入手动控制模式") @@ -1913,41 +2003,34 @@ def run_manual_control(self): 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_display.should_exit_manual(): self.logger.info("收到退出手动模式指令") manual_active = False break - # 获取前视窗口的按键状态 if self.front_display: window_keys = self.front_display.get_control_inputs() self.control_keys = window_keys.copy() - # 检查前视窗口是否还在运行 if not self.front_display.display_active: self.logger.info("前视窗口已关闭，退出手动模式") manual_active = False break - # 应用手动控制（限制频率） current_time = time.time() - if current_time - last_control_time >= 0.05: # 20Hz + 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: # 10Hz + if current_time - last_image_time >= 0.1: try: camera_name = config.CAMERA['DEFAULT_NAME'] responses = self.client.simGetImages([ @@ -1965,7 +2048,6 @@ def run_manual_control(self): 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 @@ -1977,7 +2059,6 @@ def run_manual_control(self): except: display_info = {} - # 更新前视窗口 if self.front_display: manual_info = self._get_manual_control_info() self.front_display.update_image(img_bgr, display_info, manual_info) @@ -1985,7 +2066,14 @@ def run_manual_control(self): 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) + except: + pass + time.sleep(0.01) except KeyboardInterrupt: @@ -1996,17 +2084,14 @@ def run_manual_control(self): 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_display: self.front_display.set_manual_mode(False) - # 停止运动，悬停 try: self.client.hoverAsync(vehicle_name=self.drone_name).join() except: @@ -2014,15 +2099,14 @@ def run_manual_control(self): 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("="*60) - print("请选择下一步操作:") + print("请选择下一步:") print(" 1. 继续自动探索") print(" 2. 再次进入手动模式") print(" 3. 降落并结束任务") @@ -2052,23 +2136,19 @@ def run_manual_control(self): 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 @@ -2077,41 +2157,42 @@ def run_perception_loop(self): self.loop_count += 1 loop_start = time.time() - # 1. 感知阶段 perception = self.get_depth_perception() - # 2. 智能决策阶段 + 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) + except: + pass + decision = self.make_intelligent_decision(perception) - # 3. 控制执行阶段 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) - # 循环频率控制 loop_time = time.time() - loop_start if loop_time < 0.1: time.sleep(0.1 - loop_time) - # 正常结束 self.logger.info("⏰ 探索时间到，开始返航") self._finish_mission() @@ -2124,16 +2205,13 @@ def run_perception_loop(self): 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}%") @@ -2150,7 +2228,6 @@ def _generate_performance_report(self): for warning in warnings: self.logger.warning(f" {warning}") - # 记录性能数据 if self.data_logger: performance_data = { 'timestamp': datetime.now().isoformat(), @@ -2167,7 +2244,6 @@ def _generate_performance_report(self): 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() @@ -2184,15 +2260,16 @@ def make_intelligent_decision(self, perception: PerceptionResult) -> Tuple[float 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)) + self.exploration_target = self.exploration_grid.get_best_exploration_target( + (pos.x_val, pos.y_val), + perception.red_objects + ) self.target_update_time = current_time if self.exploration_target: @@ -2204,102 +2281,96 @@ def make_intelligent_decision(self, perception: PerceptionResult) -> Tuple[float 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) + self.exploration_target = self.exploration_grid.get_best_exploration_target( + current_pos, + perception.red_objects + ) self.target_update_time = time.time() - # 计算向量场 vector = self.vector_planner.compute_vector( current_pos, self.exploration_target, - perception.obstacle_positions + perception.obstacle_positions, + perception.red_objects ) - # 自适应速度调整 speed_factor = self._calculate_adaptive_speed(perception, vector.magnitude()) - # 应用PID控制平滑速度 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, '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: # 使用配置的距离 + 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 + None, + perception.obstacle_positions, + perception.red_objects ) if avoid_vector.magnitude() > 0.1: - avoid_vector = avoid_vector.normalize() * 1.5 # 避障速度 + 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.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 - # 高度PID控制 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 @@ -2307,32 +2378,27 @@ def make_intelligent_decision(self, perception: PerceptionResult) -> Tuple[float 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) - # 综合速度因子 - speed_factor = open_factor * obs_factor * vector_factor * 0.7 + red_factor = 0.8 if perception.red_objects_count > 0 else 1.0 + + speed_factor = open_factor * obs_factor * vector_factor * red_factor * 0.7 - # 限制在允许范围内 speed_factor = max( config.INTELLIGENT_DECISION['MIN_SPEED_FACTOR'], min(config.INTELLIGENT_DECISION['MAX_SPEED_FACTOR'], speed_factor) @@ -2341,14 +2407,12 @@ def _calculate_adaptive_speed(self, perception: PerceptionResult, vector_magnitu 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]: - # 使用平滑的速度控制 + if self.state in [FlightState.EXPLORING, FlightState.AVOIDING, FlightState.PLANNING, FlightState.RED_OBJECT_INSPECTION]: self.client.moveByVelocityZAsync( - target_vx, target_vy, target_z, 0.5, # 增加持续时间以平滑 + 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 @@ -2359,14 +2423,12 @@ def _execute_control_decision(self, decision): 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: @@ -2375,7 +2437,6 @@ def _execute_control_decision(self, decision): pass def _report_status(self, exploration_start, perception): - """报告系统状态""" elapsed = time.time() - exploration_start try: state = self.client.getMultirotorState(vehicle_name=self.drone_name) @@ -2388,6 +2449,8 @@ def _report_status(self, exploration_start, perception): 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" 智能决策: 向量场{self.stats['vector_field_updates']}次 " f"| 网格更新{self.stats['grid_updates']}次") self.logger.info(f" 探索网格: 前沿{len(self.exploration_grid.frontier_cells)}个") @@ -2403,7 +2466,6 @@ def _report_status(self, exploration_start, perception): self.logger.info("状态报告: 无法获取无人机状态") def _finish_mission(self): - """完成任务并生成总结报告""" self.logger.info("=" * 60) self.logger.info("探索任务完成，开始返航程序") self.logger.info("=" * 60) @@ -2411,12 +2473,10 @@ def _finish_mission(self): 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() @@ -2426,14 +2486,11 @@ def _finish_mission(self): self.logger.error(f"❌ 降落过程中出现异常: {e}") finally: - # 无论成功与否，都执行清理 self._cleanup_system() - # 生成最终报告 self._generate_summary_report() def _cleanup_system(self): - """清理系统资源""" self.logger.info("🧹 清理系统资源...") try: @@ -2443,22 +2500,18 @@ def _cleanup_system(self): except: self.logger.warning("⚠️ 释放控制时出现异常") - # 关闭前视窗口 if self.front_display: self.front_display.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: @@ -2466,7 +2519,6 @@ def _cleanup_system(self): self.logger.info(f"📄 性能报告已保存至: {report_filename}") def _generate_summary_report(self): - """生成运行总结报告""" total_time = time.time() - self.start_time self.logger.info("\n" + "=" * 60) @@ -2479,6 +2531,8 @@ def _generate_summary_report(self): 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['vector_field_updates']}") self.logger.info(f" 网格更新次数: {self.stats['grid_updates']}") self.logger.info(f" 探索前沿数量: {len(self.exploration_grid.frontier_cells)}") @@ -2491,11 +2545,10 @@ def _generate_summary_report(self): 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("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") @@ -2507,6 +2560,8 @@ def _generate_summary_report(self): 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['exceptions_caught']}\n") f.write(f"前视图像更新次数: {self.stats['front_image_updates']}\n") f.write(f"平均循环时间: {self.stats['average_loop_time']*1000:.1f}ms\n") @@ -2534,18 +2589,15 @@ def _generate_summary_report(self): 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() @@ -2554,7 +2606,6 @@ def emergency_stop(self): except Exception as e: self.logger.error(f"⚠️ 紧急降落异常: {e}") - # 关闭前视窗口 if self.front_display: self.front_display.stop() @@ -2564,10 +2615,8 @@ def emergency_stop(self): # ==================== 主程序入口 ==================== def main(): - """主程序入口""" - # 显示启动信息 print("=" * 70) - print("AirSimNH 无人机感知探索系统 - 智能决策增强版（数据闭环版）") + 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']}") @@ -2579,16 +2628,17 @@ def main(): print(" • PID平滑飞行控制") 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("=" * 70) - # 用户选择模式 print("\n请选择运行模式:") - print(" 1. 智能探索模式 (AI自主决策)") + print(" 1. 智能探索模式 (AI自主决策，包含红色物体检测)") print(" 2. 手动控制模式 (键盘控制)") print(" 3. 混合模式 (先自动探索，后可切换)") print("=" * 50) @@ -2597,10 +2647,8 @@ def main(): explorer = None try: - # 创建感知探索器 explorer = PerceptiveExplorer(drone_name="") - # 设置键盘中断处理 def signal_handler(sig, frame): print("\n⚠️ 用户中断，正在安全停止...") if explorer: @@ -2609,21 +2657,17 @@ def signal_handler(sig, frame): signal.signal(signal.SIGINT, signal_handler) - # 根据选择运行相应模式 if mode_choice == '1': - # 自动探索模式 print("\n" + "="*50) - print("启动智能探索模式") + print("启动智能探索模式（含红色物体检测）") 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) @@ -2632,27 +2676,24 @@ def signal_handler(sig, frame): print("起飞完成，可以开始手动控制") print("请切换到无人机前视窗口，使用WSAD键控制") - # 进入手动控制 explorer.run_manual_control() elif mode_choice == '3': - # 混合模式：先自动探索，后询问是否切换手动 print("\n" + "="*50) print("启动混合模式") print("="*50) - # 先运行一段时间的自动探索 - explorer.logger.info("🔍 开始智能探索...") + 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['red_objects_visited']}个") print("请选择下一步:") print(" 1. 进入手动控制模式") print(" 2. 继续智能探索") @@ -2682,7 +2723,6 @@ def signal_handler(sig, frame): print(f"\n❌ 程序启动异常: {e}") traceback.print_exc() - # 尝试安全降落 try: if explorer and explorer.client: explorer.client.landAsync().join() From 169d67cdcdc12906664951cf7943ee4ddeb35e5f Mon Sep 17 00:00:00 2001 From: rhcgg <3136924319@qq.com> Date: Mon, 22 Dec 2025 10:29:14 +0800 Subject: [PATCH 12/23] =?UTF-8?q?=E5=8A=9F=E8=83=BD=E8=BF=9B=E4=B8=80?= =?UTF-8?q?=E6=AD=A5=E5=AE=8C=E5=96=84?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit 主要新增功能说明 蓝色物体数据结构： 新增了BlueObject类，与RedObject结构相同 在PerceptionResult中添加了蓝色物体相关字段 蓝色物体检测算法： 新增了_detect_blue_objects方法，使用HSV颜色空间检测蓝色物体 蓝色物体在画面中用青色(255, 100, 0)框标记 智能决策扩展： 向量场规划器添加了蓝色物体吸引力计算 探索网格支持蓝色物体标记和吸引力计算 决策时优先考虑红色物体，其次蓝色物体 数据显示和记录： 前视窗口显示红色和蓝色物体的统计信息 数据记录器记录蓝色物体的检测和访问信息 性能报告包含蓝色物体统计 配置文件更新： 添加了蓝色物体检测参数 增加了蓝色物体探索的智能决策参数 添加了蓝色物体的HSV颜色范围配置 --- src/drone_perception/config.py | 26 + .../perceptive_drone_explorer.py | 451 ++++++++++++++++-- 2 files changed, 439 insertions(+), 38 deletions(-) diff --git a/src/drone_perception/config.py b/src/drone_perception/config.py index 3c950b3fc7..9e53e27276 100644 --- a/src/drone_perception/config.py +++ b/src/drone_perception/config.py @@ -41,6 +41,15 @@ '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, # 物体记忆时间（秒） } } @@ -85,6 +94,14 @@ '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, # 探索奖励分数 } } @@ -110,6 +127,7 @@ 'REFRESH_RATE_MS': 30, # 刷新率 (毫秒)，建议30-50 'GRID_DISPLAY_SIZE': 150, # 探索网格显示大小 (像素) 'SHOW_RED_OBJECTS': True, # 在画面中标记红色物体 + 'SHOW_BLUE_OBJECTS': True, # 在画面中标记蓝色物体（新增） } # ==================== 系统与安全参数 ==================== @@ -142,6 +160,12 @@ '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], # 蓝色上限 } } @@ -154,6 +178,7 @@ 'LOG_VECTOR_FIELD': False, # 是否记录向量场详细信息 'PERFORMANCE_PROFILING': False, # 是否启用性能分析 'SAVE_RED_OBJECT_IMAGES': False, # 是否保存检测到红色物体的图像 + 'SAVE_BLUE_OBJECT_IMAGES': False, # 是否保存检测到蓝色物体的图像（新增） } # ==================== 数据记录参数 ==================== @@ -167,6 +192,7 @@ 'PERFORMANCE_MONITORING': True, # 启用性能监控 'SYSTEM_METRICS_INTERVAL': 5.0, # 系统指标记录间隔（秒） 'RECORD_RED_OBJECTS': True, # 记录红色物体信息 + 'RECORD_BLUE_OBJECTS': True, # 记录蓝色物体信息（新增） } # ==================== 性能监控参数 ==================== diff --git a/src/drone_perception/perceptive_drone_explorer.py b/src/drone_perception/perceptive_drone_explorer.py index a99c3c8bf9..292541a1a1 100644 --- a/src/drone_perception/perceptive_drone_explorer.py +++ b/src/drone_perception/perceptive_drone_explorer.py @@ -1,10 +1,10 @@ """ -AirSimNH 感知驱动自主探索无人机 - 智能决策增强版（红色物体检测版） +AirSimNH 感知驱动自主探索无人机 - 智能决策增强版（红色与蓝色物体检测版） 核心：视觉感知 → 语义理解 → 智能决策 → 安全执行 集成：配置管理、日志系统、异常恢复、前视窗口显示 新增：向量场避障算法、基于网格的信息增益探索、平滑飞行控制 -新增：性能监控与数据闭环系统、红色物体检测与记录 -版本: 3.3 (红色物体检测版) +新增：性能监控与数据闭环系统、红色与蓝色物体检测与记录 +版本: 3.4 (双色物体检测版) """ import airsim @@ -48,15 +48,20 @@ class DefaultConfig: 'OPENNESS_THRESHOLD': 0.7}, 'RED_OBJECT_DETECTION': {'ENABLED': True, 'MIN_AREA': 50, 'MAX_AREA': 10000, 'UPDATE_INTERVAL': 1.0, - 'MEMORY_TIME': 5.0}} + 'MEMORY_TIME': 5.0}, + 'BLUE_OBJECT_DETECTION': {'ENABLED': True, 'MIN_AREA': 50, + 'MAX_AREA': 10000, 'UPDATE_INTERVAL': 1.0, + 'MEMORY_TIME': 5.0}} DISPLAY = {'WINDOW_WIDTH': 640, 'WINDOW_HEIGHT': 480, 'ENABLE_SHARPENING': True, - 'SHOW_INFO_OVERLAY': True, 'REFRESH_RATE_MS': 30, 'SHOW_RED_OBJECTS': True} + 'SHOW_INFO_OVERLAY': True, 'REFRESH_RATE_MS': 30, 'SHOW_RED_OBJECTS': True, + 'SHOW_BLUE_OBJECTS': True} 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]}} + 'LOWER2': [170, 120, 70], 'UPPER2': [180, 255, 255]}, + 'BLUE_COLOR_RANGE': {'LOWER': [100, 150, 50], 'UPPER': [130, 255, 255]}} MANUAL = { 'CONTROL_SPEED': 3.0, 'ALTITUDE_SPEED': 2.0, @@ -91,13 +96,16 @@ class DefaultConfig: '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} + '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} } DEBUG = { 'SAVE_PERCEPTION_IMAGES': False, 'IMAGE_SAVE_INTERVAL': 50, 'LOG_DECISION_DETAILS': False, - 'SAVE_RED_OBJECT_IMAGES': False + 'SAVE_RED_OBJECT_IMAGES': False, + 'SAVE_BLUE_OBJECT_IMAGES': False } DATA_RECORDING = { 'ENABLED': True, @@ -108,7 +116,8 @@ class DefaultConfig: 'JSON_FILENAME': 'flight_data.json', 'PERFORMANCE_MONITORING': True, 'SYSTEM_METRICS_INTERVAL': 5.0, - 'RECORD_RED_OBJECTS': True + 'RECORD_RED_OBJECTS': True, + 'RECORD_BLUE_OBJECTS': True } PERFORMANCE = { 'ENABLE_REALTIME_METRICS': True, @@ -133,6 +142,7 @@ class FlightState(Enum): MANUAL = "手动控制" PLANNING = "路径规划" RED_OBJECT_INSPECTION = "红色物体检查" + BLUE_OBJECT_INSPECTION = "蓝色物体检查" # 新增 @dataclass @@ -148,6 +158,19 @@ class RedObject: 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 + + class Vector2D: """二维向量类""" def __init__(self, x=0.0, y=0.0): @@ -232,6 +255,7 @@ def __init__(self, resolution=2.0, grid_size=50): 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.current_idx = (self.half_size, self.half_size) self.frontier_cells = set() @@ -317,7 +341,21 @@ def update_red_objects(self, red_objects): if 0 <= nx < self.grid_size and 0 <= ny < self.grid_size: self.red_object_grid[nx, ny] = True - def get_best_exploration_target(self, current_pos, red_objects=None): + 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 get_best_exploration_target(self, current_pos, red_objects=None, blue_objects=None): + # 优先检查红色物体 if red_objects and len(red_objects) > 0: nearest_obj = None min_distance = float('inf') @@ -334,6 +372,23 @@ def get_best_exploration_target(self, current_pos, red_objects=None): 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 not self.frontier_cells: angle = random.uniform(0, 2 * math.pi) distance = 10.0 @@ -360,11 +415,15 @@ def get_best_exploration_target(self, current_pos, red_objects=None): 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'] + score = ( config.INTELLIGENT_DECISION['CURIOUSITY_WEIGHT'] * info_gain + (1 - config.INTELLIGENT_DECISION['MEMORY_WEIGHT'] * time_factor) - distance_cost * 0.3 + - red_bonus + red_bonus + blue_bonus # 新增蓝色奖励 ) if score > best_score: @@ -391,7 +450,9 @@ def visualize_grid(self, size=300): elif self.obstacle_grid[x, y]: color = (0, 0, 255) elif self.red_object_grid[x, y]: - color = (0, 100, 255) + color = (0, 100, 255) # 红色物体显示为橙色 + elif self.blue_object_grid[x, y]: + color = (255, 100, 0) # 蓝色物体显示为青色 elif self.grid[x, y] > 0.7: color = (200, 200, 200) elif self.grid[x, y] > 0.3: @@ -452,6 +513,7 @@ def __init__(self, enable_csv=True, enable_json=True, csv_filename=None, json_fi } self.red_objects_detected = [] + self.blue_objects_detected = [] # 新增 self.csv_columns = [ 'timestamp', 'loop_count', 'state', 'pos_x', 'pos_y', 'pos_z', @@ -463,7 +525,8 @@ def __init__(self, enable_csv=True, enable_json=True, csv_filename=None, json_fi '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' + 'red_objects_count', 'red_objects_detected', 'red_objects_visited', + 'blue_objects_count', 'blue_objects_detected', 'blue_objects_visited' # 新增 ] if self.enable_csv: @@ -528,6 +591,28 @@ def record_red_object(self, red_object): 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 + } + + self.blue_objects_detected.append(blue_object_data) + + if 'blue_objects' not in self.json_data: + self.json_data['blue_objects'] = [] + + self.json_data['blue_objects'].append(blue_object_data) + + except Exception as e: + print(f"⚠️ 记录蓝色物体时出错: {e}") + def _collect_system_metrics(self): try: cpu_percent = psutil.cpu_percent(interval=0.1) @@ -576,6 +661,7 @@ def save_json_data(self): 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)) @@ -585,6 +671,16 @@ def save_json_data(self): '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 + } + with open(self.json_filename, 'w', encoding='utf-8') as f: json.dump(self.json_data, f, indent=2, ensure_ascii=False) @@ -666,6 +762,11 @@ def generate_performance_report(self): 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" + report += "="*60 + "\n" warnings = [] @@ -704,6 +805,9 @@ class PerceptionResult: 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 # 新增 def __post_init__(self): if self.safe_directions is None: @@ -712,6 +816,8 @@ def __post_init__(self): self.obstacle_positions = [] if self.red_objects is None: self.red_objects = [] + if self.blue_objects is None: # 新增 + self.blue_objects = [] class VectorFieldPlanner: @@ -722,6 +828,7 @@ def __init__(self): 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.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']) @@ -729,14 +836,19 @@ def __init__(self): 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): + def compute_vector(self, current_pos, goal_pos, obstacles, red_objects=None, blue_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() + if red_objects: red_attraction_vector = self._compute_red_attraction(current_pos, red_objects) - combined_vector = attraction_vector + repulsion_vector + red_attraction_vector + if blue_objects: # 新增 + blue_attraction_vector = self._compute_blue_attraction(current_pos, blue_objects) + + combined_vector = attraction_vector + repulsion_vector + red_attraction_vector + blue_attraction_vector smoothed_vector = self._smooth_vector(combined_vector) limited_vector = self._limit_turn_angle(smoothed_vector) @@ -788,6 +900,22 @@ def _compute_red_attraction(self, current_pos, red_objects): 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 _smooth_vector(self, new_vector): self.vector_history.append(new_vector) @@ -1097,14 +1225,20 @@ def _add_info_overlay(self, image: np.ndarray, info: Dict, manual_info: List[str red_objects_count = info.get('red_objects_count', 0) red_objects_visited = info.get('red_objects_visited', 0) - if red_objects_count > 0: + blue_objects_count = info.get('blue_objects_count', 0) # 新增 + blue_objects_visited = info.get('blue_objects_visited', 0) # 新增 + + if red_objects_count > 0 or blue_objects_count > 0: red_text = f"红色物体: {red_objects_visited}/{red_objects_count}" + blue_text = f"蓝色物体: {blue_objects_visited}/{blue_objects_count}" cv2.putText(image, red_text, (10, 90), cv2.FONT_HERSHEY_SIMPLEX, 0.6, (0, 100, 255), 2) + cv2.putText(image, blue_text, (10, 110), + cv2.FONT_HERSHEY_SIMPLEX, 0.6, (255, 100, 0), 2) decision_info = info.get('decision_info', {}) if decision_info: - y_pos = 120 if red_objects_count > 0 else 90 + y_pos = 130 if red_objects_count > 0 or blue_objects_count > 0 else 90 for key, value in decision_info.items(): if key == 'vector_angle': cv2.putText(image, f"方向: {math.degrees(value):.0f}°", (10, y_pos), @@ -1116,7 +1250,7 @@ def _add_info_overlay(self, image: np.ndarray, info: Dict, manual_info: List[str y_pos += 20 if is_manual and manual_info: - y_start = 130 if red_objects_count > 0 else 100 + y_start = 150 if (red_objects_count > 0 or blue_objects_count > 0) else 100 for i, line in enumerate(manual_info): y_pos = y_start + i * 20 cv2.putText(image, line, (10, y_pos), @@ -1124,7 +1258,7 @@ def _add_info_overlay(self, image: np.ndarray, info: Dict, manual_info: List[str cv2.putText(image, "手动控制中...", (width - 150, 60), cv2.FONT_HERSHEY_SIMPLEX, 0.6, (255, 255, 0), 1) - elif not is_manual and red_objects_count == 0: + elif not is_manual and red_objects_count == 0 and blue_objects_count == 0: obs_dist = info.get('obstacle_distance', 0.0) obs_color = (0, 0, 255) if obs_dist < 5.0 else (0, 165, 255) if obs_dist < 10.0 else (0, 255, 0) cv2.putText(image, f"障碍: {obs_dist:.1f}m", (10, 90), @@ -1179,12 +1313,12 @@ def _save_screenshot(self, image: Optional[np.ndarray]): class PerceptiveExplorer: - """基于感知的自主探索无人机 - 智能决策增强版（红色物体检测版）""" + """基于感知的自主探索无人机 - 智能决策增强版（双色物体检测版）""" def __init__(self, drone_name=""): self._setup_logging() self.logger.info("=" * 60) - self.logger.info("AirSimNH 感知驱动自主探索系统 - 红色物体检测版") + self.logger.info("AirSimNH 感知驱动自主探索系统 - 双色物体检测版") self.logger.info("=" * 60) self.client = None @@ -1240,6 +1374,12 @@ def __init__(self, drone_name=""): 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.visited_positions = deque(maxlen=100) self.loop_count = 0 @@ -1273,6 +1413,8 @@ def __init__(self, drone_name=""): 'min_loop_time': 100.0, 'red_objects_detected': 0, 'red_objects_visited': 0, + 'blue_objects_detected': 0, # 新增 + 'blue_objects_visited': 0, # 新增 } self.front_display = None @@ -1284,11 +1426,13 @@ def __init__(self, drone_name=""): 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" 蓝色物体检测: 已启用") def _setup_logging(self): self.logger = logging.getLogger('DroneExplorer') @@ -1489,7 +1633,7 @@ def _detect_red_objects(self, image: np.ndarray, depth_array: Optional[np.ndarra 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"ID:{red_object.id} ({confidence:.2f})" + label = f"R:{red_object.id} ({confidence:.2f})" cv2.putText(marked_image, label, (x, y-10), cv2.FONT_HERSHEY_SIMPLEX, 0.5, color, 1) @@ -1506,6 +1650,133 @@ def _detect_red_objects(self, image: np.ndarray, depth_array: Optional[np.ndarra 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 _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( @@ -1522,6 +1793,22 @@ def _is_same_object(self, obj1: RedObject, obj2: RedObject, distance_threshold=2 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 _check_red_object_proximity(self, current_pos): for obj in self.red_objects: if not obj.visited: @@ -1552,6 +1839,36 @@ def _check_red_object_proximity(self, current_pos): 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 get_depth_perception(self) -> PerceptionResult: result = PerceptionResult() self.stats['perception_cycles'] += 1 @@ -1649,6 +1966,8 @@ def get_depth_perception(self) -> PerceptionResult: 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 @@ -1656,6 +1975,14 @@ def get_depth_perception(self) -> PerceptionResult: 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 + result.front_image = img_bgr display_info = self._prepare_display_info(result) @@ -1672,7 +1999,15 @@ def get_depth_perception(self) -> PerceptionResult: grid_img = self.exploration_grid.visualize_grid(size=150) additional_images = {'grid': grid_img} if grid_img is not None else {} - display_image = result.red_objects_image if result.red_objects_image is not None else img_bgr + # 合并红色和蓝色物体标记 + display_image = img_bgr.copy() + if config.DISPLAY['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['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.PERFORMANCE['ENABLE_REALTIME_METRICS']: cpu_usage = psutil.cpu_percent(interval=0) @@ -1689,7 +2024,7 @@ def get_depth_perception(self) -> PerceptionResult: 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.open_space_score:.2f}, 红色物体={result.red_objects_count}个, 蓝色物体={result.blue_objects_count}个") except Exception as e: if "ClientException" in str(type(e)) or "Connection" in str(e): @@ -1729,6 +2064,9 @@ def _record_flight_data(self, perception: PerceptionResult): 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) # 新增 + data_dict = { 'timestamp': datetime.now().isoformat(), 'loop_count': self.loop_count, @@ -1759,6 +2097,9 @@ def _record_flight_data(self, perception: PerceptionResult): '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, # 新增 } self.data_logger.record_flight_data(data_dict) @@ -1820,6 +2161,9 @@ def _update_exploration_grid(self, perception: PerceptionResult): 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) + self.stats['grid_updates'] += 1 except Exception as e: @@ -1837,6 +2181,8 @@ def _prepare_display_info(self, perception: PerceptionResult) -> Dict: '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), # 新增 } if hasattr(self, 'last_decision_info'): @@ -1883,6 +2229,10 @@ def _get_manual_control_info(self): 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.manual_control_start > 0: elapsed = time.time() - self.manual_control_start info_lines.append(f"手动模式: {elapsed:.1f}秒") @@ -2071,6 +2421,7 @@ def run_manual_control(self): 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) # 新增 except: pass @@ -2105,6 +2456,7 @@ def run_manual_control(self): 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. 继续自动探索") @@ -2166,6 +2518,9 @@ def run_perception_loop(self): 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) except: pass @@ -2268,7 +2623,8 @@ def make_intelligent_decision(self, perception: PerceptionResult) -> Tuple[float self.exploration_target = self.exploration_grid.get_best_exploration_target( (pos.x_val, pos.y_val), - perception.red_objects + perception.red_objects, + perception.blue_objects # 新增 ) self.target_update_time = current_time @@ -2288,7 +2644,8 @@ def make_intelligent_decision(self, perception: PerceptionResult) -> Tuple[float if self.exploration_target is None: self.exploration_target = self.exploration_grid.get_best_exploration_target( current_pos, - perception.red_objects + perception.red_objects, + perception.blue_objects # 新增 ) self.target_update_time = time.time() @@ -2296,7 +2653,8 @@ def make_intelligent_decision(self, perception: PerceptionResult) -> Tuple[float current_pos, self.exploration_target, perception.obstacle_positions, - perception.red_objects + perception.red_objects, + perception.blue_objects # 新增 ) speed_factor = self._calculate_adaptive_speed(perception, vector.magnitude()) @@ -2318,6 +2676,7 @@ def make_intelligent_decision(self, perception: PerceptionResult) -> Tuple[float '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, # 新增 'decision_time': time.time() - decision_start } @@ -2339,7 +2698,8 @@ def make_intelligent_decision(self, perception: PerceptionResult) -> Tuple[float current_pos, None, perception.obstacle_positions, - perception.red_objects + perception.red_objects, + perception.blue_objects # 新增 ) if avoid_vector.magnitude() > 0.1: @@ -2357,6 +2717,11 @@ def make_intelligent_decision(self, perception: PerceptionResult) -> Tuple[float 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.EMERGENCY: target_vx, target_vy, target_yaw = 0, 0, 0 target_z = max(pos.z_val, -20) @@ -2396,8 +2761,10 @@ def _calculate_adaptive_speed(self, perception: PerceptionResult, vector_magnitu 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 # 新增 + color_factor = min(red_factor, blue_factor) # 新增 - speed_factor = open_factor * obs_factor * vector_factor * red_factor * 0.7 + speed_factor = open_factor * obs_factor * vector_factor * color_factor * 0.7 # 更新 speed_factor = max( config.INTELLIGENT_DECISION['MIN_SPEED_FACTOR'], @@ -2410,7 +2777,8 @@ 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]: + if self.state in [FlightState.EXPLORING, FlightState.AVOIDING, FlightState.PLANNING, + FlightState.RED_OBJECT_INSPECTION, FlightState.BLUE_OBJECT_INSPECTION]: self.client.moveByVelocityZAsync( target_vx, target_vy, target_z, 0.5, drivetrain=airsim.DrivetrainType.MaxDegreeOfFreedom, @@ -2451,6 +2819,8 @@ def _report_status(self, exploration_start, perception): 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" 智能决策: 向量场{self.stats['vector_field_updates']}次 " f"| 网格更新{self.stats['grid_updates']}次") self.logger.info(f" 探索网格: 前沿{len(self.exploration_grid.frontier_cells)}个") @@ -2533,6 +2903,8 @@ def _generate_summary_report(self): 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['vector_field_updates']}") self.logger.info(f" 网格更新次数: {self.stats['grid_updates']}") self.logger.info(f" 探索前沿数量: {len(self.exploration_grid.frontier_cells)}") @@ -2548,7 +2920,7 @@ def _generate_summary_report(self): 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("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") @@ -2562,6 +2934,8 @@ def _generate_summary_report(self): 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['exceptions_caught']}\n") f.write(f"前视图像更新次数: {self.stats['front_image_updates']}\n") f.write(f"平均循环时间: {self.stats['average_loop_time']*1000:.1f}ms\n") @@ -2616,7 +2990,7 @@ def emergency_stop(self): def main(): print("=" * 70) - print("AirSimNH 无人机感知探索系统 - 智能决策增强版（红色物体检测版）") + 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']}") @@ -2628,17 +3002,18 @@ def main(): print(" • PID平滑飞行控制") 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(" 1. 智能探索模式 (AI自主决策，包含双色物体检测)") print(" 2. 手动控制模式 (键盘控制)") print(" 3. 混合模式 (先自动探索，后可切换)") print("=" * 50) @@ -2659,7 +3034,7 @@ def signal_handler(sig, frame): if mode_choice == '1': print("\n" + "="*50) - print("启动智能探索模式（含红色物体检测）") + print("启动智能探索模式（含双色物体检测）") print("="*50) explorer.run_perception_loop() @@ -2683,7 +3058,7 @@ def signal_handler(sig, frame): print("启动混合模式") print("="*50) - explorer.logger.info("🔍 开始智能探索（含红色物体检测）...") + explorer.logger.info("🔍 开始智能探索（含双色物体检测）...") original_time = config.EXPLORATION['TOTAL_TIME'] explorer.exploration_time = min(60, original_time) @@ -2693,7 +3068,7 @@ def signal_handler(sig, frame): print("\n" + "="*50) print("智能探索阶段结束") print(f"检测到红色物体: {explorer.stats['red_objects_detected']}个") - print(f"已访问红色物体: {explorer.stats['red_objects_visited']}个") + print(f"检测到蓝色物体: {explorer.stats['blue_objects_detected']}个") print("请选择下一步:") print(" 1. 进入手动控制模式") print(" 2. 继续智能探索") From f0aaf0270f92640124238efac27c0a602ef7f202 Mon Sep 17 00:00:00 2001 From: rhcgg <3136924319@qq.com> Date: Mon, 22 Dec 2025 10:35:07 +0800 Subject: [PATCH 13/23] =?UTF-8?q?=E5=8A=9F=E8=83=BD=E8=BF=9B=E4=B8=80?= =?UTF-8?q?=E6=AD=A5=E5=AE=8C=E5=96=84?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit 主要新增功能说明 蓝色物体数据结构： 新增了BlueObject类，与RedObject结构相同 在PerceptionResult中添加了蓝色物体相关字段 蓝色物体检测算法： 新增了_detect_blue_objects方法，使用HSV颜色空间检测蓝色物体 蓝色物体在画面中用青色(255, 100, 0)框标记 智能决策扩展： 向量场规划器添加了蓝色物体吸引力计算 探索网格支持蓝色物体标记和吸引力计算 决策时优先考虑红色物体，其次蓝色物体 数据显示和记录： 前视窗口显示红色和蓝色物体的统计信息 数据记录器记录蓝色物体的检测和访问信息 性能报告包含蓝色物体统计 配置文件更新： 添加了蓝色物体检测参数 增加了蓝色物体探索的智能决策参数 添加了蓝色物体的HSV颜色范围配置 --- src/drone_perception/config.py | 26 + .../perceptive_drone_explorer.py | 451 ++++++++++++++++-- 2 files changed, 439 insertions(+), 38 deletions(-) diff --git a/src/drone_perception/config.py b/src/drone_perception/config.py index 3c950b3fc7..9e53e27276 100644 --- a/src/drone_perception/config.py +++ b/src/drone_perception/config.py @@ -41,6 +41,15 @@ '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, # 物体记忆时间（秒） } } @@ -85,6 +94,14 @@ '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, # 探索奖励分数 } } @@ -110,6 +127,7 @@ 'REFRESH_RATE_MS': 30, # 刷新率 (毫秒)，建议30-50 'GRID_DISPLAY_SIZE': 150, # 探索网格显示大小 (像素) 'SHOW_RED_OBJECTS': True, # 在画面中标记红色物体 + 'SHOW_BLUE_OBJECTS': True, # 在画面中标记蓝色物体（新增） } # ==================== 系统与安全参数 ==================== @@ -142,6 +160,12 @@ '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], # 蓝色上限 } } @@ -154,6 +178,7 @@ 'LOG_VECTOR_FIELD': False, # 是否记录向量场详细信息 'PERFORMANCE_PROFILING': False, # 是否启用性能分析 'SAVE_RED_OBJECT_IMAGES': False, # 是否保存检测到红色物体的图像 + 'SAVE_BLUE_OBJECT_IMAGES': False, # 是否保存检测到蓝色物体的图像（新增） } # ==================== 数据记录参数 ==================== @@ -167,6 +192,7 @@ 'PERFORMANCE_MONITORING': True, # 启用性能监控 'SYSTEM_METRICS_INTERVAL': 5.0, # 系统指标记录间隔（秒） 'RECORD_RED_OBJECTS': True, # 记录红色物体信息 + 'RECORD_BLUE_OBJECTS': True, # 记录蓝色物体信息（新增） } # ==================== 性能监控参数 ==================== diff --git a/src/drone_perception/perceptive_drone_explorer.py b/src/drone_perception/perceptive_drone_explorer.py index a99c3c8bf9..292541a1a1 100644 --- a/src/drone_perception/perceptive_drone_explorer.py +++ b/src/drone_perception/perceptive_drone_explorer.py @@ -1,10 +1,10 @@ """ -AirSimNH 感知驱动自主探索无人机 - 智能决策增强版（红色物体检测版） +AirSimNH 感知驱动自主探索无人机 - 智能决策增强版（红色与蓝色物体检测版） 核心：视觉感知 → 语义理解 → 智能决策 → 安全执行 集成：配置管理、日志系统、异常恢复、前视窗口显示 新增：向量场避障算法、基于网格的信息增益探索、平滑飞行控制 -新增：性能监控与数据闭环系统、红色物体检测与记录 -版本: 3.3 (红色物体检测版) +新增：性能监控与数据闭环系统、红色与蓝色物体检测与记录 +版本: 3.4 (双色物体检测版) """ import airsim @@ -48,15 +48,20 @@ class DefaultConfig: 'OPENNESS_THRESHOLD': 0.7}, 'RED_OBJECT_DETECTION': {'ENABLED': True, 'MIN_AREA': 50, 'MAX_AREA': 10000, 'UPDATE_INTERVAL': 1.0, - 'MEMORY_TIME': 5.0}} + 'MEMORY_TIME': 5.0}, + 'BLUE_OBJECT_DETECTION': {'ENABLED': True, 'MIN_AREA': 50, + 'MAX_AREA': 10000, 'UPDATE_INTERVAL': 1.0, + 'MEMORY_TIME': 5.0}} DISPLAY = {'WINDOW_WIDTH': 640, 'WINDOW_HEIGHT': 480, 'ENABLE_SHARPENING': True, - 'SHOW_INFO_OVERLAY': True, 'REFRESH_RATE_MS': 30, 'SHOW_RED_OBJECTS': True} + 'SHOW_INFO_OVERLAY': True, 'REFRESH_RATE_MS': 30, 'SHOW_RED_OBJECTS': True, + 'SHOW_BLUE_OBJECTS': True} 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]}} + 'LOWER2': [170, 120, 70], 'UPPER2': [180, 255, 255]}, + 'BLUE_COLOR_RANGE': {'LOWER': [100, 150, 50], 'UPPER': [130, 255, 255]}} MANUAL = { 'CONTROL_SPEED': 3.0, 'ALTITUDE_SPEED': 2.0, @@ -91,13 +96,16 @@ class DefaultConfig: '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} + '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} } DEBUG = { 'SAVE_PERCEPTION_IMAGES': False, 'IMAGE_SAVE_INTERVAL': 50, 'LOG_DECISION_DETAILS': False, - 'SAVE_RED_OBJECT_IMAGES': False + 'SAVE_RED_OBJECT_IMAGES': False, + 'SAVE_BLUE_OBJECT_IMAGES': False } DATA_RECORDING = { 'ENABLED': True, @@ -108,7 +116,8 @@ class DefaultConfig: 'JSON_FILENAME': 'flight_data.json', 'PERFORMANCE_MONITORING': True, 'SYSTEM_METRICS_INTERVAL': 5.0, - 'RECORD_RED_OBJECTS': True + 'RECORD_RED_OBJECTS': True, + 'RECORD_BLUE_OBJECTS': True } PERFORMANCE = { 'ENABLE_REALTIME_METRICS': True, @@ -133,6 +142,7 @@ class FlightState(Enum): MANUAL = "手动控制" PLANNING = "路径规划" RED_OBJECT_INSPECTION = "红色物体检查" + BLUE_OBJECT_INSPECTION = "蓝色物体检查" # 新增 @dataclass @@ -148,6 +158,19 @@ class RedObject: 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 + + class Vector2D: """二维向量类""" def __init__(self, x=0.0, y=0.0): @@ -232,6 +255,7 @@ def __init__(self, resolution=2.0, grid_size=50): 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.current_idx = (self.half_size, self.half_size) self.frontier_cells = set() @@ -317,7 +341,21 @@ def update_red_objects(self, red_objects): if 0 <= nx < self.grid_size and 0 <= ny < self.grid_size: self.red_object_grid[nx, ny] = True - def get_best_exploration_target(self, current_pos, red_objects=None): + 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 get_best_exploration_target(self, current_pos, red_objects=None, blue_objects=None): + # 优先检查红色物体 if red_objects and len(red_objects) > 0: nearest_obj = None min_distance = float('inf') @@ -334,6 +372,23 @@ def get_best_exploration_target(self, current_pos, red_objects=None): 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 not self.frontier_cells: angle = random.uniform(0, 2 * math.pi) distance = 10.0 @@ -360,11 +415,15 @@ def get_best_exploration_target(self, current_pos, red_objects=None): 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'] + score = ( config.INTELLIGENT_DECISION['CURIOUSITY_WEIGHT'] * info_gain + (1 - config.INTELLIGENT_DECISION['MEMORY_WEIGHT'] * time_factor) - distance_cost * 0.3 + - red_bonus + red_bonus + blue_bonus # 新增蓝色奖励 ) if score > best_score: @@ -391,7 +450,9 @@ def visualize_grid(self, size=300): elif self.obstacle_grid[x, y]: color = (0, 0, 255) elif self.red_object_grid[x, y]: - color = (0, 100, 255) + color = (0, 100, 255) # 红色物体显示为橙色 + elif self.blue_object_grid[x, y]: + color = (255, 100, 0) # 蓝色物体显示为青色 elif self.grid[x, y] > 0.7: color = (200, 200, 200) elif self.grid[x, y] > 0.3: @@ -452,6 +513,7 @@ def __init__(self, enable_csv=True, enable_json=True, csv_filename=None, json_fi } self.red_objects_detected = [] + self.blue_objects_detected = [] # 新增 self.csv_columns = [ 'timestamp', 'loop_count', 'state', 'pos_x', 'pos_y', 'pos_z', @@ -463,7 +525,8 @@ def __init__(self, enable_csv=True, enable_json=True, csv_filename=None, json_fi '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' + 'red_objects_count', 'red_objects_detected', 'red_objects_visited', + 'blue_objects_count', 'blue_objects_detected', 'blue_objects_visited' # 新增 ] if self.enable_csv: @@ -528,6 +591,28 @@ def record_red_object(self, red_object): 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 + } + + self.blue_objects_detected.append(blue_object_data) + + if 'blue_objects' not in self.json_data: + self.json_data['blue_objects'] = [] + + self.json_data['blue_objects'].append(blue_object_data) + + except Exception as e: + print(f"⚠️ 记录蓝色物体时出错: {e}") + def _collect_system_metrics(self): try: cpu_percent = psutil.cpu_percent(interval=0.1) @@ -576,6 +661,7 @@ def save_json_data(self): 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)) @@ -585,6 +671,16 @@ def save_json_data(self): '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 + } + with open(self.json_filename, 'w', encoding='utf-8') as f: json.dump(self.json_data, f, indent=2, ensure_ascii=False) @@ -666,6 +762,11 @@ def generate_performance_report(self): 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" + report += "="*60 + "\n" warnings = [] @@ -704,6 +805,9 @@ class PerceptionResult: 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 # 新增 def __post_init__(self): if self.safe_directions is None: @@ -712,6 +816,8 @@ def __post_init__(self): self.obstacle_positions = [] if self.red_objects is None: self.red_objects = [] + if self.blue_objects is None: # 新增 + self.blue_objects = [] class VectorFieldPlanner: @@ -722,6 +828,7 @@ def __init__(self): 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.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']) @@ -729,14 +836,19 @@ def __init__(self): 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): + def compute_vector(self, current_pos, goal_pos, obstacles, red_objects=None, blue_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() + if red_objects: red_attraction_vector = self._compute_red_attraction(current_pos, red_objects) - combined_vector = attraction_vector + repulsion_vector + red_attraction_vector + if blue_objects: # 新增 + blue_attraction_vector = self._compute_blue_attraction(current_pos, blue_objects) + + combined_vector = attraction_vector + repulsion_vector + red_attraction_vector + blue_attraction_vector smoothed_vector = self._smooth_vector(combined_vector) limited_vector = self._limit_turn_angle(smoothed_vector) @@ -788,6 +900,22 @@ def _compute_red_attraction(self, current_pos, red_objects): 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 _smooth_vector(self, new_vector): self.vector_history.append(new_vector) @@ -1097,14 +1225,20 @@ def _add_info_overlay(self, image: np.ndarray, info: Dict, manual_info: List[str red_objects_count = info.get('red_objects_count', 0) red_objects_visited = info.get('red_objects_visited', 0) - if red_objects_count > 0: + blue_objects_count = info.get('blue_objects_count', 0) # 新增 + blue_objects_visited = info.get('blue_objects_visited', 0) # 新增 + + if red_objects_count > 0 or blue_objects_count > 0: red_text = f"红色物体: {red_objects_visited}/{red_objects_count}" + blue_text = f"蓝色物体: {blue_objects_visited}/{blue_objects_count}" cv2.putText(image, red_text, (10, 90), cv2.FONT_HERSHEY_SIMPLEX, 0.6, (0, 100, 255), 2) + cv2.putText(image, blue_text, (10, 110), + cv2.FONT_HERSHEY_SIMPLEX, 0.6, (255, 100, 0), 2) decision_info = info.get('decision_info', {}) if decision_info: - y_pos = 120 if red_objects_count > 0 else 90 + y_pos = 130 if red_objects_count > 0 or blue_objects_count > 0 else 90 for key, value in decision_info.items(): if key == 'vector_angle': cv2.putText(image, f"方向: {math.degrees(value):.0f}°", (10, y_pos), @@ -1116,7 +1250,7 @@ def _add_info_overlay(self, image: np.ndarray, info: Dict, manual_info: List[str y_pos += 20 if is_manual and manual_info: - y_start = 130 if red_objects_count > 0 else 100 + y_start = 150 if (red_objects_count > 0 or blue_objects_count > 0) else 100 for i, line in enumerate(manual_info): y_pos = y_start + i * 20 cv2.putText(image, line, (10, y_pos), @@ -1124,7 +1258,7 @@ def _add_info_overlay(self, image: np.ndarray, info: Dict, manual_info: List[str cv2.putText(image, "手动控制中...", (width - 150, 60), cv2.FONT_HERSHEY_SIMPLEX, 0.6, (255, 255, 0), 1) - elif not is_manual and red_objects_count == 0: + elif not is_manual and red_objects_count == 0 and blue_objects_count == 0: obs_dist = info.get('obstacle_distance', 0.0) obs_color = (0, 0, 255) if obs_dist < 5.0 else (0, 165, 255) if obs_dist < 10.0 else (0, 255, 0) cv2.putText(image, f"障碍: {obs_dist:.1f}m", (10, 90), @@ -1179,12 +1313,12 @@ def _save_screenshot(self, image: Optional[np.ndarray]): class PerceptiveExplorer: - """基于感知的自主探索无人机 - 智能决策增强版（红色物体检测版）""" + """基于感知的自主探索无人机 - 智能决策增强版（双色物体检测版）""" def __init__(self, drone_name=""): self._setup_logging() self.logger.info("=" * 60) - self.logger.info("AirSimNH 感知驱动自主探索系统 - 红色物体检测版") + self.logger.info("AirSimNH 感知驱动自主探索系统 - 双色物体检测版") self.logger.info("=" * 60) self.client = None @@ -1240,6 +1374,12 @@ def __init__(self, drone_name=""): 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.visited_positions = deque(maxlen=100) self.loop_count = 0 @@ -1273,6 +1413,8 @@ def __init__(self, drone_name=""): 'min_loop_time': 100.0, 'red_objects_detected': 0, 'red_objects_visited': 0, + 'blue_objects_detected': 0, # 新增 + 'blue_objects_visited': 0, # 新增 } self.front_display = None @@ -1284,11 +1426,13 @@ def __init__(self, drone_name=""): 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" 蓝色物体检测: 已启用") def _setup_logging(self): self.logger = logging.getLogger('DroneExplorer') @@ -1489,7 +1633,7 @@ def _detect_red_objects(self, image: np.ndarray, depth_array: Optional[np.ndarra 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"ID:{red_object.id} ({confidence:.2f})" + label = f"R:{red_object.id} ({confidence:.2f})" cv2.putText(marked_image, label, (x, y-10), cv2.FONT_HERSHEY_SIMPLEX, 0.5, color, 1) @@ -1506,6 +1650,133 @@ def _detect_red_objects(self, image: np.ndarray, depth_array: Optional[np.ndarra 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 _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( @@ -1522,6 +1793,22 @@ def _is_same_object(self, obj1: RedObject, obj2: RedObject, distance_threshold=2 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 _check_red_object_proximity(self, current_pos): for obj in self.red_objects: if not obj.visited: @@ -1552,6 +1839,36 @@ def _check_red_object_proximity(self, current_pos): 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 get_depth_perception(self) -> PerceptionResult: result = PerceptionResult() self.stats['perception_cycles'] += 1 @@ -1649,6 +1966,8 @@ def get_depth_perception(self) -> PerceptionResult: 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 @@ -1656,6 +1975,14 @@ def get_depth_perception(self) -> PerceptionResult: 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 + result.front_image = img_bgr display_info = self._prepare_display_info(result) @@ -1672,7 +1999,15 @@ def get_depth_perception(self) -> PerceptionResult: grid_img = self.exploration_grid.visualize_grid(size=150) additional_images = {'grid': grid_img} if grid_img is not None else {} - display_image = result.red_objects_image if result.red_objects_image is not None else img_bgr + # 合并红色和蓝色物体标记 + display_image = img_bgr.copy() + if config.DISPLAY['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['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.PERFORMANCE['ENABLE_REALTIME_METRICS']: cpu_usage = psutil.cpu_percent(interval=0) @@ -1689,7 +2024,7 @@ def get_depth_perception(self) -> PerceptionResult: 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.open_space_score:.2f}, 红色物体={result.red_objects_count}个, 蓝色物体={result.blue_objects_count}个") except Exception as e: if "ClientException" in str(type(e)) or "Connection" in str(e): @@ -1729,6 +2064,9 @@ def _record_flight_data(self, perception: PerceptionResult): 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) # 新增 + data_dict = { 'timestamp': datetime.now().isoformat(), 'loop_count': self.loop_count, @@ -1759,6 +2097,9 @@ def _record_flight_data(self, perception: PerceptionResult): '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, # 新增 } self.data_logger.record_flight_data(data_dict) @@ -1820,6 +2161,9 @@ def _update_exploration_grid(self, perception: PerceptionResult): 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) + self.stats['grid_updates'] += 1 except Exception as e: @@ -1837,6 +2181,8 @@ def _prepare_display_info(self, perception: PerceptionResult) -> Dict: '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), # 新增 } if hasattr(self, 'last_decision_info'): @@ -1883,6 +2229,10 @@ def _get_manual_control_info(self): 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.manual_control_start > 0: elapsed = time.time() - self.manual_control_start info_lines.append(f"手动模式: {elapsed:.1f}秒") @@ -2071,6 +2421,7 @@ def run_manual_control(self): 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) # 新增 except: pass @@ -2105,6 +2456,7 @@ def run_manual_control(self): 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. 继续自动探索") @@ -2166,6 +2518,9 @@ def run_perception_loop(self): 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) except: pass @@ -2268,7 +2623,8 @@ def make_intelligent_decision(self, perception: PerceptionResult) -> Tuple[float self.exploration_target = self.exploration_grid.get_best_exploration_target( (pos.x_val, pos.y_val), - perception.red_objects + perception.red_objects, + perception.blue_objects # 新增 ) self.target_update_time = current_time @@ -2288,7 +2644,8 @@ def make_intelligent_decision(self, perception: PerceptionResult) -> Tuple[float if self.exploration_target is None: self.exploration_target = self.exploration_grid.get_best_exploration_target( current_pos, - perception.red_objects + perception.red_objects, + perception.blue_objects # 新增 ) self.target_update_time = time.time() @@ -2296,7 +2653,8 @@ def make_intelligent_decision(self, perception: PerceptionResult) -> Tuple[float current_pos, self.exploration_target, perception.obstacle_positions, - perception.red_objects + perception.red_objects, + perception.blue_objects # 新增 ) speed_factor = self._calculate_adaptive_speed(perception, vector.magnitude()) @@ -2318,6 +2676,7 @@ def make_intelligent_decision(self, perception: PerceptionResult) -> Tuple[float '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, # 新增 'decision_time': time.time() - decision_start } @@ -2339,7 +2698,8 @@ def make_intelligent_decision(self, perception: PerceptionResult) -> Tuple[float current_pos, None, perception.obstacle_positions, - perception.red_objects + perception.red_objects, + perception.blue_objects # 新增 ) if avoid_vector.magnitude() > 0.1: @@ -2357,6 +2717,11 @@ def make_intelligent_decision(self, perception: PerceptionResult) -> Tuple[float 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.EMERGENCY: target_vx, target_vy, target_yaw = 0, 0, 0 target_z = max(pos.z_val, -20) @@ -2396,8 +2761,10 @@ def _calculate_adaptive_speed(self, perception: PerceptionResult, vector_magnitu 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 # 新增 + color_factor = min(red_factor, blue_factor) # 新增 - speed_factor = open_factor * obs_factor * vector_factor * red_factor * 0.7 + speed_factor = open_factor * obs_factor * vector_factor * color_factor * 0.7 # 更新 speed_factor = max( config.INTELLIGENT_DECISION['MIN_SPEED_FACTOR'], @@ -2410,7 +2777,8 @@ 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]: + if self.state in [FlightState.EXPLORING, FlightState.AVOIDING, FlightState.PLANNING, + FlightState.RED_OBJECT_INSPECTION, FlightState.BLUE_OBJECT_INSPECTION]: self.client.moveByVelocityZAsync( target_vx, target_vy, target_z, 0.5, drivetrain=airsim.DrivetrainType.MaxDegreeOfFreedom, @@ -2451,6 +2819,8 @@ def _report_status(self, exploration_start, perception): 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" 智能决策: 向量场{self.stats['vector_field_updates']}次 " f"| 网格更新{self.stats['grid_updates']}次") self.logger.info(f" 探索网格: 前沿{len(self.exploration_grid.frontier_cells)}个") @@ -2533,6 +2903,8 @@ def _generate_summary_report(self): 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['vector_field_updates']}") self.logger.info(f" 网格更新次数: {self.stats['grid_updates']}") self.logger.info(f" 探索前沿数量: {len(self.exploration_grid.frontier_cells)}") @@ -2548,7 +2920,7 @@ def _generate_summary_report(self): 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("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") @@ -2562,6 +2934,8 @@ def _generate_summary_report(self): 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['exceptions_caught']}\n") f.write(f"前视图像更新次数: {self.stats['front_image_updates']}\n") f.write(f"平均循环时间: {self.stats['average_loop_time']*1000:.1f}ms\n") @@ -2616,7 +2990,7 @@ def emergency_stop(self): def main(): print("=" * 70) - print("AirSimNH 无人机感知探索系统 - 智能决策增强版（红色物体检测版）") + 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']}") @@ -2628,17 +3002,18 @@ def main(): print(" • PID平滑飞行控制") 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(" 1. 智能探索模式 (AI自主决策，包含双色物体检测)") print(" 2. 手动控制模式 (键盘控制)") print(" 3. 混合模式 (先自动探索，后可切换)") print("=" * 50) @@ -2659,7 +3034,7 @@ def signal_handler(sig, frame): if mode_choice == '1': print("\n" + "="*50) - print("启动智能探索模式（含红色物体检测）") + print("启动智能探索模式（含双色物体检测）") print("="*50) explorer.run_perception_loop() @@ -2683,7 +3058,7 @@ def signal_handler(sig, frame): print("启动混合模式") print("="*50) - explorer.logger.info("🔍 开始智能探索（含红色物体检测）...") + explorer.logger.info("🔍 开始智能探索（含双色物体检测）...") original_time = config.EXPLORATION['TOTAL_TIME'] explorer.exploration_time = min(60, original_time) @@ -2693,7 +3068,7 @@ def signal_handler(sig, frame): print("\n" + "="*50) print("智能探索阶段结束") print(f"检测到红色物体: {explorer.stats['red_objects_detected']}个") - print(f"已访问红色物体: {explorer.stats['red_objects_visited']}个") + print(f"检测到蓝色物体: {explorer.stats['blue_objects_detected']}个") print("请选择下一步:") print(" 1. 进入手动控制模式") print(" 2. 继续智能探索") From 9895ea12dacf77b10914f470620b68270a43e80e Mon Sep 17 00:00:00 2001 From: rhcgg <3136924319@qq.com> Date: Mon, 22 Dec 2025 11:44:41 +0800 Subject: [PATCH 14/23] =?UTF-8?q?=E6=B7=BB=E5=8A=A0=E4=B8=80=E4=B8=AA?= =?UTF-8?q?=E4=BF=A1=E6=81=AF=E7=AA=97=E5=8F=A3=EF=BC=8C=E5=88=86=E6=8B=85?= =?UTF-8?q?=E5=89=8D=E8=A7=86=E7=AA=97=E5=8F=A3=E5=8E=8B=E5=8A=9B?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit 双窗口设计使得系统更加模块化，前视窗口专注于飞行操作，信息窗口专注于系统监控，提高了用户体验和系统可维护性。 --- src/drone_perception/config.py | 39 +- .../perceptive_drone_explorer.py | 713 +++++++++++++----- 2 files changed, 570 insertions(+), 182 deletions(-) diff --git a/src/drone_perception/config.py b/src/drone_perception/config.py index 9e53e27276..550cefc4ff 100644 --- a/src/drone_perception/config.py +++ b/src/drone_perception/config.py @@ -118,16 +118,37 @@ 'MAX_ALTITUDE_LIMIT': -30.0, # 最高飞行高度限制 } -# ==================== 前视窗口参数 ==================== +# ==================== 窗口显示参数 ==================== DISPLAY = { - 'WINDOW_WIDTH': 640, # 窗口宽度 (像素) - 'WINDOW_HEIGHT': 480, # 窗口高度 (像素) - 'ENABLE_SHARPENING': True, # 启用图像锐化，改善模糊 - 'SHOW_INFO_OVERLAY': True, # 显示信息叠加层 - 'REFRESH_RATE_MS': 30, # 刷新率 (毫秒)，建议30-50 - 'GRID_DISPLAY_SIZE': 150, # 探索网格显示大小 (像素) - 'SHOW_RED_OBJECTS': True, # 在画面中标记红色物体 - 'SHOW_BLUE_OBJECTS': True, # 在画面中标记蓝色物体（新增） + # 前视窗口参数 + '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, # 在画面中标记蓝色物体 + }, + + # 信息显示窗口参数 + '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, # 显示性能信息 + } } # ==================== 系统与安全参数 ==================== diff --git a/src/drone_perception/perceptive_drone_explorer.py b/src/drone_perception/perceptive_drone_explorer.py index 292541a1a1..61249b2357 100644 --- a/src/drone_perception/perceptive_drone_explorer.py +++ b/src/drone_perception/perceptive_drone_explorer.py @@ -4,7 +4,8 @@ 集成：配置管理、日志系统、异常恢复、前视窗口显示 新增：向量场避障算法、基于网格的信息增益探索、平滑飞行控制 新增：性能监控与数据闭环系统、红色与蓝色物体检测与记录 -版本: 3.4 (双色物体检测版) +新增：信息显示窗口，分离前视画面与系统信息 +版本: 3.5 (双窗口双色物体检测版) """ import airsim @@ -52,9 +53,17 @@ class DefaultConfig: 'BLUE_OBJECT_DETECTION': {'ENABLED': True, 'MIN_AREA': 50, 'MAX_AREA': 10000, 'UPDATE_INTERVAL': 1.0, 'MEMORY_TIME': 5.0}} - DISPLAY = {'WINDOW_WIDTH': 640, 'WINDOW_HEIGHT': 480, 'ENABLE_SHARPENING': True, - 'SHOW_INFO_OVERLAY': True, 'REFRESH_RATE_MS': 30, 'SHOW_RED_OBJECTS': True, - 'SHOW_BLUE_OBJECTS': True} + 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}, + '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}} 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} @@ -142,7 +151,7 @@ class FlightState(Enum): MANUAL = "手动控制" PLANNING = "路径规划" RED_OBJECT_INSPECTION = "红色物体检查" - BLUE_OBJECT_INSPECTION = "蓝色物体检查" # 新增 + BLUE_OBJECT_INSPECTION = "蓝色物体检查" @dataclass @@ -159,7 +168,7 @@ class RedObject: @dataclass -class BlueObject: # 新增蓝色物体数据结构 +class BlueObject: """蓝色物体数据结构""" id: int position: Tuple[float, float, float] @@ -255,7 +264,7 @@ def __init__(self, resolution=2.0, grid_size=50): 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.blue_object_grid = np.zeros((grid_size, grid_size), dtype=bool) self.current_idx = (self.half_size, self.half_size) self.frontier_cells = set() @@ -341,7 +350,7 @@ def update_red_objects(self, red_objects): 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): # 新增 + def update_blue_objects(self, blue_objects): self.blue_object_grid.fill(False) for obj in blue_objects: @@ -373,7 +382,7 @@ def get_best_exploration_target(self, current_pos, red_objects=None, blue_object return (nearest_obj.position[0], nearest_obj.position[1]) # 其次检查蓝色物体 - if blue_objects and len(blue_objects) > 0: # 新增 + if blue_objects and len(blue_objects) > 0: nearest_obj = None min_distance = float('inf') current_x, current_y = current_pos @@ -415,7 +424,7 @@ def get_best_exploration_target(self, current_pos, red_objects=None, blue_object if self.red_object_grid[fx, fy]: red_bonus = config.INTELLIGENT_DECISION['RED_OBJECT_EXPLORATION']['EXPLORATION_BONUS'] - blue_bonus = 0.0 # 新增 + blue_bonus = 0.0 if self.blue_object_grid[fx, fy]: blue_bonus = config.INTELLIGENT_DECISION['BLUE_OBJECT_EXPLORATION']['EXPLORATION_BONUS'] @@ -423,7 +432,7 @@ def get_best_exploration_target(self, current_pos, red_objects=None, blue_object config.INTELLIGENT_DECISION['CURIOUSITY_WEIGHT'] * info_gain + (1 - config.INTELLIGENT_DECISION['MEMORY_WEIGHT'] * time_factor) - distance_cost * 0.3 + - red_bonus + blue_bonus # 新增蓝色奖励 + red_bonus + blue_bonus ) if score > best_score: @@ -513,7 +522,7 @@ def __init__(self, enable_csv=True, enable_json=True, csv_filename=None, json_fi } self.red_objects_detected = [] - self.blue_objects_detected = [] # 新增 + self.blue_objects_detected = [] self.csv_columns = [ 'timestamp', 'loop_count', 'state', 'pos_x', 'pos_y', 'pos_z', @@ -526,7 +535,7 @@ def __init__(self, enable_csv=True, enable_json=True, csv_filename=None, json_fi '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' # 新增 + 'blue_objects_count', 'blue_objects_detected', 'blue_objects_visited' ] if self.enable_csv: @@ -591,7 +600,7 @@ def record_red_object(self, red_object): except Exception as e: print(f"⚠️ 记录红色物体时出错: {e}") - def record_blue_object(self, blue_object): # 新增 + def record_blue_object(self, blue_object): try: blue_object_data = { 'id': blue_object.id, @@ -671,7 +680,7 @@ def save_json_data(self): '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)) @@ -762,7 +771,7 @@ def generate_performance_report(self): 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: # 新增 + 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" @@ -805,9 +814,9 @@ class PerceptionResult: 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 # 新增 + blue_objects: List[BlueObject] = None + blue_objects_count: int = 0 + blue_objects_image: Optional[np.ndarray] = None def __post_init__(self): if self.safe_directions is None: @@ -816,7 +825,7 @@ def __post_init__(self): self.obstacle_positions = [] if self.red_objects is None: self.red_objects = [] - if self.blue_objects is None: # 新增 + if self.blue_objects is None: self.blue_objects = [] @@ -828,7 +837,7 @@ def __init__(self): 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.blue_attraction_gain = config.INTELLIGENT_DECISION['BLUE_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']) @@ -845,7 +854,7 @@ def compute_vector(self, current_pos, goal_pos, obstacles, red_objects=None, blu if red_objects: red_attraction_vector = self._compute_red_attraction(current_pos, red_objects) - if blue_objects: # 新增 + if blue_objects: blue_attraction_vector = self._compute_blue_attraction(current_pos, blue_objects) combined_vector = attraction_vector + repulsion_vector + red_attraction_vector + blue_attraction_vector @@ -900,7 +909,7 @@ def _compute_red_attraction(self, current_pos, red_objects): return attraction - def _compute_blue_attraction(self, current_pos, blue_objects): # 新增 + def _compute_blue_attraction(self, current_pos, blue_objects): attraction = Vector2D() for obj in blue_objects: @@ -956,18 +965,18 @@ def _limit_turn_angle(self, vector): return Vector2D.from_angle(limited_angle, magnitude) -class FrontViewDisplay: - """前视画面显示管理器""" +class FrontViewWindow: + """前视窗口 - 显示摄像头画面和手动控制""" - def __init__(self, window_name="无人机前视画面", width=None, height=None, + def __init__(self, window_name=None, width=None, height=None, enable_sharpening=None, show_info=None): - self.window_name = window_name - self.window_width = width if width is not None else config.DISPLAY['WINDOW_WIDTH'] - self.window_height = height if height is not None else config.DISPLAY['WINDOW_HEIGHT'] + self.window_name = window_name if window_name else config.DISPLAY['FRONT_VIEW_WINDOW']['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['ENABLE_SHARPENING']) + else config.DISPLAY['FRONT_VIEW_WINDOW']['ENABLE_SHARPENING']) self.show_info = (show_info if show_info is not None - else config.DISPLAY['SHOW_INFO_OVERLAY']) + else config.DISPLAY['FRONT_VIEW_WINDOW']['SHOW_INFO_OVERLAY']) self.image_queue = queue.Queue(maxsize=3) self.display_active = True @@ -987,12 +996,6 @@ def __init__(self, window_name="无人机前视画面", width=None, height=None, 'frame_count': 0 } - self.performance_info = { - 'cpu_usage': 0.0, - 'memory_usage': 0.0, - 'update_time': time.time() - } - self.start() def start(self): @@ -1003,7 +1006,7 @@ def start(self): self.display_thread = threading.Thread( target=self._display_loop, daemon=True, - name="FrontViewDisplay" + name="FrontViewWindow" ) self.display_thread.start() @@ -1014,8 +1017,7 @@ def stop(self): 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, - additional_images: Optional[Dict] = None): + manual_info: Optional[List[str]] = None): if not self.display_active or self.paused or image_data is None: return @@ -1036,7 +1038,6 @@ def update_image(self, image_data: np.ndarray, info: Optional[Dict] = None, 'image': image_data.copy(), 'info': info.copy() if info else {}, 'manual_info': manual_info.copy() if manual_info else [], - 'additional_images': additional_images.copy() if additional_images else {}, 'timestamp': time.time() } @@ -1045,11 +1046,6 @@ def update_image(self, image_data: np.ndarray, info: Optional[Dict] = None, except Exception as e: print(f"⚠️ 更新图像时出错: {e}") - def update_performance_info(self, cpu_usage, memory_usage): - self.performance_info['cpu_usage'] = cpu_usage - self.performance_info['memory_usage'] = memory_usage - self.performance_info['update_time'] = time.time() - def set_manual_mode(self, manual_mode): self.manual_mode = manual_mode self.exit_manual_flag = False @@ -1086,7 +1082,6 @@ def _display_loop(self): display_image = None info = {} manual_info = [] - additional_images = {} try: if not self.image_queue.empty(): @@ -1094,7 +1089,6 @@ def _display_loop(self): display_image = packet['image'] info = packet['info'] manual_info = packet['manual_info'] - additional_images = packet.get('additional_images', {}) self._update_stats() @@ -1108,7 +1102,7 @@ def _display_loop(self): if display_image is not None: if self.show_info: - display_image = self._add_info_overlay(display_image, info, manual_info, additional_images) + display_image = self._add_info_overlay(display_image, info, manual_info) cv2.imshow(self.window_name, display_image) elif self.paused: @@ -1117,7 +1111,7 @@ def _display_loop(self): cv2.FONT_HERSHEY_SIMPLEX, 1.5, (0, 0, 255), 3) cv2.imshow(self.window_name, blank) - key = cv2.waitKey(config.DISPLAY.get('REFRESH_RATE_MS', 30)) & 0xFF + key = cv2.waitKey(config.DISPLAY['FRONT_VIEW_WINDOW'].get('REFRESH_RATE_MS', 30)) & 0xFF current_keys = {} if key != 255: @@ -1197,8 +1191,7 @@ def _update_stats(self): 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, - additional_images: Dict = None) -> np.ndarray: + def _add_info_overlay(self, image: np.ndarray, info: Dict, manual_info: List[str] = None) -> np.ndarray: if image is None or image.size == 0: return image @@ -1208,8 +1201,7 @@ def _add_info_overlay(self, image: np.ndarray, info: Dict, manual_info: List[str is_manual = info.get('state', '') == "手动控制" - grid_img = additional_images.get('grid') if additional_images else None - info_height = 180 if (is_manual and manual_info) or grid_img is not None else 100 + info_height = 180 if is_manual and manual_info else 100 cv2.rectangle(overlay, (0, 0), (width, info_height), (0, 0, 0), -1) cv2.addWeighted(overlay, 0.7, image, 0.3, 0, image) @@ -1225,8 +1217,8 @@ def _add_info_overlay(self, image: np.ndarray, info: Dict, manual_info: List[str red_objects_count = info.get('red_objects_count', 0) red_objects_visited = info.get('red_objects_visited', 0) - blue_objects_count = info.get('blue_objects_count', 0) # 新增 - blue_objects_visited = info.get('blue_objects_visited', 0) # 新增 + blue_objects_count = info.get('blue_objects_count', 0) + blue_objects_visited = info.get('blue_objects_visited', 0) if red_objects_count > 0 or blue_objects_count > 0: red_text = f"红色物体: {red_objects_visited}/{red_objects_count}" @@ -1236,19 +1228,6 @@ def _add_info_overlay(self, image: np.ndarray, info: Dict, manual_info: List[str cv2.putText(image, blue_text, (10, 110), cv2.FONT_HERSHEY_SIMPLEX, 0.6, (255, 100, 0), 2) - decision_info = info.get('decision_info', {}) - if decision_info: - y_pos = 130 if red_objects_count > 0 or blue_objects_count > 0 else 90 - for key, value in decision_info.items(): - if key == 'vector_angle': - cv2.putText(image, f"方向: {math.degrees(value):.0f}°", (10, y_pos), - cv2.FONT_HERSHEY_SIMPLEX, 0.5, (200, 255, 200), 1) - y_pos += 20 - elif key == 'grid_score': - cv2.putText(image, f"探索得分: {value:.2f}", (10, y_pos), - cv2.FONT_HERSHEY_SIMPLEX, 0.5, (200, 200, 255), 1) - y_pos += 20 - if is_manual and manual_info: y_start = 150 if (red_objects_count > 0 or blue_objects_count > 0) else 100 for i, line in enumerate(manual_info): @@ -1268,35 +1247,6 @@ def _add_info_overlay(self, image: np.ndarray, info: Dict, manual_info: List[str cv2.putText(image, fps_text, (width - 120, 30), cv2.FONT_HERSHEY_SIMPLEX, 0.6, (255, 255, 0), 1) - if self.performance_info['cpu_usage'] > 0: - cpu_text = f"CPU: {self.performance_info['cpu_usage']:.1f}%" - mem_text = f"MEM: {self.performance_info['memory_usage']:.1f}%" - - cpu_color = (0, 200, 255) if self.performance_info['cpu_usage'] > 80 else (0, 255, 0) - mem_color = (0, 200, 255) if self.performance_info['memory_usage'] > 80 else (0, 255, 0) - - cv2.putText(image, cpu_text, (width - 120, 60), - cv2.FONT_HERSHEY_SIMPLEX, 0.5, cpu_color, 1) - cv2.putText(image, mem_text, (width - 120, 80), - cv2.FONT_HERSHEY_SIMPLEX, 0.5, mem_color, 1) - - if grid_img is not None and grid_img.size > 0: - grid_size = 150 - grid_resized = cv2.resize(grid_img, (grid_size, grid_size)) - - x_offset = width - grid_size - 10 - y_offset = info_height + 10 - - if y_offset + grid_size < height: - cv2.rectangle(image, (x_offset-2, y_offset-2), - (x_offset+grid_size+2, y_offset+grid_size+2), - (255, 255, 255), 1) - - image[y_offset:y_offset+grid_size, x_offset:x_offset+grid_size] = grid_resized - - cv2.putText(image, "探索网格", (x_offset, y_offset-5), - cv2.FONT_HERSHEY_SIMPLEX, 0.5, (255, 255, 255), 1) - return image except Exception as e: print(f"⚠️ 添加信息叠加层出错: {e}") @@ -1312,13 +1262,351 @@ def _save_screenshot(self, image: Optional[np.ndarray]): print("⚠️ 无法保存截图：无有效图像数据") +class InfoDisplayWindow: + """信息显示窗口 - 显示系统状态、探索网格、物体统计等信息""" + + def __init__(self, window_name=None, width=None, height=None): + self.window_name = window_name if window_name else config.DISPLAY['INFO_WINDOW']['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.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 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 = "无人机信息面板" + cv2.putText(img, title, (center_x - 150, center_y - 100), + cv2.FONT_HERSHEY_SIMPLEX, 1.2, self.display_config['HIGHLIGHT_COLOR'], 2) + + # 状态信息 + status = "等待数据..." + cv2.putText(img, status, (center_x - 80, center_y), + cv2.FONT_HERSHEY_SIMPLEX, 0.8, self.display_config['TEXT_COLOR'], 1) + + # 提示 + tip = "系统正在初始化，请稍候..." + cv2.putText(img, tip, (center_x - 120, center_y + 50), + cv2.FONT_HERSHEY_SIMPLEX, 0.6, self.display_config['TEXT_COLOR'], 1) + + return 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 = "无人机信息面板" + cv2.putText(img, title, (self.window_width // 2 - 100, 30), + cv2.FONT_HERSHEY_SIMPLEX, 1.0, 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'] + state_color = success_color if '探索' in state else highlight_color if '悬停' in state else warning_color if '紧急' in state else text_color + cv2.putText(img, f"飞行状态: {state}", (x_offset, y_offset), + cv2.FONT_HERSHEY_SIMPLEX, 0.7, 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" + cv2.putText(img, pos_text, (x_offset, y_offset), + cv2.FONT_HERSHEY_SIMPLEX, 0.6, 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 + cv2.putText(img, obs_text, (x_offset, y_offset), + cv2.FONT_HERSHEY_SIMPLEX, 0.6, obs_color, 1) + y_offset += 25 + + open_text = f"开阔度: {perception.get('open_space_score', 0):.2f}" + cv2.putText(img, open_text, (x_offset, y_offset), + cv2.FONT_HERSHEY_SIMPLEX, 0.6, 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 + cv2.putText(img, red_text, (x_offset, y_offset), + cv2.FONT_HERSHEY_SIMPLEX, 0.7, 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 + cv2.putText(img, blue_text, (x_offset, y_offset), + cv2.FONT_HERSHEY_SIMPLEX, 0.7, blue_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}已探索" + cv2.putText(img, grid_text, (x_offset, y_offset), + cv2.FONT_HERSHEY_SIMPLEX, 0.6, 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 + + cv2.putText(img, f"CPU使用率: {cpu_usage:.1f}%", (x_offset, y_offset), + cv2.FONT_HERSHEY_SIMPLEX, 0.6, cpu_color, 1) + y_offset += 25 + + cv2.putText(img, f"内存使用率: {memory_usage:.1f}%", (x_offset, y_offset), + cv2.FONT_HERSHEY_SIMPLEX, 0.6, mem_color, 1) + y_offset += 25 + + cv2.putText(img, f"循环时间: {loop_time:.1f}ms", (x_offset, y_offset), + cv2.FONT_HERSHEY_SIMPLEX, 0.6, loop_color, 1) + y_offset += 25 + + # 7. 探索网格图像（右侧） + 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 + + # 添加网格标题 + cv2.putText(img, "探索网格", (grid_x, grid_y - 10), + cv2.FONT_HERSHEY_SIMPLEX, 0.6, highlight_color, 1) + + # 添加图例 + legend_y = grid_y + grid_size + 20 + cv2.putText(img, "图例:", (grid_x, legend_y), + cv2.FONT_HERSHEY_SIMPLEX, 0.5, text_color, 1) + legend_y += 20 + + # 当前位置 + cv2.rectangle(img, (grid_x, legend_y), (grid_x + 15, legend_y + 15), (0, 255, 0), -1) + cv2.putText(img, "当前位置", (grid_x + 20, legend_y + 12), + cv2.FONT_HERSHEY_SIMPLEX, 0.4, text_color, 1) + legend_y += 25 + + # 障碍物 + cv2.rectangle(img, (grid_x, legend_y), (grid_x + 15, legend_y + 15), (0, 0, 255), -1) + cv2.putText(img, "障碍物", (grid_x + 20, legend_y + 12), + cv2.FONT_HERSHEY_SIMPLEX, 0.4, text_color, 1) + legend_y += 25 + + # 红色物体 + cv2.rectangle(img, (grid_x, legend_y), (grid_x + 15, legend_y + 15), (0, 100, 255), -1) + cv2.putText(img, "红色物体", (grid_x + 20, legend_y + 12), + cv2.FONT_HERSHEY_SIMPLEX, 0.4, text_color, 1) + legend_y += 25 + + # 蓝色物体 + cv2.rectangle(img, (grid_x, legend_y), (grid_x + 15, legend_y + 15), (255, 100, 0), -1) + cv2.putText(img, "蓝色物体", (grid_x + 20, legend_y + 12), + cv2.FONT_HERSHEY_SIMPLEX, 0.4, text_color, 1) + legend_y += 25 + + # 前沿区域 + cv2.rectangle(img, (grid_x, legend_y), (grid_x + 15, legend_y + 15), (0, 200, 0), -1) + cv2.putText(img, "探索前沿", (grid_x + 20, legend_y + 12), + cv2.FONT_HERSHEY_SIMPLEX, 0.4, text_color, 1) + + # 将网格图像放到主图像上 + img[grid_y:grid_y+grid_size, grid_x:grid_x+grid_size] = grid_resized + + # 8. 时间戳 + if 'timestamp' in info_data: + timestamp = info_data['timestamp'] + time_text = f"更新时间: {timestamp}" + cv2.putText(img, time_text, (self.window_width - 200, self.window_height - 10), + cv2.FONT_HERSHEY_SIMPLEX, 0.5, text_color, 1) + + # 9. 底部提示 + hint_text = "按 Q 或 ESC 关闭窗口" + cv2.putText(img, hint_text, (self.window_width // 2 - 80, self.window_height - 30), + cv2.FONT_HERSHEY_SIMPLEX, 0.5, 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'] + cv2.putText(error_img, "渲染错误", (self.window_width // 2 - 50, self.window_height // 2), + cv2.FONT_HERSHEY_SIMPLEX, 1.0, 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("AirSimNH 感知驱动自主探索系统 - 双窗口双色物体检测版") self.logger.info("=" * 60) self.client = None @@ -1374,11 +1662,11 @@ def __init__(self, drone_name=""): 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.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.visited_positions = deque(maxlen=100) @@ -1413,12 +1701,14 @@ def __init__(self, drone_name=""): 'min_loop_time': 100.0, 'red_objects_detected': 0, 'red_objects_visited': 0, - 'blue_objects_detected': 0, # 新增 - 'blue_objects_visited': 0, # 新增 + 'blue_objects_detected': 0, + 'blue_objects_visited': 0, } - self.front_display = None - self._setup_front_display() + # 初始化两个窗口 + self.front_window = None + self.info_window = None + self._setup_windows() self.manual_control_start = 0 self.control_keys = {} @@ -1427,6 +1717,7 @@ def __init__(self, drone_name=""): 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']: @@ -1505,19 +1796,84 @@ def _check_connection_health(self): except: return False - def _setup_front_display(self): + def _setup_windows(self): + """初始化两个显示窗口""" try: - self.front_display = FrontViewDisplay( - window_name=f"无人机前视 - {self.drone_name or 'AirSimNH'}", - width=config.DISPLAY['WINDOW_WIDTH'], - height=config.DISPLAY['WINDOW_HEIGHT'], - enable_sharpening=config.DISPLAY['ENABLE_SHARPENING'], - show_info=config.DISPLAY['SHOW_INFO_OVERLAY'] + # 前视窗口 + self.front_window = FrontViewWindow( + window_name=f"{config.DISPLAY['FRONT_VIEW_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("🎥 前视窗口已初始化") + + # 信息显示窗口 + self.info_window = InfoDisplayWindow( + window_name=f"{config.DISPLAY['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 + + # 准备信息数据 + info_data = { + 'timestamp': datetime.now().strftime("%H:%M:%S"), + 'state': self.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), + 'red_in_view': perception.red_objects_count, + 'blue_in_view': perception.blue_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) + } + } + + # 添加网格图像 + 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.error(f"❌ 前视窗口初始化失败: {e}") - self.front_display = None + 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 = [] @@ -1650,7 +2006,7 @@ def _detect_red_objects(self, image: np.ndarray, depth_array: Optional[np.ndarra return red_objects, marked_image - def _detect_blue_objects(self, image: np.ndarray, depth_array: Optional[np.ndarray] = None) -> Tuple[List[BlueObject], np.ndarray]: # 新增 + 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 @@ -1793,7 +2149,7 @@ def _is_same_object(self, obj1: RedObject, obj2: RedObject, distance_threshold=2 return pixel_distance < 50 and time_diff < 5.0 - def _is_same_object_blue(self, obj1: BlueObject, obj2: BlueObject, distance_threshold=2.0) -> bool: # 新增 + 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 + @@ -1839,7 +2195,7 @@ def _check_red_object_proximity(self, current_pos): return False - def _check_blue_object_proximity(self, current_pos): # 新增 + def _check_blue_object_proximity(self, current_pos): for obj in self.blue_objects: if not obj.visited: distance = math.sqrt( @@ -1975,7 +2331,7 @@ def get_depth_perception(self) -> PerceptionResult: 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 @@ -1991,30 +2347,25 @@ def get_depth_perception(self) -> PerceptionResult: self._record_flight_data(result) - if self.front_display: + # 更新信息窗口 + self._update_info_window(result) + + if self.front_window: manual_info = None if self.state == FlightState.MANUAL: manual_info = self._get_manual_control_info() - grid_img = self.exploration_grid.visualize_grid(size=150) - additional_images = {'grid': grid_img} if grid_img is not None else {} - # 合并红色和蓝色物体标记 display_image = img_bgr.copy() - if config.DISPLAY['SHOW_RED_OBJECTS'] and result.red_objects_image is not None: + 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['SHOW_BLUE_OBJECTS'] and result.blue_objects_image is not None: + 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.PERFORMANCE['ENABLE_REALTIME_METRICS']: - cpu_usage = psutil.cpu_percent(interval=0) - memory_usage = psutil.virtual_memory().percent - self.front_display.update_performance_info(cpu_usage, memory_usage) - - self.front_display.update_image(display_image, display_info, manual_info, additional_images) + self.front_window.update_image(display_image, display_info, manual_info) self.stats['front_image_updates'] += 1 except Exception as e: @@ -2064,8 +2415,8 @@ def _record_flight_data(self, perception: PerceptionResult): 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) # 新增 + blue_objects_count = perception.blue_objects_count + blue_objects_visited = sum(1 for obj in self.blue_objects if obj.visited) data_dict = { 'timestamp': datetime.now().isoformat(), @@ -2097,9 +2448,9 @@ def _record_flight_data(self, perception: PerceptionResult): '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, # 新增 + 'blue_objects_count': blue_objects_count, + 'blue_objects_detected': self.stats['blue_objects_detected'], + 'blue_objects_visited': blue_objects_visited, } self.data_logger.record_flight_data(data_dict) @@ -2161,7 +2512,7 @@ def _update_exploration_grid(self, perception: PerceptionResult): if perception.red_objects: self.exploration_grid.update_red_objects(perception.red_objects) - if perception.blue_objects: # 新增 + if perception.blue_objects: self.exploration_grid.update_blue_objects(perception.blue_objects) self.stats['grid_updates'] += 1 @@ -2181,8 +2532,8 @@ def _prepare_display_info(self, perception: PerceptionResult) -> Dict: '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), # 新增 + 'blue_objects_count': perception.blue_objects_count, + 'blue_objects_visited': sum(1 for obj in self.blue_objects if obj.visited), } if hasattr(self, 'last_decision_info'): @@ -2229,7 +2580,7 @@ def _get_manual_control_info(self): 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: # 新增 + 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)}") @@ -2330,7 +2681,7 @@ def run_manual_control(self): self.logger.info("启动手动控制模式") self.logger.info("=" * 60) - if not self.front_display: + if not self.front_window: self.logger.error("❌ 前视窗口未初始化") return @@ -2338,7 +2689,7 @@ def run_manual_control(self): self.change_state(FlightState.MANUAL) self.manual_control_start = time.time() - self.front_display.set_manual_mode(True) + self.front_window.set_manual_mode(True) self.logger.info("🕹️ 进入手动控制模式") print("\n" + "="*60) @@ -2361,16 +2712,16 @@ def run_manual_control(self): while manual_active and not self.emergency_flag: try: - if self.front_display.should_exit_manual(): + if self.front_window.should_exit_manual(): self.logger.info("收到退出手动模式指令") manual_active = False break - if self.front_display: - window_keys = self.front_display.get_control_inputs() + if self.front_window: + window_keys = self.front_window.get_control_inputs() self.control_keys = window_keys.copy() - if not self.front_display.display_active: + if not self.front_window.display_active: self.logger.info("前视窗口已关闭，退出手动模式") manual_active = False break @@ -2409,9 +2760,9 @@ def run_manual_control(self): except: display_info = {} - if self.front_display: + if self.front_window: manual_info = self._get_manual_control_info() - self.front_display.update_image(img_bgr, display_info, manual_info) + self.front_window.update_image(img_bgr, display_info, manual_info) last_image_time = current_time except Exception as img_error: pass @@ -2421,7 +2772,7 @@ def run_manual_control(self): 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_blue_object_proximity(current_pos) except: pass @@ -2440,8 +2791,8 @@ def run_manual_control(self): self.manual_control_start = 0 self.control_keys = {} - if self.front_display: - self.front_display.set_manual_mode(False) + if self.front_window: + self.front_window.set_manual_mode(False) try: self.client.hoverAsync(vehicle_name=self.drone_name).join() @@ -2456,7 +2807,7 @@ def run_manual_control(self): print("手动控制模式已结束") print(f"控制时间: {manual_time:.1f}秒") print(f"检测到红色物体: {self.stats['red_objects_detected']}个") - print(f"检测到蓝色物体: {self.stats['blue_objects_detected']}个") # 新增 + print(f"检测到蓝色物体: {self.stats['blue_objects_detected']}个") print("="*60) print("请选择下一步:") print(" 1. 继续自动探索") @@ -2518,7 +2869,7 @@ def run_perception_loop(self): if self._check_red_object_proximity(current_pos): time.sleep(2) self.change_state(FlightState.EXPLORING) - if self._check_blue_object_proximity(current_pos): # 新增 + if self._check_blue_object_proximity(current_pos): time.sleep(2) self.change_state(FlightState.EXPLORING) except: @@ -2624,7 +2975,7 @@ def make_intelligent_decision(self, perception: PerceptionResult) -> Tuple[float self.exploration_target = self.exploration_grid.get_best_exploration_target( (pos.x_val, pos.y_val), perception.red_objects, - perception.blue_objects # 新增 + perception.blue_objects ) self.target_update_time = current_time @@ -2645,7 +2996,7 @@ def make_intelligent_decision(self, perception: PerceptionResult) -> Tuple[float self.exploration_target = self.exploration_grid.get_best_exploration_target( current_pos, perception.red_objects, - perception.blue_objects # 新增 + perception.blue_objects ) self.target_update_time = time.time() @@ -2654,7 +3005,7 @@ def make_intelligent_decision(self, perception: PerceptionResult) -> Tuple[float self.exploration_target, perception.obstacle_positions, perception.red_objects, - perception.blue_objects # 新增 + perception.blue_objects ) speed_factor = self._calculate_adaptive_speed(perception, vector.magnitude()) @@ -2676,7 +3027,7 @@ def make_intelligent_decision(self, perception: PerceptionResult) -> Tuple[float '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, # 新增 + 'blue_objects_in_view': perception.blue_objects_count, 'decision_time': time.time() - decision_start } @@ -2699,7 +3050,7 @@ def make_intelligent_decision(self, perception: PerceptionResult) -> Tuple[float None, perception.obstacle_positions, perception.red_objects, - perception.blue_objects # 新增 + perception.blue_objects ) if avoid_vector.magnitude() > 0.1: @@ -2717,7 +3068,7 @@ def make_intelligent_decision(self, perception: PerceptionResult) -> Tuple[float time.sleep(2) self.change_state(FlightState.EXPLORING) - elif self.state == FlightState.BLUE_OBJECT_INSPECTION: # 新增 + elif self.state == FlightState.BLUE_OBJECT_INSPECTION: target_vx, target_vy = 0.0, 0.0 time.sleep(2) self.change_state(FlightState.EXPLORING) @@ -2761,10 +3112,10 @@ def _calculate_adaptive_speed(self, perception: PerceptionResult, vector_magnitu 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 # 新增 - color_factor = min(red_factor, blue_factor) # 新增 + blue_factor = 0.8 if perception.blue_objects_count > 0 else 1.0 + color_factor = min(red_factor, blue_factor) - speed_factor = open_factor * obs_factor * vector_factor * color_factor * 0.7 # 更新 + speed_factor = open_factor * obs_factor * vector_factor * color_factor * 0.7 speed_factor = max( config.INTELLIGENT_DECISION['MIN_SPEED_FACTOR'], @@ -2820,7 +3171,7 @@ def _report_status(self, exploration_start, perception): 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']}个") # 新增 + f"| 已访问{self.stats['blue_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)}个") @@ -2870,10 +3221,14 @@ def _cleanup_system(self): except: self.logger.warning("⚠️ 释放控制时出现异常") - if self.front_display: - self.front_display.stop() + 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() @@ -2903,8 +3258,8 @@ def _generate_summary_report(self): 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['blue_objects_detected']}个") + self.logger.info(f" 蓝色物体访问: {self.stats['blue_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)}") @@ -2920,7 +3275,7 @@ def _generate_summary_report(self): 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("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") @@ -2980,8 +3335,11 @@ def emergency_stop(self): except Exception as e: self.logger.error(f"⚠️ 紧急降落异常: {e}") - if self.front_display: - self.front_display.stop() + if self.front_window: + self.front_window.stop() + + if self.info_window: + self.info_window.stop() self._cleanup_system() @@ -2990,7 +3348,7 @@ def emergency_stop(self): def main(): print("=" * 70) - print("AirSimNH 无人机感知探索系统 - 智能决策增强版（双色物体检测版）") + 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']}") @@ -3004,6 +3362,11 @@ def main(): 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)}") @@ -3036,6 +3399,10 @@ def signal_handler(sig, frame): print("\n" + "="*50) print("启动智能探索模式（含双色物体检测）") print("="*50) + print("注意：将打开两个窗口:") + print(" 1. 前视窗口 - 显示摄像头画面") + print(" 2. 信息窗口 - 显示系统状态和探索信息") + print("="*50) explorer.run_perception_loop() elif mode_choice == '2': From a8907ba6b09b7f51f66c84bcad6da5140053fe2c Mon Sep 17 00:00:00 2001 From: rhcgg <3136924319@qq.com> Date: Mon, 22 Dec 2025 20:28:18 +0800 Subject: [PATCH 15/23] =?UTF-8?q?=E5=8A=9F=E8=83=BD=E5=AE=8C=E5=96=84?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit 添加了黑色物体检测， --- src/drone_perception/config.py | 26 ++ .../perceptive_drone_explorer.py | 424 ++++++++++++++++-- 2 files changed, 424 insertions(+), 26 deletions(-) diff --git a/src/drone_perception/config.py b/src/drone_perception/config.py index 550cefc4ff..8010714bbd 100644 --- a/src/drone_perception/config.py +++ b/src/drone_perception/config.py @@ -50,6 +50,15 @@ '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, # 物体记忆时间（秒） } } @@ -102,6 +111,14 @@ '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, # 探索奖励分数 } } @@ -130,6 +147,7 @@ 'REFRESH_RATE_MS': 30, # 刷新率 (毫秒)，建议30-50 'SHOW_RED_OBJECTS': True, # 在画面中标记红色物体 'SHOW_BLUE_OBJECTS': True, # 在画面中标记蓝色物体 + 'SHOW_BLACK_OBJECTS': True, # 在画面中标记黑色物体（新增） }, # 信息显示窗口参数 @@ -187,6 +205,12 @@ 'BLUE_COLOR_RANGE': { 'LOWER': [100, 150, 50], # 蓝色下限 'UPPER': [130, 255, 255], # 蓝色上限 + }, + + # 黑色物体检测颜色范围（HSV空间）（新增） + 'BLACK_COLOR_RANGE': { + 'LOWER': [0, 0, 0], # 黑色下限（色相任意，饱和度和亮度都很低） + 'UPPER': [180, 255, 50], # 黑色上限（亮度阈值50，避免过暗） } } @@ -200,6 +224,7 @@ 'PERFORMANCE_PROFILING': False, # 是否启用性能分析 'SAVE_RED_OBJECT_IMAGES': False, # 是否保存检测到红色物体的图像 'SAVE_BLUE_OBJECT_IMAGES': False, # 是否保存检测到蓝色物体的图像（新增） + 'SAVE_BLACK_OBJECT_IMAGES': False, # 是否保存检测到黑色物体的图像（新增） } # ==================== 数据记录参数 ==================== @@ -214,6 +239,7 @@ 'SYSTEM_METRICS_INTERVAL': 5.0, # 系统指标记录间隔（秒） 'RECORD_RED_OBJECTS': True, # 记录红色物体信息 'RECORD_BLUE_OBJECTS': True, # 记录蓝色物体信息（新增） + 'RECORD_BLACK_OBJECTS': True, # 记录黑色物体信息（新增） } # ==================== 性能监控参数 ==================== diff --git a/src/drone_perception/perceptive_drone_explorer.py b/src/drone_perception/perceptive_drone_explorer.py index 61249b2357..ab6b044daa 100644 --- a/src/drone_perception/perceptive_drone_explorer.py +++ b/src/drone_perception/perceptive_drone_explorer.py @@ -1,11 +1,11 @@ """ -AirSimNH 感知驱动自主探索无人机 - 智能决策增强版（红色与蓝色物体检测版） +AirSimNH 感知驱动自主探索无人机 - 智能决策增强版（红色、蓝色与黑色物体检测版） 核心：视觉感知 → 语义理解 → 智能决策 → 安全执行 集成：配置管理、日志系统、异常恢复、前视窗口显示 新增：向量场避障算法、基于网格的信息增益探索、平滑飞行控制 -新增：性能监控与数据闭环系统、红色与蓝色物体检测与记录 +新增：性能监控与数据闭环系统、红色、蓝色与黑色物体检测与记录 新增：信息显示窗口，分离前视画面与系统信息 -版本: 3.5 (双窗口双色物体检测版) +版本: 3.6 (双窗口三色物体检测版) """ import airsim @@ -52,11 +52,14 @@ class DefaultConfig: 'MEMORY_TIME': 5.0}, 'BLUE_OBJECT_DETECTION': {'ENABLED': True, 'MIN_AREA': 50, 'MAX_AREA': 10000, 'UPDATE_INTERVAL': 1.0, - 'MEMORY_TIME': 5.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_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), @@ -70,7 +73,8 @@ class DefaultConfig: 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]}} + '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, @@ -107,14 +111,17 @@ class DefaultConfig: '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} + '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_BLUE_OBJECT_IMAGES': False, + 'SAVE_BLACK_OBJECT_IMAGES': False } DATA_RECORDING = { 'ENABLED': True, @@ -126,7 +133,8 @@ class DefaultConfig: 'PERFORMANCE_MONITORING': True, 'SYSTEM_METRICS_INTERVAL': 5.0, 'RECORD_RED_OBJECTS': True, - 'RECORD_BLUE_OBJECTS': True + 'RECORD_BLUE_OBJECTS': True, + 'RECORD_BLACK_OBJECTS': True } PERFORMANCE = { 'ENABLE_REALTIME_METRICS': True, @@ -152,6 +160,7 @@ class FlightState(Enum): PLANNING = "路径规划" RED_OBJECT_INSPECTION = "红色物体检查" BLUE_OBJECT_INSPECTION = "蓝色物体检查" + BLACK_OBJECT_INSPECTION = "黑色物体检查" @dataclass @@ -180,6 +189,19 @@ class BlueObject: 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): @@ -265,6 +287,7 @@ def __init__(self, resolution=2.0, grid_size=50): 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() @@ -363,7 +386,20 @@ def update_blue_objects(self, blue_objects): if 0 <= nx < self.grid_size and 0 <= ny < self.grid_size: self.blue_object_grid[nx, ny] = True - def get_best_exploration_target(self, current_pos, red_objects=None, blue_objects=None): + 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 @@ -398,6 +434,23 @@ def get_best_exploration_target(self, current_pos, red_objects=None, blue_object 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 @@ -428,11 +481,15 @@ def get_best_exploration_target(self, current_pos, red_objects=None, blue_object 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 + red_bonus + blue_bonus + black_bonus ) if score > best_score: @@ -462,6 +519,8 @@ def visualize_grid(self, size=300): 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: @@ -523,6 +582,7 @@ def __init__(self, enable_csv=True, enable_json=True, csv_filename=None, json_fi self.red_objects_detected = [] self.blue_objects_detected = [] + self.black_objects_detected = [] self.csv_columns = [ 'timestamp', 'loop_count', 'state', 'pos_x', 'pos_y', 'pos_z', @@ -535,7 +595,8 @@ def __init__(self, enable_csv=True, enable_json=True, csv_filename=None, json_fi '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' + 'blue_objects_count', 'blue_objects_detected', 'blue_objects_visited', + 'black_objects_count', 'black_objects_detected', 'black_objects_visited' ] if self.enable_csv: @@ -622,6 +683,28 @@ def record_blue_object(self, blue_object): 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 + } + + self.black_objects_detected.append(black_object_data) + + if 'black_objects' not in self.json_data: + self.json_data['black_objects'] = [] + + 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) @@ -690,6 +773,16 @@ def save_json_data(self): '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) @@ -776,6 +869,11 @@ def generate_performance_report(self): 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 = [] @@ -817,6 +915,9 @@ class PerceptionResult: 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: @@ -827,6 +928,8 @@ def __post_init__(self): self.red_objects = [] if self.blue_objects is None: self.blue_objects = [] + if self.black_objects is None: + self.black_objects = [] class VectorFieldPlanner: @@ -838,6 +941,7 @@ def __init__(self): 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']) @@ -845,11 +949,12 @@ def __init__(self): 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): + 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) @@ -857,7 +962,10 @@ def compute_vector(self, current_pos, goal_pos, obstacles, red_objects=None, blu if blue_objects: blue_attraction_vector = self._compute_blue_attraction(current_pos, blue_objects) - combined_vector = attraction_vector + repulsion_vector + red_attraction_vector + blue_attraction_vector + 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) @@ -925,6 +1033,22 @@ def _compute_blue_attraction(self, current_pos, blue_objects): 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) @@ -1219,17 +1343,22 @@ def _add_info_overlay(self, image: np.ndarray, info: Dict, manual_info: List[str red_objects_visited = info.get('red_objects_visited', 0) blue_objects_count = info.get('blue_objects_count', 0) blue_objects_visited = info.get('blue_objects_visited', 0) + black_objects_count = info.get('black_objects_count', 0) + black_objects_visited = info.get('black_objects_visited', 0) - if red_objects_count > 0 or blue_objects_count > 0: + if red_objects_count > 0 or blue_objects_count > 0 or black_objects_count > 0: red_text = f"红色物体: {red_objects_visited}/{red_objects_count}" blue_text = f"蓝色物体: {blue_objects_visited}/{blue_objects_count}" + black_text = f"黑色物体: {black_objects_visited}/{black_objects_count}" cv2.putText(image, red_text, (10, 90), cv2.FONT_HERSHEY_SIMPLEX, 0.6, (0, 100, 255), 2) cv2.putText(image, blue_text, (10, 110), cv2.FONT_HERSHEY_SIMPLEX, 0.6, (255, 100, 0), 2) + cv2.putText(image, black_text, (10, 130), + cv2.FONT_HERSHEY_SIMPLEX, 0.6, (128, 128, 128), 2) if is_manual and manual_info: - y_start = 150 if (red_objects_count > 0 or blue_objects_count > 0) else 100 + y_start = 170 if (red_objects_count > 0 or blue_objects_count > 0 or black_objects_count > 0) else 100 for i, line in enumerate(manual_info): y_pos = y_start + i * 20 cv2.putText(image, line, (10, y_pos), @@ -1237,7 +1366,7 @@ def _add_info_overlay(self, image: np.ndarray, info: Dict, manual_info: List[str cv2.putText(image, "手动控制中...", (width - 150, 60), cv2.FONT_HERSHEY_SIMPLEX, 0.6, (255, 255, 0), 1) - elif not is_manual and red_objects_count == 0 and blue_objects_count == 0: + elif not is_manual and red_objects_count == 0 and blue_objects_count == 0 and black_objects_count == 0: obs_dist = info.get('obstacle_distance', 0.0) obs_color = (0, 0, 255) if obs_dist < 5.0 else (0, 165, 255) if obs_dist < 10.0 else (0, 255, 0) cv2.putText(image, f"障碍: {obs_dist:.1f}m", (10, 90), @@ -1569,6 +1698,12 @@ def _render_info_display(self, info_data: Dict) -> np.ndarray: cv2.FONT_HERSHEY_SIMPLEX, 0.4, text_color, 1) legend_y += 25 + # 黑色物体 + cv2.rectangle(img, (grid_x, legend_y), (grid_x + 15, legend_y + 15), (128, 128, 128), -1) + cv2.putText(img, "黑色物体", (grid_x + 20, legend_y + 12), + cv2.FONT_HERSHEY_SIMPLEX, 0.4, text_color, 1) + legend_y += 25 + # 前沿区域 cv2.rectangle(img, (grid_x, legend_y), (grid_x + 15, legend_y + 15), (0, 200, 0), -1) cv2.putText(img, "探索前沿", (grid_x + 20, legend_y + 12), @@ -1668,6 +1803,12 @@ def __init__(self, drone_name=""): 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 @@ -1703,6 +1844,8 @@ def __init__(self, drone_name=""): 'red_objects_visited': 0, 'blue_objects_detected': 0, 'blue_objects_visited': 0, + 'black_objects_detected': 0, + 'black_objects_visited': 0, } # 初始化两个窗口 @@ -1716,7 +1859,7 @@ def __init__(self, drone_name=""): 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" 智能决策: 向量场避障 + 网格探索 + 三色物体检测") self.logger.info(f" 显示系统: 双窗口模式 (前视窗口 + 信息窗口)") if config.DATA_RECORDING['ENABLED']: self.logger.info(f" 数据记录: CSV + JSON 格式") @@ -1724,6 +1867,8 @@ def __init__(self, drone_name=""): 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') @@ -1849,8 +1994,11 @@ def _update_info_window(self, perception: PerceptionResult): '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 + 'blue_in_view': perception.blue_objects_count, + 'black_in_view': perception.black_objects_count }, 'grid_stats': { 'frontiers': len(self.exploration_grid.frontier_cells), @@ -2133,6 +2281,133 @@ def _detect_blue_objects(self, image: np.ndarray, depth_array: Optional[np.ndarr 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( @@ -2165,6 +2440,22 @@ def _is_same_object_blue(self, obj1: BlueObject, obj2: BlueObject, distance_thre 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: @@ -2225,6 +2516,36 @@ def _check_blue_object_proximity(self, current_pos): 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 @@ -2339,6 +2660,14 @@ def get_depth_perception(self) -> PerceptionResult: 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) @@ -2365,6 +2694,10 @@ def get_depth_perception(self) -> PerceptionResult: 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] + self.front_window.update_image(display_image, display_info, manual_info) self.stats['front_image_updates'] += 1 @@ -2375,7 +2708,8 @@ def get_depth_perception(self) -> PerceptionResult: 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}个, 蓝色物体={result.blue_objects_count}个") + 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): @@ -2418,6 +2752,9 @@ def _record_flight_data(self, perception: PerceptionResult): 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, @@ -2451,6 +2788,9 @@ def _record_flight_data(self, perception: PerceptionResult): '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) @@ -2515,6 +2855,9 @@ def _update_exploration_grid(self, perception: PerceptionResult): 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: @@ -2534,6 +2877,8 @@ def _prepare_display_info(self, perception: PerceptionResult) -> Dict: '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'): @@ -2584,6 +2929,10 @@ def _get_manual_control_info(self): 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}秒") @@ -2773,6 +3122,7 @@ def run_manual_control(self): 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) except: pass @@ -2872,6 +3222,9 @@ def run_perception_loop(self): 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 @@ -2975,7 +3328,8 @@ def make_intelligent_decision(self, perception: PerceptionResult) -> Tuple[float self.exploration_target = self.exploration_grid.get_best_exploration_target( (pos.x_val, pos.y_val), perception.red_objects, - perception.blue_objects + perception.blue_objects, + perception.black_objects ) self.target_update_time = current_time @@ -2996,7 +3350,8 @@ def make_intelligent_decision(self, perception: PerceptionResult) -> Tuple[float self.exploration_target = self.exploration_grid.get_best_exploration_target( current_pos, perception.red_objects, - perception.blue_objects + perception.blue_objects, + perception.black_objects ) self.target_update_time = time.time() @@ -3005,7 +3360,8 @@ def make_intelligent_decision(self, perception: PerceptionResult) -> Tuple[float self.exploration_target, perception.obstacle_positions, perception.red_objects, - perception.blue_objects + perception.blue_objects, + perception.black_objects ) speed_factor = self._calculate_adaptive_speed(perception, vector.magnitude()) @@ -3028,6 +3384,7 @@ def make_intelligent_decision(self, perception: PerceptionResult) -> Tuple[float '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 } @@ -3050,7 +3407,8 @@ def make_intelligent_decision(self, perception: PerceptionResult) -> Tuple[float None, perception.obstacle_positions, perception.red_objects, - perception.blue_objects + perception.blue_objects, + perception.black_objects ) if avoid_vector.magnitude() > 0.1: @@ -3073,6 +3431,11 @@ def make_intelligent_decision(self, perception: PerceptionResult) -> Tuple[float 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) @@ -3113,7 +3476,8 @@ def _calculate_adaptive_speed(self, perception: PerceptionResult, vector_magnitu 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 - color_factor = min(red_factor, blue_factor) + 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 @@ -3129,7 +3493,8 @@ def _execute_control_decision(self, decision): 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.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, @@ -3172,6 +3537,8 @@ def _report_status(self, exploration_start, perception): 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)}个") @@ -3260,6 +3627,8 @@ def _generate_summary_report(self): 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)}") @@ -3291,6 +3660,8 @@ def _generate_summary_report(self): 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") @@ -3436,6 +3807,7 @@ def signal_handler(sig, frame): 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. 继续智能探索") From 31c452d74e7385f3747a01f906bde91546659379 Mon Sep 17 00:00:00 2001 From: rhcgg <3136924319@qq.com> Date: Tue, 23 Dec 2025 15:10:39 +0800 Subject: [PATCH 16/23] =?UTF-8?q?=E5=86=85=E5=AD=98=E4=BC=98=E5=8C=96?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit 存优化总结 1. 数据记录优化 限制 flight_data 缓冲区：最大 500 条，超过后自动保存并清空 限制物体记录列表：最大 200 个（红/蓝/黑） 限制事件记录列表：最大 100 个 自动保存机制：每 60 秒自动保存数据到临时文件并清空缓冲区 2. 性能指标优化 性能指标缓冲区从 1000 减少到 500 CPU、内存、循环时间指标都应用此限制 3. 图像处理优化 图像队列大小从 3 减少到 2 智能图像复制：仅在需要标记时才复制图像 队列为空时直接使用图像引用，避免不必要的复制 4. 垃圾回收机制 在主循环中每 20 次循环执行一次垃圾回收 自动保存数据后立即执行垃圾回收 --- src/drone_perception/config.py | 10 ++ .../perceptive_drone_explorer.py | 123 +++++++++++++++--- 2 files changed, 116 insertions(+), 17 deletions(-) diff --git a/src/drone_perception/config.py b/src/drone_perception/config.py index 8010714bbd..5896c8503a 100644 --- a/src/drone_perception/config.py +++ b/src/drone_perception/config.py @@ -148,6 +148,9 @@ 'SHOW_RED_OBJECTS': True, # 在画面中标记红色物体 'SHOW_BLUE_OBJECTS': True, # 在画面中标记蓝色物体 'SHOW_BLACK_OBJECTS': True, # 在画面中标记黑色物体（新增） + # 内存优化参数 + 'QUEUE_MAXSIZE': 2, # 图像队列最大大小，减少内存占用（原为3） + 'REDUCE_IMAGE_COPY': True, # 减少图像复制，仅在必要时复制 }, # 信息显示窗口参数 @@ -240,6 +243,11 @@ '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, # 自动保存间隔（秒），定期保存数据到文件 } # ==================== 性能监控参数 ==================== @@ -250,4 +258,6 @@ 'LOOP_TIME_WARNING_THRESHOLD': 0.2, # 循环时间警告阈值（秒） 'SAVE_PERFORMANCE_REPORT': True, # 保存性能报告 'REPORT_INTERVAL': 30.0, # 性能报告间隔（秒） + # 内存优化参数 + 'MAX_METRICS_BUFFER': 500, # 最大性能指标缓冲区大小（个），减少内存占用 } \ No newline at end of file diff --git a/src/drone_perception/perceptive_drone_explorer.py b/src/drone_perception/perceptive_drone_explorer.py index ab6b044daa..341207a7c6 100644 --- a/src/drone_perception/perceptive_drone_explorer.py +++ b/src/drone_perception/perceptive_drone_explorer.py @@ -29,6 +29,7 @@ import random import psutil import os +import gc # ============ 导入配置文件 ============ try: @@ -584,6 +585,14 @@ def __init__(self, enable_csv=True, enable_json=True, csv_filename=None, json_fi 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', @@ -630,9 +639,18 @@ def record_flight_data(self, data_dict): 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() @@ -651,11 +669,17 @@ def record_red_object(self, red_object): '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: @@ -673,11 +697,17 @@ def record_blue_object(self, blue_object): '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: @@ -695,11 +725,17 @@ def record_black_object(self, black_object): '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: @@ -714,7 +750,8 @@ def _collect_system_metrics(self): memory_percent = memory_info.percent self.performance_metrics['memory_usage'].append(memory_percent) - max_length = 1000 + # 内存优化：使用配置的最大缓冲区大小 + 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: @@ -726,7 +763,8 @@ def _collect_system_metrics(self): def record_loop_time(self, loop_time): self.performance_metrics['loop_times'].append(loop_time) - max_length = 1000 + # 内存优化：使用配置的最大缓冲区大小 + 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:] @@ -741,11 +779,39 @@ def record_event(self, event_type, 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 @@ -1102,7 +1168,10 @@ def __init__(self, window_name=None, width=None, height=None, self.show_info = (show_info if show_info is not None else config.DISPLAY['FRONT_VIEW_WINDOW']['SHOW_INFO_OVERLAY']) - self.image_queue = queue.Queue(maxsize=3) + # 内存优化：使用配置的队列大小 + 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 @@ -1158,8 +1227,18 @@ def update_image(self, image_data: np.ndarray, info: Optional[Dict] = None, 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': image_data.copy(), + 'image': display_image, 'info': info.copy() if info else {}, 'manual_info': manual_info.copy() if manual_info else [], 'timestamp': time.time() @@ -2684,19 +2763,27 @@ def get_depth_perception(self) -> PerceptionResult: if self.state == FlightState.MANUAL: manual_info = self._get_manual_control_info() - # 合并红色和蓝色物体标记 - 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] + # 内存优化：仅在需要标记时才复制图像 + 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 @@ -3247,6 +3334,8 @@ def run_perception_loop(self): 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: From f1c458ccd9a26cc739251502733ea44f54a210f6 Mon Sep 17 00:00:00 2001 From: rhcgg <3136924319@qq.com> Date: Wed, 24 Dec 2025 17:24:32 +0800 Subject: [PATCH 17/23] =?UTF-8?q?=E4=BF=AE=E5=A4=8D=E9=83=A8=E5=88=86?= =?UTF-8?q?=E6=98=BE=E7=A4=BA=E7=95=8C=E9=9D=A2=E5=BC=82=E5=B8=B8?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit 收复部分显示界面无法显示汉字问题，改为显示英文 --- .../perceptive_drone_explorer.py | 261 +++++++++++++----- 1 file changed, 193 insertions(+), 68 deletions(-) diff --git a/src/drone_perception/perceptive_drone_explorer.py b/src/drone_perception/perceptive_drone_explorer.py index 341207a7c6..a7c5d2d8e2 100644 --- a/src/drone_perception/perceptive_drone_explorer.py +++ b/src/drone_perception/perceptive_drone_explorer.py @@ -1,3 +1,4 @@ +# -*- coding: utf-8 -*- """ AirSimNH 感知驱动自主探索无人机 - 智能决策增强版（红色、蓝色与黑色物体检测版） 核心：视觉感知 → 语义理解 → 智能决策 → 安全执行 @@ -31,6 +32,17 @@ import os import gc +# 导入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 = {} + # ============ 导入配置文件 ============ try: import config @@ -1155,6 +1167,144 @@ def _limit_turn_angle(self, vector): 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并打印警告（仅第一次） + if font is None and 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 put_chinese_text(img, text, position, font_size=20, color=(255, 255, 255), thickness=1): + """ + 在OpenCV图像上绘制中文文本 + 使用PIL/Pillow来支持中文显示 + """ + if not PIL_AVAILABLE: + # 如果PIL不可用，使用英文替代 + text_en = text.replace("状态:", "State:").replace("位置:", "Pos:").replace("红色物体:", "Red:").replace("蓝色物体:", "Blue:").replace("黑色物体:", "Black:").replace("障碍:", "Obs:").replace("手动控制中...", "Manual Ctrl").replace("等待无人机图像...", "Waiting...").replace("飞行状态:", "State:").replace("障碍距离:", "Obs:").replace("开阔度:", "Open:").replace("探索网格:", "Grid:").replace("CPU使用率:", "CPU:").replace("内存使用率:", "Mem:").replace("循环时间:", "Loop:").replace("更新时间:", "Time:").replace("按 Q 或 ESC 关闭窗口", "Press Q/ESC to close").replace("渲染错误", "Render Error").replace("探索前沿", "Frontier").replace("当前位置", "Current").replace("障碍物", "Obstacle").replace("图例:", "Legend:").replace("无人机信息面板", "Info Panel").replace("等待数据...", "Waiting...").replace("系统正在初始化，请稍候...", "Initializing...") + cv2.putText(img, text_en, position, cv2.FONT_HERSHEY_SIMPLEX, font_size / 30.0, color, thickness) + return img + + try: + # 将OpenCV图像转换为PIL图像 + img_pil = Image.fromarray(cv2.cvtColor(img, cv2.COLOR_BGR2RGB)) + draw = ImageDraw.Draw(img_pil) + + # 加载中文字体 + font = _load_chinese_font(font_size) + + if font is None: + # 如果找不到字体，回退到英文显示 + raise Exception("未找到支持中文的字体") + + # 绘制文本（PIL使用RGB颜色） + color_rgb = (color[2], color[1], color[0]) # BGR转RGB + + # PIL的text函数位置参数是(x, y) + x, y = position + draw.text((x, y), text, font=font, fill=color_rgb) + + # 转换回OpenCV格式 + img = cv2.cvtColor(np.array(img_pil), cv2.COLOR_RGB2BGR) + + except Exception as e: + # 如果出错（包括字体加载失败），回退到英文显示 + try: + # 将中文转换为英文 + text_en = text.replace("状态:", "State:").replace("位置:", "Pos:").replace("红色物体:", "Red:").replace("蓝色物体:", "Blue:").replace("黑色物体:", "Black:").replace("障碍:", "Obs:").replace("手动控制中...", "Manual Ctrl").replace("等待无人机图像...", "Waiting...").replace("飞行状态:", "State:").replace("障碍距离:", "Obs:").replace("开阔度:", "Open:").replace("探索网格:", "Grid:").replace("CPU使用率:", "CPU:").replace("内存使用率:", "Mem:").replace("循环时间:", "Loop:").replace("更新时间:", "Time:").replace("按 Q 或 ESC 关闭窗口", "Press Q/ESC to close").replace("渲染错误", "Render Error").replace("探索前沿", "Frontier").replace("当前位置", "Current").replace("障碍物", "Obstacle").replace("图例:", "Legend:").replace("无人机信息面板", "Info Panel").replace("等待数据...", "Waiting...").replace("系统正在初始化，请稍候...", "Initializing...") + cv2.putText(img, text_en, position, cv2.FONT_HERSHEY_SIMPLEX, font_size / 30.0, color, thickness) + except: + pass + + return img + + class FrontViewWindow: """前视窗口 - 显示摄像头画面和手动控制""" @@ -1267,8 +1417,7 @@ def _display_loop(self): cv2.resizeWindow(self.window_name, self.window_width, self.window_height) wait_img = np.zeros((300, 400, 3), dtype=np.uint8) - cv2.putText(wait_img, "等待无人机图像...", (50, 150), - cv2.FONT_HERSHEY_SIMPLEX, 0.8, (255, 255, 255), 2) + wait_img = put_chinese_text(wait_img, "等待无人机图像...", (50, 150), 24, (255, 255, 255), 2) cv2.imshow(self.window_name, wait_img) cv2.waitKey(100) @@ -1411,12 +1560,10 @@ def _add_info_overlay(self, image: np.ndarray, info: Dict, manual_info: List[str state = info.get('state', 'UNKNOWN') state_color = (0, 255, 0) if '探索' in state else (0, 255, 255) if '悬停' in state else (255, 255, 0) if '手动' in state else (0, 0, 255) - cv2.putText(image, f"状态: {state}", (10, 30), - cv2.FONT_HERSHEY_SIMPLEX, 0.7, state_color, 2) + image = put_chinese_text(image, f"状态: {state}", (10, 30), 21, state_color, 2) pos = info.get('position', (0, 0, 0)) - cv2.putText(image, f"位置: ({pos[0]:.1f}, {pos[1]:.1f}, {-pos[2]:.1f}m)", (10, 60), - cv2.FONT_HERSHEY_SIMPLEX, 0.6, (255, 255, 255), 1) + image = put_chinese_text(image, f"位置: ({pos[0]:.1f}, {pos[1]:.1f}, {-pos[2]:.1f}m)", (10, 60), 18, (255, 255, 255), 1) red_objects_count = info.get('red_objects_count', 0) red_objects_visited = info.get('red_objects_visited', 0) @@ -1429,27 +1576,21 @@ def _add_info_overlay(self, image: np.ndarray, info: Dict, manual_info: List[str red_text = f"红色物体: {red_objects_visited}/{red_objects_count}" blue_text = f"蓝色物体: {blue_objects_visited}/{blue_objects_count}" black_text = f"黑色物体: {black_objects_visited}/{black_objects_count}" - cv2.putText(image, red_text, (10, 90), - cv2.FONT_HERSHEY_SIMPLEX, 0.6, (0, 100, 255), 2) - cv2.putText(image, blue_text, (10, 110), - cv2.FONT_HERSHEY_SIMPLEX, 0.6, (255, 100, 0), 2) - cv2.putText(image, black_text, (10, 130), - cv2.FONT_HERSHEY_SIMPLEX, 0.6, (128, 128, 128), 2) + image = put_chinese_text(image, red_text, (10, 90), 18, (0, 100, 255), 2) + image = put_chinese_text(image, blue_text, (10, 110), 18, (255, 100, 0), 2) + image = put_chinese_text(image, black_text, (10, 130), 18, (128, 128, 128), 2) if is_manual and manual_info: y_start = 170 if (red_objects_count > 0 or blue_objects_count > 0 or black_objects_count > 0) else 100 for i, line in enumerate(manual_info): y_pos = y_start + i * 20 - cv2.putText(image, line, (10, y_pos), - cv2.FONT_HERSHEY_SIMPLEX, 0.5, (200, 255, 200), 1) + image = put_chinese_text(image, line, (10, y_pos), 15, (200, 255, 200), 1) - cv2.putText(image, "手动控制中...", (width - 150, 60), - cv2.FONT_HERSHEY_SIMPLEX, 0.6, (255, 255, 0), 1) + image = put_chinese_text(image, "手动控制中...", (width - 150, 60), 18, (255, 255, 0), 1) elif not is_manual and red_objects_count == 0 and blue_objects_count == 0 and black_objects_count == 0: obs_dist = info.get('obstacle_distance', 0.0) obs_color = (0, 0, 255) if obs_dist < 5.0 else (0, 165, 255) if obs_dist < 10.0 else (0, 255, 0) - cv2.putText(image, f"障碍: {obs_dist:.1f}m", (10, 90), - cv2.FONT_HERSHEY_SIMPLEX, 0.7, obs_color, 2) + image = put_chinese_text(image, f"障碍: {obs_dist:.1f}m", (10, 90), 21, obs_color, 2) fps_text = f"FPS: {self.display_stats['fps']:.1f}" cv2.putText(image, fps_text, (width - 120, 30), @@ -1584,18 +1725,15 @@ def _create_waiting_screen(self): # 标题 title = "无人机信息面板" - cv2.putText(img, title, (center_x - 150, center_y - 100), - cv2.FONT_HERSHEY_SIMPLEX, 1.2, self.display_config['HIGHLIGHT_COLOR'], 2) + img = put_chinese_text(img, title, (center_x - 150, center_y - 100), 36, self.display_config['HIGHLIGHT_COLOR'], 2) # 状态信息 status = "等待数据..." - cv2.putText(img, status, (center_x - 80, center_y), - cv2.FONT_HERSHEY_SIMPLEX, 0.8, self.display_config['TEXT_COLOR'], 1) + img = put_chinese_text(img, status, (center_x - 80, center_y), 24, self.display_config['TEXT_COLOR'], 1) # 提示 tip = "系统正在初始化，请稍候..." - cv2.putText(img, tip, (center_x - 120, center_y + 50), - cv2.FONT_HERSHEY_SIMPLEX, 0.6, self.display_config['TEXT_COLOR'], 1) + img = put_chinese_text(img, tip, (center_x - 120, center_y + 50), 18, self.display_config['TEXT_COLOR'], 1) return img @@ -1617,8 +1755,7 @@ def _render_info_display(self, info_data: Dict) -> np.ndarray: # 标题栏 title = "无人机信息面板" - cv2.putText(img, title, (self.window_width // 2 - 100, 30), - cv2.FONT_HERSHEY_SIMPLEX, 1.0, highlight_color, 2) + 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) @@ -1629,16 +1766,14 @@ def _render_info_display(self, info_data: Dict) -> np.ndarray: if 'state' in info_data: state = info_data['state'] state_color = success_color if '探索' in state else highlight_color if '悬停' in state else warning_color if '紧急' in state else text_color - cv2.putText(img, f"飞行状态: {state}", (x_offset, y_offset), - cv2.FONT_HERSHEY_SIMPLEX, 0.7, state_color, 2) + 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" - cv2.putText(img, pos_text, (x_offset, y_offset), - cv2.FONT_HERSHEY_SIMPLEX, 0.6, text_color, 1) + img = put_chinese_text(img, pos_text, (x_offset, y_offset), 18, text_color, 1) y_offset += 25 # 3. 环境感知信息 @@ -1646,13 +1781,11 @@ def _render_info_display(self, info_data: Dict) -> np.ndarray: 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 - cv2.putText(img, obs_text, (x_offset, y_offset), - cv2.FONT_HERSHEY_SIMPLEX, 0.6, obs_color, 1) + 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}" - cv2.putText(img, open_text, (x_offset, y_offset), - cv2.FONT_HERSHEY_SIMPLEX, 0.6, text_color, 1) + img = put_chinese_text(img, open_text, (x_offset, y_offset), 18, text_color, 1) y_offset += 25 # 4. 物体检测统计 @@ -1664,8 +1797,7 @@ def _render_info_display(self, info_data: Dict) -> np.ndarray: 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 - cv2.putText(img, red_text, (x_offset, y_offset), - cv2.FONT_HERSHEY_SIMPLEX, 0.7, red_color, 1) + img = put_chinese_text(img, red_text, (x_offset, y_offset), 21, red_color, 1) y_offset += 30 # 蓝色物体统计 @@ -1673,9 +1805,17 @@ def _render_info_display(self, info_data: Dict) -> np.ndarray: 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 - cv2.putText(img, blue_text, (x_offset, y_offset), - cv2.FONT_HERSHEY_SIMPLEX, 0.7, blue_color, 1) + 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: @@ -1685,8 +1825,7 @@ def _render_info_display(self, info_data: Dict) -> np.ndarray: total = grid_stats.get('total', 1) grid_text = f"探索网格: {frontiers}前沿 | {explored}/{total}已探索" - cv2.putText(img, grid_text, (x_offset, y_offset), - cv2.FONT_HERSHEY_SIMPLEX, 0.6, text_color, 1) + img = put_chinese_text(img, grid_text, (x_offset, y_offset), 18, text_color, 1) y_offset += 25 # 探索进度条 @@ -1721,16 +1860,13 @@ def _render_info_display(self, info_data: Dict) -> np.ndarray: mem_color = warning_color if memory_usage > 80 else text_color loop_color = warning_color if loop_time > 200 else text_color - cv2.putText(img, f"CPU使用率: {cpu_usage:.1f}%", (x_offset, y_offset), - cv2.FONT_HERSHEY_SIMPLEX, 0.6, cpu_color, 1) + img = put_chinese_text(img, f"CPU使用率: {cpu_usage:.1f}%", (x_offset, y_offset), 18, cpu_color, 1) y_offset += 25 - cv2.putText(img, f"内存使用率: {memory_usage:.1f}%", (x_offset, y_offset), - cv2.FONT_HERSHEY_SIMPLEX, 0.6, mem_color, 1) + img = put_chinese_text(img, f"内存使用率: {memory_usage:.1f}%", (x_offset, y_offset), 18, mem_color, 1) y_offset += 25 - cv2.putText(img, f"循环时间: {loop_time:.1f}ms", (x_offset, y_offset), - cv2.FONT_HERSHEY_SIMPLEX, 0.6, loop_color, 1) + img = put_chinese_text(img, f"循环时间: {loop_time:.1f}ms", (x_offset, y_offset), 18, loop_color, 1) y_offset += 25 # 7. 探索网格图像（右侧） @@ -1744,49 +1880,41 @@ def _render_info_display(self, info_data: Dict) -> np.ndarray: grid_y = 80 # 添加网格标题 - cv2.putText(img, "探索网格", (grid_x, grid_y - 10), - cv2.FONT_HERSHEY_SIMPLEX, 0.6, highlight_color, 1) + img = put_chinese_text(img, "探索网格", (grid_x, grid_y - 10), 18, highlight_color, 1) # 添加图例 legend_y = grid_y + grid_size + 20 - cv2.putText(img, "图例:", (grid_x, legend_y), - cv2.FONT_HERSHEY_SIMPLEX, 0.5, text_color, 1) + 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) - cv2.putText(img, "当前位置", (grid_x + 20, legend_y + 12), - cv2.FONT_HERSHEY_SIMPLEX, 0.4, text_color, 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) - cv2.putText(img, "障碍物", (grid_x + 20, legend_y + 12), - cv2.FONT_HERSHEY_SIMPLEX, 0.4, text_color, 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) - cv2.putText(img, "红色物体", (grid_x + 20, legend_y + 12), - cv2.FONT_HERSHEY_SIMPLEX, 0.4, text_color, 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) - cv2.putText(img, "蓝色物体", (grid_x + 20, legend_y + 12), - cv2.FONT_HERSHEY_SIMPLEX, 0.4, text_color, 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) - cv2.putText(img, "黑色物体", (grid_x + 20, legend_y + 12), - cv2.FONT_HERSHEY_SIMPLEX, 0.4, text_color, 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) - cv2.putText(img, "探索前沿", (grid_x + 20, legend_y + 12), - cv2.FONT_HERSHEY_SIMPLEX, 0.4, text_color, 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 @@ -1795,13 +1923,11 @@ def _render_info_display(self, info_data: Dict) -> np.ndarray: if 'timestamp' in info_data: timestamp = info_data['timestamp'] time_text = f"更新时间: {timestamp}" - cv2.putText(img, time_text, (self.window_width - 200, self.window_height - 10), - cv2.FONT_HERSHEY_SIMPLEX, 0.5, text_color, 1) + img = put_chinese_text(img, time_text, (self.window_width - 200, self.window_height - 10), 15, text_color, 1) # 9. 底部提示 hint_text = "按 Q 或 ESC 关闭窗口" - cv2.putText(img, hint_text, (self.window_width // 2 - 80, self.window_height - 30), - cv2.FONT_HERSHEY_SIMPLEX, 0.5, text_color, 1) + img = put_chinese_text(img, hint_text, (self.window_width // 2 - 80, self.window_height - 30), 15, text_color, 1) return img @@ -1809,8 +1935,7 @@ def _render_info_display(self, info_data: Dict) -> np.ndarray: print(f"⚠️ 渲染信息显示时出错: {e}") error_img = np.zeros((self.window_height, self.window_width, 3), dtype=np.uint8) error_img[:, :] = self.display_config['BACKGROUND_COLOR'] - cv2.putText(error_img, "渲染错误", (self.window_width // 2 - 50, self.window_height // 2), - cv2.FONT_HERSHEY_SIMPLEX, 1.0, warning_color, 2) + error_img = put_chinese_text(error_img, "渲染错误", (self.window_width // 2 - 50, self.window_height // 2), 30, warning_color, 2) return error_img From 8f7c879e750c5ca282be33c25435bc7dbd851378 Mon Sep 17 00:00:00 2001 From: rhcgg <3136924319@qq.com> Date: Wed, 24 Dec 2025 20:28:52 +0800 Subject: [PATCH 18/23] =?UTF-8?q?=E5=AE=8C=E6=95=B4=E4=BF=AE=E5=A4=8D?= =?UTF-8?q?=E6=98=BE=E7=A4=BA=E7=95=8C=E9=9D=A2=E5=BC=82=E5=B8=B8=E9=97=AE?= =?UTF-8?q?=E9=A2=98?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit 完整修复显示界面异常问题 --- .../perceptive_drone_explorer.py | 179 ++++++++++++++---- 1 file changed, 142 insertions(+), 37 deletions(-) diff --git a/src/drone_perception/perceptive_drone_explorer.py b/src/drone_perception/perceptive_drone_explorer.py index a7c5d2d8e2..3391236fb8 100644 --- a/src/drone_perception/perceptive_drone_explorer.py +++ b/src/drone_perception/perceptive_drone_explorer.py @@ -42,6 +42,8 @@ # 全局字体缓存 _chinese_font_cache = {} +# 全局标志：是否禁用中文显示（如果字体加载失败） +_chinese_font_disabled = False # ============ 导入配置文件 ============ try: @@ -1248,59 +1250,139 @@ def _load_chinese_font(font_size=20): print(f"✅ 成功加载中文字体: {os.path.basename(loaded_font_path)}") # 如果找不到字体，缓存None并打印警告（仅第一次） - if font is None and 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等）") + 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来支持中文显示 + 使用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: - # 如果PIL不可用，使用英文替代 - text_en = text.replace("状态:", "State:").replace("位置:", "Pos:").replace("红色物体:", "Red:").replace("蓝色物体:", "Blue:").replace("黑色物体:", "Black:").replace("障碍:", "Obs:").replace("手动控制中...", "Manual Ctrl").replace("等待无人机图像...", "Waiting...").replace("飞行状态:", "State:").replace("障碍距离:", "Obs:").replace("开阔度:", "Open:").replace("探索网格:", "Grid:").replace("CPU使用率:", "CPU:").replace("内存使用率:", "Mem:").replace("循环时间:", "Loop:").replace("更新时间:", "Time:").replace("按 Q 或 ESC 关闭窗口", "Press Q/ESC to close").replace("渲染错误", "Render Error").replace("探索前沿", "Frontier").replace("当前位置", "Current").replace("障碍物", "Obstacle").replace("图例:", "Legend:").replace("无人机信息面板", "Info Panel").replace("等待数据...", "Waiting...").replace("系统正在初始化，请稍候...", "Initializing...") + _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) - # 加载中文字体 - font = _load_chinese_font(font_size) - - if font is None: - # 如果找不到字体，回退到英文显示 - raise Exception("未找到支持中文的字体") - # 绘制文本（PIL使用RGB颜色） color_rgb = (color[2], color[1], color[0]) # BGR转RGB # PIL的text函数位置参数是(x, y) x, y = position - draw.text((x, y), text, font=font, fill=color_rgb) - # 转换回OpenCV格式 - img = cv2.cvtColor(np.array(img_pil), cv2.COLOR_RGB2BGR) + # 尝试绘制文本，如果失败则回退到英文 + 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: - # 如果出错（包括字体加载失败），回退到英文显示 - try: - # 将中文转换为英文 - text_en = text.replace("状态:", "State:").replace("位置:", "Pos:").replace("红色物体:", "Red:").replace("蓝色物体:", "Blue:").replace("黑色物体:", "Black:").replace("障碍:", "Obs:").replace("手动控制中...", "Manual Ctrl").replace("等待无人机图像...", "Waiting...").replace("飞行状态:", "State:").replace("障碍距离:", "Obs:").replace("开阔度:", "Open:").replace("探索网格:", "Grid:").replace("CPU使用率:", "CPU:").replace("内存使用率:", "Mem:").replace("循环时间:", "Loop:").replace("更新时间:", "Time:").replace("按 Q 或 ESC 关闭窗口", "Press Q/ESC to close").replace("渲染错误", "Render Error").replace("探索前沿", "Frontier").replace("当前位置", "Current").replace("障碍物", "Obstacle").replace("图例:", "Legend:").replace("无人机信息面板", "Info Panel").replace("等待数据...", "Waiting...").replace("系统正在初始化，请稍候...", "Initializing...") - cv2.putText(img, text_en, position, cv2.FONT_HERSHEY_SIMPLEX, font_size / 30.0, color, thickness) - except: - pass + # 如果绘制失败，回退到英文显示并设置禁用标志 + _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 @@ -1310,7 +1392,13 @@ class FrontViewWindow: def __init__(self, window_name=None, width=None, height=None, enable_sharpening=None, show_info=None): - self.window_name = window_name if window_name else config.DISPLAY['FRONT_VIEW_WINDOW']['NAME'] + # 窗口标题使用英文，避免乱码 + 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 @@ -1615,7 +1703,13 @@ class InfoDisplayWindow: """信息显示窗口 - 显示系统状态、探索网格、物体统计等信息""" def __init__(self, window_name=None, width=None, height=None): - self.window_name = window_name if window_name else config.DISPLAY['INFO_WINDOW']['NAME'] + # 窗口标题使用英文，避免乱码 + 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'] @@ -1765,7 +1859,8 @@ def _render_info_display(self, info_data: Dict) -> np.ndarray: # 1. 飞行状态信息 if 'state' in info_data: state = info_data['state'] - state_color = success_color if '探索' in state else highlight_color if '悬停' in state else warning_color if '紧急' in state else text_color + # 状态值已经在_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 @@ -1879,8 +1974,11 @@ def _render_info_display(self, info_data: Dict) -> np.ndarray: grid_x = self.window_width - grid_size - 20 grid_y = 80 - # 添加网格标题 - img = put_chinese_text(img, "探索网格", (grid_x, grid_y - 10), 18, highlight_color, 1) + # 添加网格标题（直接使用英文，避免问号问题） + 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 @@ -2148,9 +2246,10 @@ def _check_connection_health(self): def _setup_windows(self): """初始化两个显示窗口""" try: - # 前视窗口 + # 前视窗口（窗口标题使用英文，避免乱码） + front_window_name = _translate_to_english(config.DISPLAY['FRONT_VIEW_WINDOW']['NAME']) self.front_window = FrontViewWindow( - window_name=f"{config.DISPLAY['FRONT_VIEW_WINDOW']['NAME']} - {self.drone_name or 'AirSimNH'}", + 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'], @@ -2158,9 +2257,10 @@ def _setup_windows(self): ) self.logger.info("🎥 前视窗口已初始化") - # 信息显示窗口 + # 信息显示窗口（窗口标题使用英文，避免乱码） + info_window_name = _translate_to_english(config.DISPLAY['INFO_WINDOW']['NAME']) self.info_window = InfoDisplayWindow( - window_name=f"{config.DISPLAY['INFO_WINDOW']['NAME']} - {self.drone_name or 'AirSimNH'}", + window_name=f"{info_window_name} - {self.drone_name or 'AirSimNH'}", width=config.DISPLAY['INFO_WINDOW']['WIDTH'], height=config.DISPLAY['INFO_WINDOW']['HEIGHT'] ) @@ -2184,9 +2284,14 @@ def _update_info_window(self, perception: PerceptionResult): 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) + info_data = { 'timestamp': datetime.now().strftime("%H:%M:%S"), - 'state': self.state.value, + 'state': state_value, 'position': (pos.x_val, pos.y_val, pos.z_val), 'perception': { 'obstacle_distance': perception.obstacle_distance, From 453cb3058ffe4b6002628fd58de728b609172641 Mon Sep 17 00:00:00 2001 From: rhcgg <3136924319@qq.com> Date: Thu, 25 Dec 2025 10:13:09 +0800 Subject: [PATCH 19/23] =?UTF-8?q?=E5=89=8D=E8=A7=86=E7=AA=97=E5=8F=A3?= =?UTF-8?q?=E4=BC=98=E5=8C=96?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit 前视窗口简化功能，只用来前视和手动控制不再显示提示， --- .../perceptive_drone_explorer.py | 110 +++++++----------- 1 file changed, 42 insertions(+), 68 deletions(-) diff --git a/src/drone_perception/perceptive_drone_explorer.py b/src/drone_perception/perceptive_drone_explorer.py index 3391236fb8..43ce60d3dc 100644 --- a/src/drone_perception/perceptive_drone_explorer.py +++ b/src/drone_perception/perceptive_drone_explorer.py @@ -31,6 +31,7 @@ import psutil import os import gc +import platform # 导入PIL用于中文文本绘制 try: @@ -1504,8 +1505,8 @@ 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((300, 400, 3), dtype=np.uint8) - wait_img = put_chinese_text(wait_img, "等待无人机图像...", (50, 150), 24, (255, 255, 255), 2) + # 等待画面：只显示黑色背景，不显示任何文字 + wait_img = np.zeros((self.window_height, self.window_width, 3), dtype=np.uint8) cv2.imshow(self.window_name, wait_img) cv2.waitKey(100) @@ -1541,14 +1542,11 @@ def _display_loop(self): pass if display_image is not None: - if self.show_info: - display_image = self._add_info_overlay(display_image, info, manual_info) - + # 前视窗口只显示纯图像，不添加任何信息叠加 cv2.imshow(self.window_name, display_image) elif self.paused: - blank = np.zeros((300, 400, 3), dtype=np.uint8) - cv2.putText(blank, "PAUSED", (120, 150), - cv2.FONT_HERSHEY_SIMPLEX, 1.5, (0, 0, 255), 3) + # 暂停时也显示黑色背景，不显示任何文字 + 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 @@ -1632,62 +1630,9 @@ def _update_stats(self): self.display_stats['last_update'] = now def _add_info_overlay(self, image: np.ndarray, info: Dict, manual_info: List[str] = None) -> np.ndarray: - if image is None or image.size == 0: - return image - - try: - overlay = image.copy() - height, width = image.shape[:2] - - is_manual = info.get('state', '') == "手动控制" - - info_height = 180 if is_manual and manual_info else 100 - - cv2.rectangle(overlay, (0, 0), (width, info_height), (0, 0, 0), -1) - cv2.addWeighted(overlay, 0.7, image, 0.3, 0, image) - - state = info.get('state', 'UNKNOWN') - state_color = (0, 255, 0) if '探索' in state else (0, 255, 255) if '悬停' in state else (255, 255, 0) if '手动' in state else (0, 0, 255) - image = put_chinese_text(image, f"状态: {state}", (10, 30), 21, state_color, 2) - - pos = info.get('position', (0, 0, 0)) - image = put_chinese_text(image, f"位置: ({pos[0]:.1f}, {pos[1]:.1f}, {-pos[2]:.1f}m)", (10, 60), 18, (255, 255, 255), 1) - - red_objects_count = info.get('red_objects_count', 0) - red_objects_visited = info.get('red_objects_visited', 0) - blue_objects_count = info.get('blue_objects_count', 0) - blue_objects_visited = info.get('blue_objects_visited', 0) - black_objects_count = info.get('black_objects_count', 0) - black_objects_visited = info.get('black_objects_visited', 0) - - if red_objects_count > 0 or blue_objects_count > 0 or black_objects_count > 0: - red_text = f"红色物体: {red_objects_visited}/{red_objects_count}" - blue_text = f"蓝色物体: {blue_objects_visited}/{blue_objects_count}" - black_text = f"黑色物体: {black_objects_visited}/{black_objects_count}" - image = put_chinese_text(image, red_text, (10, 90), 18, (0, 100, 255), 2) - image = put_chinese_text(image, blue_text, (10, 110), 18, (255, 100, 0), 2) - image = put_chinese_text(image, black_text, (10, 130), 18, (128, 128, 128), 2) - - if is_manual and manual_info: - y_start = 170 if (red_objects_count > 0 or blue_objects_count > 0 or black_objects_count > 0) else 100 - for i, line in enumerate(manual_info): - y_pos = y_start + i * 20 - image = put_chinese_text(image, line, (10, y_pos), 15, (200, 255, 200), 1) - - image = put_chinese_text(image, "手动控制中...", (width - 150, 60), 18, (255, 255, 0), 1) - elif not is_manual and red_objects_count == 0 and blue_objects_count == 0 and black_objects_count == 0: - obs_dist = info.get('obstacle_distance', 0.0) - obs_color = (0, 0, 255) if obs_dist < 5.0 else (0, 165, 255) if obs_dist < 10.0 else (0, 255, 0) - image = put_chinese_text(image, f"障碍: {obs_dist:.1f}m", (10, 90), 21, obs_color, 2) - - fps_text = f"FPS: {self.display_stats['fps']:.1f}" - cv2.putText(image, fps_text, (width - 120, 30), - cv2.FONT_HERSHEY_SIMPLEX, 0.6, (255, 255, 0), 1) - - return image - except Exception as e: - print(f"⚠️ 添加信息叠加层出错: {e}") - return image + """前视窗口不再显示任何信息叠加，只显示纯图像""" + # 直接返回原始图像，不添加任何文字或叠加层 + return image def _save_screenshot(self, image: Optional[np.ndarray]): if image is not None and image.size > 0: @@ -1964,7 +1909,18 @@ def _render_info_display(self, info_data: Dict) -> np.ndarray: img = put_chinese_text(img, f"循环时间: {loop_time:.1f}ms", (x_offset, y_offset), 18, loop_color, 1) y_offset += 25 - # 7. 探索网格图像（右侧） + # 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: @@ -2017,13 +1973,13 @@ def _render_info_display(self, info_data: Dict) -> np.ndarray: # 将网格图像放到主图像上 img[grid_y:grid_y+grid_size, grid_x:grid_x+grid_size] = grid_resized - # 8. 时间戳 + # 9. 时间戳 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) - # 9. 底部提示 + # 10. 底部提示 hint_text = "按 Q 或 ESC 关闭窗口" img = put_chinese_text(img, hint_text, (self.window_width // 2 - 80, self.window_height - 30), 15, text_color, 1) @@ -2289,6 +2245,11 @@ def _update_info_window(self, perception: PerceptionResult): 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, @@ -2318,7 +2279,8 @@ def _update_info_window(self, perception: PerceptionResult): 'cpu_usage': cpu_usage, 'memory_usage': memory_usage, 'loop_time': self.stats.get('average_loop_time', 0) - } + }, + 'manual_info': manual_info # 添加手动控制信息 } # 添加网格图像 @@ -3440,6 +3402,18 @@ def run_manual_control(self): 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 From 7a8f3d74a7cd22b3caa1ddfd4d05436a18a2eeeb Mon Sep 17 00:00:00 2001 From: rhcgg <3136924319@qq.com> Date: Thu, 25 Dec 2025 20:52:41 +0800 Subject: [PATCH 20/23] =?UTF-8?q?=E5=8A=9F=E8=83=BD=E4=BC=98=E5=8C=96?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit 在信息窗口增加运动轨迹图， --- src/drone_perception/config.py | 6 + .../perceptive_drone_explorer.py | 213 +++++++++++++++++- 2 files changed, 216 insertions(+), 3 deletions(-) diff --git a/src/drone_perception/config.py b/src/drone_perception/config.py index 5896c8503a..e647d453bb 100644 --- a/src/drone_perception/config.py +++ b/src/drone_perception/config.py @@ -169,6 +169,12 @@ '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), # 起始位置颜色（黄色） } } diff --git a/src/drone_perception/perceptive_drone_explorer.py b/src/drone_perception/perceptive_drone_explorer.py index 43ce60d3dc..94a2e5f633 100644 --- a/src/drone_perception/perceptive_drone_explorer.py +++ b/src/drone_perception/perceptive_drone_explorer.py @@ -82,7 +82,11 @@ class DefaultConfig: '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_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} @@ -1664,6 +1668,10 @@ def __init__(self, window_name=None, width=None, height=None): 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): @@ -1689,6 +1697,15 @@ def update_info(self, info_data: Dict): 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() @@ -1776,6 +1793,119 @@ def _create_waiting_screen(self): 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: @@ -1973,13 +2103,90 @@ def _render_info_display(self, info_data: Dict) -> np.ndarray: # 将网格图像放到主图像上 img[grid_y:grid_y+grid_size, grid_x:grid_x+grid_size] = grid_resized - # 9. 时间戳 + # 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) - # 10. 底部提示 + # 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) From 18c3e2a6133928bd1c687d4829264420360c6592 Mon Sep 17 00:00:00 2001 From: rhcgg <3136924319@qq.com> Date: Fri, 26 Dec 2025 17:26:42 +0800 Subject: [PATCH 21/23] =?UTF-8?q?=E4=BB=A3=E7=A0=81=E7=A8=8B=E5=BA=8F?= =?UTF-8?q?=E4=BC=98=E5=8C=96?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit 代码程序优化 --- .../perceptive_drone_explorer.py | 507 +++++++----------- 1 file changed, 188 insertions(+), 319 deletions(-) diff --git a/src/drone_perception/perceptive_drone_explorer.py b/src/drone_perception/perceptive_drone_explorer.py index 94a2e5f633..1a01ec04aa 100644 --- a/src/drone_perception/perceptive_drone_explorer.py +++ b/src/drone_perception/perceptive_drone_explorer.py @@ -2501,31 +2501,119 @@ def _update_info_window(self, perception: PerceptionResult): 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 = [] + def _detect_color_objects_generic( + self, + image: np.ndarray, + color_type: str, + depth_array: Optional[np.ndarray] = None + ) -> Tuple[List[Any], np.ndarray]: + """ + 通用的颜色物体检测函数 + + Args: + image: 输入图像 + color_type: 颜色类型 ('red', 'blue', 'black') + depth_array: 深度数组（可选） + + Returns: + (检测到的物体列表, 标记后的图像) + """ + # 颜色类型配置映射 + COLOR_CONFIG = { + 'red': { + 'detection_config': config.PERCEPTION['RED_OBJECT_DETECTION'], + 'color_range_config': config.CAMERA['RED_COLOR_RANGE'], + 'object_class': RedObject, + 'object_list_attr': 'red_objects', + 'id_counter_attr': 'red_object_id_counter', + 'memory_time_attr': 'red_object_memory_time', + 'stats_key': 'red_objects_detected', + 'log_emoji': '🔴', + 'log_name': '红色物体', + 'mark_color': (0, 100, 255), # BGR格式 + 'label_prefix': 'R:', + 'record_func': 'record_red_object', + 'record_enabled_config': 'RECORD_RED_OBJECTS', + 'has_dual_range': True, # 红色需要两个颜色范围 + 'is_same_func': self._is_same_object, + }, + 'blue': { + 'detection_config': config.PERCEPTION['BLUE_OBJECT_DETECTION'], + 'color_range_config': config.CAMERA['BLUE_COLOR_RANGE'], + 'object_class': BlueObject, + 'object_list_attr': 'blue_objects', + 'id_counter_attr': 'blue_object_id_counter', + 'memory_time_attr': 'blue_object_memory_time', + 'stats_key': 'blue_objects_detected', + 'log_emoji': '🔵', + 'log_name': '蓝色物体', + 'mark_color': (255, 100, 0), # BGR格式 + 'label_prefix': 'B:', + 'record_func': 'record_blue_object', + 'record_enabled_config': 'RECORD_BLUE_OBJECTS', + 'has_dual_range': False, + 'is_same_func': self._is_same_object_blue, + }, + 'black': { + 'detection_config': config.PERCEPTION['BLACK_OBJECT_DETECTION'], + 'color_range_config': config.CAMERA['BLACK_COLOR_RANGE'], + 'object_class': BlackObject, + 'object_list_attr': 'black_objects', + 'id_counter_attr': 'black_object_id_counter', + 'memory_time_attr': 'black_object_memory_time', + 'stats_key': 'black_objects_detected', + 'log_emoji': '⚫', + 'log_name': '黑色物体', + 'mark_color': (128, 128, 128), # BGR格式 + 'label_prefix': 'K:', + 'record_func': 'record_black_object', + 'record_enabled_config': 'RECORD_BLACK_OBJECTS', + 'has_dual_range': False, + 'is_same_func': self._is_same_object_black, + } + } + + if color_type not in COLOR_CONFIG: + raise ValueError(f"不支持的颜色类型: {color_type}，支持的类型: {list(COLOR_CONFIG.keys())}") + + cfg = COLOR_CONFIG[color_type] + detected_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 - + + # 检查是否启用检测 + if not cfg['detection_config']['ENABLED'] or image is None: + return detected_objects, marked_image + try: + # 转换为HSV颜色空间 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) - + + # 创建颜色掩码（红色需要特殊处理，有两个范围） + if cfg['has_dual_range']: + # 红色：两个颜色范围 + lower1 = np.array(cfg['color_range_config']['LOWER1']) + upper1 = np.array(cfg['color_range_config']['UPPER1']) + lower2 = np.array(cfg['color_range_config']['LOWER2']) + upper2 = np.array(cfg['color_range_config']['UPPER2']) + + mask1 = cv2.inRange(hsv, lower1, upper1) + mask2 = cv2.inRange(hsv, lower2, upper2) + color_mask = cv2.bitwise_or(mask1, mask2) + else: + # 其他颜色：单个颜色范围 + lower = np.array(cfg['color_range_config']['LOWER']) + upper = np.array(cfg['color_range_config']['UPPER']) + color_mask = cv2.inRange(hsv, lower, upper) + + # 形态学操作 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) - + color_mask = cv2.morphologyEx(color_mask, cv2.MORPH_CLOSE, kernel) + color_mask = cv2.morphologyEx(color_mask, cv2.MORPH_OPEN, kernel) + + # 查找轮廓 + contours, _ = cv2.findContours(color_mask, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE) + + # 获取无人机状态 try: state = self.client.getMultirotorState(vehicle_name=self.drone_name) drone_pos = state.kinematics_estimated.position @@ -2534,47 +2622,56 @@ def _detect_red_objects(self, image: np.ndarray, depth_array: Optional[np.ndarra except: drone_pos = None yaw = 0.0 - + + # 获取配置参数 + min_area = cfg['detection_config']['MIN_AREA'] + max_area = cfg['detection_config']['MAX_AREA'] + existing_objects = getattr(self, cfg['object_list_attr']) + id_counter_attr = cfg['id_counter_attr'] + + # 处理每个轮廓 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, + + # 创建物体对象 + current_id = getattr(self, id_counter_attr) + color_object = cfg['object_class']( + id=current_id, position=world_pos if world_pos else (0.0, 0.0, 0.0), pixel_position=(center_x, center_y), size=area, @@ -2583,308 +2680,80 @@ def _detect_red_objects(self, image: np.ndarray, depth_array: Optional[np.ndarra 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): + for existing_obj in existing_objects: + if cfg['is_same_func'](color_object, existing_obj): existing_obj.last_seen = time.time() - existing_obj.pixel_position = red_object.pixel_position + existing_obj.pixel_position = color_object.pixel_position existing_obj.confidence = max(existing_obj.confidence, confidence) if world_pos: existing_obj.position = world_pos - red_object = existing_obj + color_object = existing_obj is_new_object = False break - + + # 如果是新物体，增加ID计数器和统计 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) + setattr(self, id_counter_attr, current_id + 1) + detected_objects.append(color_object) + self.stats[cfg['stats_key']] += 1 + self.logger.info(f"{cfg['log_emoji']} 检测到{cfg['log_name']} #{color_object.id} (置信度: {confidence:.2f})") + + # 记录到数据日志 + if self.data_logger and config.DATA_RECORDING.get(cfg['record_enabled_config'], False): + record_func = getattr(self.data_logger, cfg['record_func'], None) + if record_func: + record_func(color_object) else: - red_objects.append(red_object) - + detected_objects.append(color_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})" + mark_color = cfg['mark_color'] + if color_object.visited: + mark_color = (0, 200, 0) # 已访问的物体显示为绿色 + + cv2.rectangle(marked_image, (x, y), (x+w, y+h), mark_color, 2) + cv2.circle(marked_image, (center_x, center_y), 5, mark_color, -1) + + label = f"{cfg['label_prefix']}{color_object.id} ({confidence:.2f})" cv2.putText(marked_image, label, (x, y-10), - cv2.FONT_HERSHEY_SIMPLEX, 0.5, color, 1) - + cv2.FONT_HERSHEY_SIMPLEX, 0.5, mark_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}个") - + memory_time = getattr(self, cfg['memory_time_attr']) + filtered_objects = [obj for obj in existing_objects + if current_time - obj.last_seen < memory_time] + setattr(self, cfg['object_list_attr'], filtered_objects) + + # 更新统计信息 + visited_count = sum(1 for obj in filtered_objects if obj.visited) + if len(detected_objects) > 0: + self.logger.debug(f"{cfg['log_emoji']} 当前{cfg['log_name']}: {len(filtered_objects)}个, 已访问: {visited_count}个") + except Exception as e: - self.logger.warning(f"⚠️ 红色物体检测失败: {e}") + cfg = COLOR_CONFIG[color_type] + self.logger.warning(f"⚠️ {cfg['log_name']}检测失败: {e}") + + return detected_objects, marked_image - return red_objects, marked_image + def _detect_red_objects(self, image: np.ndarray, depth_array: Optional[np.ndarray] = None) -> Tuple[List[RedObject], np.ndarray]: + """检测红色物体 - 使用通用检测函数""" + objects, marked_image = self._detect_color_objects_generic(image, 'red', depth_array) + return 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 + """检测蓝色物体 - 使用通用检测函数""" + objects, marked_image = self._detect_color_objects_generic(image, 'blue', depth_array) + return 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 + """检测黑色物体 - 使用通用检测函数""" + objects, marked_image = self._detect_color_objects_generic(image, 'black', depth_array) + return 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): From b82c10426453fab3c655accff4209689a9ae9f39 Mon Sep 17 00:00:00 2001 From: rhcgg <3136924319@qq.com> Date: Sat, 27 Dec 2025 16:43:21 +0800 Subject: [PATCH 22/23] =?UTF-8?q?=E4=BB=A3=E7=A0=81=E6=94=B9=E8=BF=9B?= =?UTF-8?q?=E7=BB=9F?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit 代码改进统 --- .../perceptive_drone_explorer.py | 202 +++++++++--------- 1 file changed, 102 insertions(+), 100 deletions(-) diff --git a/src/drone_perception/perceptive_drone_explorer.py b/src/drone_perception/perceptive_drone_explorer.py index 1a01ec04aa..2930d37b1f 100644 --- a/src/drone_perception/perceptive_drone_explorer.py +++ b/src/drone_perception/perceptive_drone_explorer.py @@ -2535,7 +2535,7 @@ def _detect_color_objects_generic( 'record_func': 'record_red_object', 'record_enabled_config': 'RECORD_RED_OBJECTS', 'has_dual_range': True, # 红色需要两个颜色范围 - 'is_same_func': self._is_same_object, + 'is_same_func': self._is_same_object_generic, }, 'blue': { 'detection_config': config.PERCEPTION['BLUE_OBJECT_DETECTION'], @@ -2552,7 +2552,7 @@ def _detect_color_objects_generic( 'record_func': 'record_blue_object', 'record_enabled_config': 'RECORD_BLUE_OBJECTS', 'has_dual_range': False, - 'is_same_func': self._is_same_object_blue, + 'is_same_func': self._is_same_object_generic, }, 'black': { 'detection_config': config.PERCEPTION['BLACK_OBJECT_DETECTION'], @@ -2569,7 +2569,7 @@ def _detect_color_objects_generic( 'record_func': 'record_black_object', 'record_enabled_config': 'RECORD_BLACK_OBJECTS', 'has_dual_range': False, - 'is_same_func': self._is_same_object_black, + 'is_same_func': self._is_same_object_generic, } } @@ -2755,7 +2755,20 @@ def _detect_black_objects(self, image: np.ndarray, depth_array: Optional[np.ndar objects, marked_image = self._detect_color_objects_generic(image, 'black', depth_array) return objects, marked_image - def _is_same_object(self, obj1: RedObject, obj2: RedObject, distance_threshold=2.0) -> bool: + def _is_same_object_generic(self, obj1: Any, obj2: Any, distance_threshold=2.0) -> bool: + """ + 通用的物体相似度判断函数 + 适用于RedObject、BlueObject、BlackObject等具有相同结构的对象 + + Args: + obj1: 第一个物体对象 + obj2: 第二个物体对象 + distance_threshold: 距离阈值（米），默认2.0米 + + Returns: + bool: 如果两个物体被认为是同一个物体，返回True + """ + # 如果两个物体都有有效的世界坐标位置，使用世界坐标距离判断 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 + @@ -2763,62 +2776,98 @@ def _is_same_object(self, obj1: RedObject, obj2: RedObject, distance_threshold=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) + # 像素距离小于50且时间差小于5秒，认为是同一个物体 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) + def _is_same_object(self, obj1: RedObject, obj2: RedObject, distance_threshold=2.0) -> bool: + """检测红色物体是否相同 - 使用通用函数""" + return self._is_same_object_generic(obj1, obj2, distance_threshold) - return pixel_distance < 50 and time_diff < 5.0 + def _is_same_object_blue(self, obj1: BlueObject, obj2: BlueObject, distance_threshold=2.0) -> bool: + """检测蓝色物体是否相同 - 使用通用函数""" + return self._is_same_object_generic(obj1, obj2, distance_threshold) 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 + """检测黑色物体是否相同 - 使用通用函数""" + return self._is_same_object_generic(obj1, obj2, 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: + def _check_object_proximity_generic(self, current_pos: Tuple[float, float], color_type: str) -> bool: + """ + 通用的物体接近检测函数 + 检查当前位置是否接近指定颜色类型的未访问物体 + + Args: + current_pos: 当前位置 (x, y) + color_type: 颜色类型 ('red', 'blue', 'black') + + Returns: + bool: 如果检测到接近物体并触发了访问，返回True；否则返回False + """ + # 颜色类型配置映射 + PROXIMITY_CONFIG = { + 'red': { + 'objects_attr': 'red_objects', + 'exploration_config': config.INTELLIGENT_DECISION['RED_OBJECT_EXPLORATION'], + 'stats_key': 'red_objects_visited', + 'log_name': '红色物体', + 'event_type': 'red_object_visited', + 'inspection_state': FlightState.RED_OBJECT_INSPECTION, + }, + 'blue': { + 'objects_attr': 'blue_objects', + 'exploration_config': config.INTELLIGENT_DECISION['BLUE_OBJECT_EXPLORATION'], + 'stats_key': 'blue_objects_visited', + 'log_name': '蓝色物体', + 'event_type': 'blue_object_visited', + 'inspection_state': FlightState.BLUE_OBJECT_INSPECTION, + }, + 'black': { + 'objects_attr': 'black_objects', + 'exploration_config': config.INTELLIGENT_DECISION['BLACK_OBJECT_EXPLORATION'], + 'stats_key': 'black_objects_visited', + 'log_name': '黑色物体', + 'event_type': 'black_object_visited', + 'inspection_state': FlightState.BLACK_OBJECT_INSPECTION, + } + } + + if color_type not in PROXIMITY_CONFIG: + raise ValueError(f"不支持的颜色类型: {color_type}，支持的类型: {list(PROXIMITY_CONFIG.keys())}") + + cfg = PROXIMITY_CONFIG[color_type] + + # 获取物体列表 + objects = getattr(self, cfg['objects_attr']) + + # 遍历所有未访问的物体 + for obj in 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'] + + # 获取最小接近距离 + min_distance = cfg['exploration_config']['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)") - + self.stats[cfg['stats_key']] += 1 + + # 记录日志 + self.logger.info(f"✅ 已访问{cfg['log_name']} #{obj.id} (距离: {distance:.1f}m)") + + # 记录事件到数据日志 if self.data_logger: event_data = { 'object_id': obj.id, @@ -2826,72 +2875,25 @@ def _check_red_object_proximity(self, current_pos): 'distance': distance, 'timestamp': time.time() } - self.data_logger.record_event('red_object_visited', event_data) - - self.change_state(FlightState.RED_OBJECT_INSPECTION) + self.data_logger.record_event(cfg['event_type'], event_data) + + # 改变状态为物体检查状态 + self.change_state(cfg['inspection_state']) 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 + def _check_red_object_proximity(self, current_pos): + """检查红色物体接近 - 使用通用函数""" + return self._check_object_proximity_generic(current_pos, 'red') - return False + def _check_blue_object_proximity(self, current_pos): + """检查蓝色物体接近 - 使用通用函数""" + return self._check_object_proximity_generic(current_pos, 'blue') 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 + """检查黑色物体接近 - 使用通用函数""" + return self._check_object_proximity_generic(current_pos, 'black') def get_depth_perception(self) -> PerceptionResult: result = PerceptionResult() From 4ee4638b73fffbc1a118c14e8bf3babdcf9b772c Mon Sep 17 00:00:00 2001 From: rhcgg <3136924319@qq.com> Date: Sat, 27 Dec 2025 23:30:29 +0800 Subject: [PATCH 23/23] =?UTF-8?q?=E9=85=8D=E7=BD=AE=E7=AE=A1=E7=90=86?= =?UTF-8?q?=E4=BC=98=E5=8C=96?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit 配置管理优化 --- src/drone_perception/config.py | 201 ++++++++++++++++++++++++++++++++- 1 file changed, 195 insertions(+), 6 deletions(-) diff --git a/src/drone_perception/config.py b/src/drone_perception/config.py index e647d453bb..58fc2b91b0 100644 --- a/src/drone_perception/config.py +++ b/src/drone_perception/config.py @@ -1,6 +1,25 @@ """ AirSimNH 无人机项目配置文件 所有可调参数集中在此管理 + +配置结构说明： +1. 物体检测配置：PERCEPTION['{COLOR}_OBJECT_DETECTION'] + - 使用 get_object_detection_config(color_type) 函数获取 + - 或直接访问 PERCEPTION['RED_OBJECT_DETECTION'] 等 + +2. 物体探索配置：INTELLIGENT_DECISION['{COLOR}_OBJECT_EXPLORATION'] + - 使用 get_object_exploration_config(color_type) 函数获取 + - 或直接访问 INTELLIGENT_DECISION['RED_OBJECT_EXPLORATION'] 等 + +3. 颜色范围配置：CAMERA['{COLOR}_COLOR_RANGE'] + - 使用 get_color_range_config(color_type) 函数获取 + - 或直接访问 CAMERA['RED_COLOR_RANGE'] 等 + +扩展新颜色类型： + 1. 在 PERCEPTION 中添加 '{COLOR}_OBJECT_DETECTION' 配置 + 2. 在 INTELLIGENT_DECISION 中添加 '{COLOR}_OBJECT_EXPLORATION' 配置 + 3. 在 CAMERA 中添加 '{COLOR}_COLOR_RANGE' 配置 + 4. 使用提供的辅助函数 create_object_detection_config() 和 create_object_exploration_config() """ import math @@ -34,6 +53,23 @@ 'OPENNESS_THRESHOLD': 0.7, # 开阔度阈值，大于此值认为开阔 }, + # ========== 物体检测配置说明 ========== + # 所有颜色类型的物体检测配置使用统一结构，包含以下字段： + # - ENABLED: 是否启用检测 (bool) + # - MIN_AREA: 最小检测面积（像素）(int) + # - MAX_AREA: 最大检测面积（像素）(int) + # - UPDATE_INTERVAL: 检测更新间隔（秒）(float) + # - MEMORY_TIME: 物体记忆时间（秒）(float) + # + # 扩展新颜色类型时，可以参考以下模板或使用 create_object_detection_config() 函数 + '_OBJECT_DETECTION_TEMPLATE': { + 'ENABLED': True, # 启用物体检测 + 'MIN_AREA': 50, # 最小检测面积（像素） + 'MAX_AREA': 10000, # 最大检测面积（像素） + 'UPDATE_INTERVAL': 1.0, # 检测更新间隔（秒） + 'MEMORY_TIME': 5.0, # 物体记忆时间（秒），避免重复计数 + }, + # 红色物体检测参数 'RED_OBJECT_DETECTION': { 'ENABLED': True, # 启用红色物体检测 @@ -43,7 +79,7 @@ 'MEMORY_TIME': 5.0, # 物体记忆时间（秒），避免重复计数 }, - # 蓝色物体检测参数（新增） + # 蓝色物体检测参数 'BLUE_OBJECT_DETECTION': { 'ENABLED': True, # 启用蓝色物体检测 'MIN_AREA': 50, # 最小检测面积（像素） @@ -52,7 +88,7 @@ 'MEMORY_TIME': 5.0, # 物体记忆时间（秒） }, - # 黑色物体检测参数（新增） + # 黑色物体检测参数 'BLACK_OBJECT_DETECTION': { 'ENABLED': True, # 启用黑色物体检测 'MIN_AREA': 50, # 最小检测面积（像素） @@ -97,7 +133,22 @@ 'TARGET_LIFETIME': 15.0, # 目标有效期 (秒) 'TARGET_REACHED_DISTANCE': 3.0, # 目标到达判定距离 (米) - # 红色物体探索参数 + # ========== 物体探索配置说明 ========== + # 所有颜色类型的物体探索配置使用统一结构，包含以下字段： + # - ATTRACTION_GAIN: 物体吸引力增益 (float) + # - DETECTION_RADIUS: 检测半径（米）(float) + # - MIN_DISTANCE: 最小接近距离（米）(float) + # - EXPLORATION_BONUS: 探索奖励分数 (float) + # + # 扩展新颜色类型时，可以参考以下模板或使用 create_object_exploration_config() 函数 + '_OBJECT_EXPLORATION_TEMPLATE': { + 'ATTRACTION_GAIN': 1.0, # 物体吸引力增益 + 'DETECTION_RADIUS': 8.0, # 检测半径（米） + 'MIN_DISTANCE': 2.0, # 最小接近距离（米） + 'EXPLORATION_BONUS': 0.2, # 探索奖励分数 + }, + + # 红色物体探索参数（优先级最高） 'RED_OBJECT_EXPLORATION': { 'ATTRACTION_GAIN': 1.5, # 红色物体吸引力增益 'DETECTION_RADIUS': 10.0, # 检测半径（米） @@ -105,7 +156,7 @@ 'EXPLORATION_BONUS': 0.5, # 探索奖励分数 }, - # 蓝色物体探索参数（新增） + # 蓝色物体探索参数 'BLUE_OBJECT_EXPLORATION': { 'ATTRACTION_GAIN': 1.2, # 蓝色物体吸引力增益 'DETECTION_RADIUS': 8.0, # 检测半径（米） @@ -113,7 +164,7 @@ 'EXPLORATION_BONUS': 0.3, # 探索奖励分数 }, - # 黑色物体探索参数（新增） + # 黑色物体探索参数 'BLACK_OBJECT_EXPLORATION': { 'ATTRACTION_GAIN': 1.0, # 黑色物体吸引力增益 'DETECTION_RADIUS': 8.0, # 检测半径（米） @@ -266,4 +317,142 @@ 'REPORT_INTERVAL': 30.0, # 性能报告间隔（秒） # 内存优化参数 'MAX_METRICS_BUFFER': 500, # 最大性能指标缓冲区大小（个），减少内存占用 -} \ No newline at end of file +} + +# ==================== 配置辅助函数 ==================== +def get_object_detection_config(color_type: str) -> dict: + """ + 获取指定颜色类型的物体检测配置 + + Args: + color_type: 颜色类型 ('red', 'blue', 'black') + + Returns: + dict: 物体检测配置字典 + + Example: + config = get_object_detection_config('red') + # 返回 PERCEPTION['RED_OBJECT_DETECTION'] + """ + config_key = f'{color_type.upper()}_OBJECT_DETECTION' + if config_key not in PERCEPTION: + raise ValueError(f"不支持的颜色类型: {color_type}，支持的类型: ['red', 'blue', 'black']") + return PERCEPTION[config_key] + + +def get_object_exploration_config(color_type: str) -> dict: + """ + 获取指定颜色类型的物体探索配置 + + Args: + color_type: 颜色类型 ('red', 'blue', 'black') + + Returns: + dict: 物体探索配置字典 + + Example: + config = get_object_exploration_config('blue') + # 返回 INTELLIGENT_DECISION['BLUE_OBJECT_EXPLORATION'] + """ + config_key = f'{color_type.upper()}_OBJECT_EXPLORATION' + if config_key not in INTELLIGENT_DECISION: + raise ValueError(f"不支持的颜色类型: {color_type}，支持的类型: ['red', 'blue', 'black']") + return INTELLIGENT_DECISION[config_key] + + +def get_color_range_config(color_type: str) -> dict: + """ + 获取指定颜色类型的颜色范围配置 + + Args: + color_type: 颜色类型 ('red', 'blue', 'black') + + Returns: + dict: 颜色范围配置字典 + + Example: + config = get_color_range_config('red') + # 返回 CAMERA['RED_COLOR_RANGE'] + """ + config_key = f'{color_type.upper()}_COLOR_RANGE' + if config_key not in CAMERA: + raise ValueError(f"不支持的颜色类型: {color_type}，支持的类型: ['red', 'blue', 'black']") + return CAMERA[config_key] + + +def get_all_color_types() -> list: + """ + 获取所有支持的颜色类型列表 + + Returns: + list: 支持的颜色类型列表 ['red', 'blue', 'black'] + """ + return ['red', 'blue', 'black'] + + +def create_object_detection_config(enabled: bool = True, min_area: int = 50, + max_area: int = 10000, update_interval: float = 1.0, + memory_time: float = 5.0) -> dict: + """ + 创建新的物体检测配置（用于扩展新颜色类型） + + Args: + enabled: 是否启用检测 + min_area: 最小检测面积（像素） + max_area: 最大检测面积（像素） + update_interval: 检测更新间隔（秒） + memory_time: 物体记忆时间（秒） + + Returns: + dict: 新的物体检测配置字典 + + Example: + # 添加黄色物体检测配置 + PERCEPTION['YELLOW_OBJECT_DETECTION'] = create_object_detection_config( + enabled=True, + min_area=50, + max_area=10000, + update_interval=1.0, + memory_time=5.0 + ) + """ + return { + 'ENABLED': enabled, + 'MIN_AREA': min_area, + 'MAX_AREA': max_area, + 'UPDATE_INTERVAL': update_interval, + 'MEMORY_TIME': memory_time, + } + + +def create_object_exploration_config(attraction_gain: float = 1.0, + detection_radius: float = 8.0, + min_distance: float = 2.0, + exploration_bonus: float = 0.2) -> dict: + """ + 创建新的物体探索配置（用于扩展新颜色类型） + + Args: + attraction_gain: 吸引力增益 + detection_radius: 检测半径（米） + min_distance: 最小接近距离（米） + exploration_bonus: 探索奖励分数 + + Returns: + dict: 新的物体探索配置字典 + + Example: + # 添加黄色物体探索配置 + INTELLIGENT_DECISION['YELLOW_OBJECT_EXPLORATION'] = create_object_exploration_config( + attraction_gain=1.3, + detection_radius=9.0, + min_distance=2.0, + exploration_bonus=0.4 + ) + """ + return { + 'ATTRACTION_GAIN': attraction_gain, + 'DETECTION_RADIUS': detection_radius, + 'MIN_DISTANCE': min_distance, + 'EXPLORATION_BONUS': exploration_bonus, + } \ No newline at end of file