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()