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':