diff --git a/src/Drone_hand_gesture_project/drone_controller.py b/src/Drone_hand_gesture_project/drone_controller.py index fd4c2f28e6..d8218a34f0 100644 --- a/src/Drone_hand_gesture_project/drone_controller.py +++ b/src/Drone_hand_gesture_project/drone_controller.py @@ -1,5 +1,6 @@ import time -from pymavlink import mavutil +import math +import numpy as np class DroneController: @@ -8,29 +9,262 @@ def __init__(self, simulation_mode=True): self.connected = False self.master = None + # 无人机状态 + self.state = { + 'position': np.array([0.0, 0.0, 0.0]), # [x, y, z] + 'velocity': np.array([0.0, 0.0, 0.0]), + 'orientation': np.array([0.0, 0.0, 0.0]), # [roll, pitch, yaw] + 'battery': 100.0, + 'armed': False, + 'mode': 'DISARMED' + } + + # 控制参数 + self.control_params = { + 'max_speed': 2.0, # 最大速度 m/s + 'max_altitude': 10.0, # 最大高度 m + 'takeoff_altitude': 2.0, # 起飞高度 m + 'hover_threshold': 0.1, # 悬停阈值 + 'battery_drain_rate': 0.01, # 电池消耗率（每分钟百分比） + } + + # 轨迹记录 + self.trajectory = [] + self.max_trajectory_points = 500 + + # 如果连接真实无人机（保留原有功能） if not simulation_mode: - self._connect_to_drone() + self._connect_to_real_drone() - def _connect_to_drone(self): + def _connect_to_real_drone(self): + """连接真实无人机（延迟导入pymavlink）""" try: + # 延迟导入，避免仿真模式下也需要pymavlink + from pymavlink import mavutil self.master = mavutil.mavlink_connection('udp:127.0.0.1:14540') self.master.wait_heartbeat() print("成功连接到无人机仿真器！") self.connected = True + except ImportError as e: + print(f"未安装pymavlink库，自动切换到仿真模式: {e}") + print("请运行: pip install pymavlink") + self.simulation_mode = True except Exception as e: print(f"连接无人机失败: {e}") self.simulation_mode = True - def send_command(self, command): - print(f"执行命令: {command}") + def send_command(self, command, intensity=1.0): + """发送控制命令""" + print(f"[DEBUG] 收到命令: {command}, 强度: {intensity}") + print(f"[DEBUG] 当前状态: armed={self.state['armed']}, mode={self.state['mode']}") if not self.simulation_mode and self.connected: self._send_mavlink_command(command) else: - self._simulate_command(command) + self._simulate_command(command, intensity) + + def _simulate_command(self, command, intensity): + """仿真模式命令处理""" + intensity = max(0.1, min(1.0, intensity)) + + if command == "takeoff": + self._takeoff_simulation(intensity) + elif command == "land": + self._land_simulation(intensity) + elif command == "forward": + self._move_simulation('forward', intensity) + elif command == "backward": + self._move_simulation('backward', intensity) + elif command == "up": + self._move_simulation('up', intensity) + elif command == "down": + self._move_simulation('down', intensity) + elif command == "left": # 新增 + self._move_simulation('left', intensity) + elif command == "right": # 新增 + self._move_simulation('right', intensity) + elif command == "hover": + self._hover_simulation() + elif command == "stop": + self._stop_simulation() + else: + print(f"未知命令: {command}") + + def _takeoff_simulation(self, intensity): + """仿真起飞""" + print(f"[DEBUG] 执行起飞命令, 强度: {intensity}") + print(f"[DEBUG] 当前armed状态: {self.state['armed']}") + + if not self.state['armed']: + self.state['armed'] = True + self.state['mode'] = 'TAKEOFF' + # 设置目标高度和速度 + target_height = self.control_params['takeoff_altitude'] * intensity + # 设置向上的速度 + self.state['velocity'][1] = 1.0 * intensity # Y轴向上 + print(f"✅ 仿真：无人机已解锁并起飞到 {target_height:.1f} 米高度") + else: + print("⚠️ 无人机已经解锁，无需再次起飞") + + def _land_simulation(self, intensity): + """仿真降落""" + if self.state['armed']: + self.state['mode'] = 'LAND' + self.state['velocity'][1] = -1.0 * intensity # 向下速度 + print("仿真：无人机降落") + + def _move_simulation(self, direction, intensity): + """仿真移动""" + print(f"[DEBUG] 尝试移动: {direction}, 强度: {intensity}") + print(f"[DEBUG] 当前armed状态: {self.state['armed']}") + + if not self.state['armed']: + print("❌ 警告：无人机未解锁，无法移动") + print(" 请先做出'张开手掌'手势进行起飞解锁") + return + + speed = self.control_params['max_speed'] * intensity + + if direction == 'forward': + self.state['velocity'][2] = speed # 向前（Z轴正方向） + self.state['mode'] = 'FORWARD' + elif direction == 'backward': + self.state['velocity'][2] = -speed # 向后（Z轴负方向） + self.state['mode'] = 'BACKWARD' + elif direction == 'up': + self.state['velocity'][1] = speed # 向上（Y轴正方向） + self.state['mode'] = 'UP' + elif direction == 'down': + self.state['velocity'][1] = -speed # 向下（Y轴负方向） + self.state['mode'] = 'DOWN' + elif direction == 'left': + self.state['velocity'][0] = -speed # 向左（X轴负方向） + self.state['mode'] = 'LEFT' + elif direction == 'right': + self.state['velocity'][0] = speed # 向右（X轴正方向） + self.state['mode'] = 'RIGHT' + + print(f"✅ 仿真：无人机{direction}移动，速度{speed:.1f}m/s") + + def _hover_simulation(self): + """仿真悬停""" + if self.state['armed']: + self.state['velocity'] = np.array([0.0, 0.0, 0.0]) + self.state['mode'] = 'HOVER' + print("仿真：无人机悬停") + + def _stop_simulation(self): + """仿真停止""" + self.state['armed'] = False + self.state['mode'] = 'DISARMED' + self.state['velocity'] = np.array([0.0, 0.0, 0.0]) + print("仿真：无人机停止") + + def update_physics(self, dt): + """更新物理状态""" + if self.state['armed']: + # 更新位置 + self.state['position'] += self.state['velocity'] * dt + + # 自动起飞完成检测 + if self.state['mode'] == 'TAKEOFF': + target_height = self.control_params['takeoff_altitude'] + if self.state['position'][1] >= target_height: + self.state['velocity'][1] = 0.0 # 停止上升 + self.state['mode'] = 'HOVER' + print("仿真：无人机已达到目标高度，开始悬停") + + # 自动降落检测 + elif self.state['mode'] == 'LAND' and self.state['position'][1] <= 0.1: + self.state['position'][1] = 0.0 + self.state['velocity'][1] = 0.0 + self.state['armed'] = False + self.state['mode'] = 'LANDED' + print("仿真：无人机已降落") + + # 限制高度 + if self.state['position'][1] < 0: + self.state['position'][1] = 0 + self.state['velocity'][1] = max(self.state['velocity'][1], 0) # 不允许继续下降 + + if self.state['position'][1] > self.control_params['max_altitude']: + self.state['position'][1] = self.control_params['max_altitude'] + self.state['velocity'][1] = min(self.state['velocity'][1], 0) # 不允许继续上升 + + # 记录轨迹 + self._record_trajectory() + + # 消耗电池 + if self.state['battery'] > 0: + battery_drain = self.control_params['battery_drain_rate'] * dt * 60 + # 移动时消耗更多电池 + if np.linalg.norm(self.state['velocity']) > 0.1: + battery_drain *= 1.5 + self.state['battery'] -= battery_drain + + if self.state['battery'] < 0: + self.state['battery'] = 0 + self._emergency_land() + + def _record_trajectory(self): + """记录飞行轨迹""" + self.trajectory.append(tuple(self.state['position'])) + if len(self.trajectory) > self.max_trajectory_points: + self.trajectory.pop(0) + + def _emergency_land(self): + """紧急降落""" + print("警告：电池耗尽，紧急降落！") + self._land_simulation(1.0) + + def get_state(self): + """获取当前状态""" + return { + 'position': self.state['position'].copy(), + 'velocity': self.state['velocity'].copy(), + 'orientation': self.state['orientation'].copy(), + 'battery': self.state['battery'], + 'armed': self.state['armed'], + 'mode': self.state['mode'] + } + + def get_trajectory(self): + """获取飞行轨迹""" + return self.trajectory.copy() + + def get_status_string(self): + """获取状态字符串""" + pos = self.state['position'] + return (f"模式: {self.state['mode']} | " + f"位置: ({pos[0]:.1f}, {pos[1]:.1f}, {pos[2]:.1f}) | " + f"电池: {self.state['battery']:.1f}% | " + f"解锁: {'是' if self.state['armed'] else '否'}") + + def reset(self, position=None, orientation=None): + """重置无人机状态""" + if position is not None: + self.state['position'] = np.array(position) + else: + self.state['position'] = np.array([0.0, 0.0, 0.0]) + if orientation is not None: + self.state['orientation'] = np.array(orientation) + else: + self.state['orientation'] = np.array([0.0, 0.0, 0.0]) + + self.state['velocity'] = np.array([0.0, 0.0, 0.0]) + self.state['battery'] = 100.0 + self.state['armed'] = False + self.state['mode'] = 'DISARMED' + self.trajectory.clear() + print("仿真：无人机状态已重置") + + # MAVLink相关方法（只有在真实模式下才需要） def _send_mavlink_command(self, command): + """发送MAVLink命令""" try: + from pymavlink import mavutil + if command == "takeoff": self._arm_and_takeoff() elif command == "land": @@ -39,37 +273,39 @@ def _send_mavlink_command(self, command): self._set_mode("LOITER") elif command == "stop": self._set_mode("HOLD") + else: + print(f"MAVLink模式下不支持的命令: {command}") + except Exception as e: - print(f"命令执行错误: {e}") - - def _simulate_command(self, command): - commands = { - "takeoff": "无人机起飞！", - "land": "无人机降落！", - "up": "无人机上升！", - "down": "无人机下降！", - "forward": "无人机前进！", - "backward": "无人机后退！", - "hover": "无人机悬停！", - "stop": "无人机停止！", - "none": "等待指令..." - } - message = commands.get(command, f"未知命令: {command}") - print(message) + print(f"MAVLink命令执行错误: {e}") def _arm_and_takeoff(self): - self.master.mav.command_long_send( - self.master.target_system, self.master.target_component, - mavutil.mavlink.MAV_CMD_COMPONENT_ARM_DISARM, 0, 1, 0, 0, 0, 0, 0, 0) - self._set_mode("TAKEOFF") - print("无人机已解锁并起飞") + """MAVLink解锁并起飞""" + try: + from pymavlink import mavutil + self.master.mav.command_long_send( + self.master.target_system, self.master.target_component, + mavutil.mavlink.MAV_CMD_COMPONENT_ARM_DISARM, 0, 1, 0, 0, 0, 0, 0, 0) + self._set_mode("TAKEOFF") + print("真实无人机：已解锁并起飞") + except Exception as e: + print(f"起飞失败: {e}") def _land(self): - self._set_mode("LAND") - print("无人机开始降落") + """MAVLink降落""" + try: + self._set_mode("LAND") + print("真实无人机：开始降落") + except Exception as e: + print(f"降落失败: {e}") def _set_mode(self, mode): - mode_id = self.master.mode_mapping()[mode] - self.master.mav.set_mode_send( - self.master.target_system, - mavutil.mavlink.MAV_MODE_FLAG_CUSTOM_MODE_ENABLED, mode_id) \ No newline at end of file + """MAVLink设置模式""" + try: + from pymavlink import mavutil + mode_id = self.master.mode_mapping()[mode] + self.master.mav.set_mode_send( + self.master.target_system, + mavutil.mavlink.MAV_MODE_FLAG_CUSTOM_MODE_ENABLED, mode_id) + except Exception as e: + print(f"设置模式失败: {e}") \ No newline at end of file diff --git a/src/Drone_hand_gesture_project/gesture_detector.py b/src/Drone_hand_gesture_project/gesture_detector.py index 1af7a3fe2b..9bf4686865 100644 --- a/src/Drone_hand_gesture_project/gesture_detector.py +++ b/src/Drone_hand_gesture_project/gesture_detector.py @@ -23,27 +23,29 @@ def __init__(self): # 手势到控制指令的映射 self.gesture_commands = { - "open_palm": "起飞", # 张开手掌 - 起飞 - "closed_fist": "降落", # 握拳 - 降落 - "pointing_up": "上升", # 食指上指 - 上升 - "pointing_down": "下降", # 食指向下 - 下降 - "victory": "前进", # 胜利手势 - 前进 - "thumb_up": "后退", # 大拇指 - 后退 - "thumb_down": "停止", # 大拇指向下 - 停止 - "ok_sign": "悬停" # OK手势 - 悬停 + "open_palm": "takeoff", # 张开手掌 - 起飞 + "closed_fist": "land", # 握拳 - 降落 + "pointing_up": "up", # 食指上指 - 上升 + "pointing_down": "down", # 食指向下 - 下降 + "victory": "forward", # 胜利手势 - 前进 + "thumb_up": "backward", # 大拇指 - 后退 + "thumb_down": "stop", # 大拇指向下 - 停止 + "ok_sign": "hover" # OK手势 - 悬停 } - def detect_gestures(self, image): + def detect_gestures(self, image, simulation_mode=False): """ 检测图像中的手势 Args: image: 输入图像 + simulation_mode: 是否为仿真模式 Returns: processed_image: 处理后的图像 gesture: 识别到的手势 confidence: 置信度 + landmarks: 关键点坐标（仅仿真模式返回） """ # 转换颜色空间 image_rgb = cv2.cvtColor(image, cv2.COLOR_BGR2RGB) @@ -51,6 +53,7 @@ def detect_gestures(self, image): gesture = "no_hand" confidence = 0.0 + landmarks_data = None if results.multi_hand_landmarks: for hand_landmarks in results.multi_hand_landmarks: @@ -66,6 +69,10 @@ def detect_gestures(self, image): # 识别具体手势 gesture, confidence = self._classify_gesture(hand_landmarks) + # 在仿真模式下提取关键点数据 + if simulation_mode: + landmarks_data = self._get_normalized_landmarks(hand_landmarks) + # 在图像上显示手势信息 cv2.putText(image, f"Gesture: {gesture}", (10, 30), cv2.FONT_HERSHEY_SIMPLEX, 1, (0, 255, 0), 2) @@ -73,11 +80,31 @@ def detect_gestures(self, image): cv2.FONT_HERSHEY_SIMPLEX, 0.7, (0, 255, 0), 2) # 显示控制指令 - command = self.gesture_commands.get(gesture, "无") + command = self.gesture_commands.get(gesture, "none") cv2.putText(image, f"Command: {command}", (10, 110), cv2.FONT_HERSHEY_SIMPLEX, 1, (255, 0, 0), 2) - return image, gesture, confidence + return image, gesture, confidence, landmarks_data + + def _get_normalized_landmarks(self, hand_landmarks): + """ + 获取归一化的关键点坐标（用于仿真模式） + + Args: + hand_landmarks: 手部关键点 + + Returns: + list: 包含21个关键点的字典列表，每个点有x,y,z坐标 + """ + landmarks = [] + for landmark in hand_landmarks.landmark: + landmarks.append({ + 'x': landmark.x, + 'y': landmark.y, + 'z': landmark.z, + 'visibility': landmark.visibility if hasattr(landmark, 'visibility') else 1.0 + }) + return landmarks def _classify_gesture(self, landmarks): """ @@ -105,13 +132,24 @@ def _classify_gesture(self, landmarks): is_victory = self._is_victory_gesture(points) is_ok_sign = self._is_ok_sign(points) + # 调试输出 + print(f"[手势特征] " + f"拇指上:{is_thumb_up} " + f"拇指向下:{is_thumb_down} " + f"张开掌:{is_open_palm} " + f"握拳:{is_closed_fist} " + f"指上:{is_pointing_up} " + f"指下:{is_pointing_down} " + f"胜利:{is_victory} " + f"OK:{is_ok_sign}") + # 根据优先级返回手势 gestures = [ (is_thumb_up, "thumb_up", 0.95), (is_thumb_down, "thumb_down", 0.95), (is_ok_sign, "ok_sign", 0.90), (is_victory, "victory", 0.85), - (is_open_palm, "open_palm", 0.80), + (is_open_palm, "open_palm", 0.80), # 提高优先级 (is_closed_fist, "closed_fist", 0.80), (is_pointing_up, "pointing_up", 0.75), (is_pointing_down, "pointing_down", 0.75), @@ -150,15 +188,16 @@ def _is_thumb_down(self, points): def _is_open_palm(self, points): """检测张开手掌手势""" finger_tips = [8, 12, 16, 20] # 食指、中指、无名指、小指指尖 - finger_dips = [6, 10, 14, 18] # 对应的指间关节 + finger_mcps = [5, 9, 13, 17] # 掌指关节 extended_fingers = 0 - for tip, dip in zip(finger_tips, finger_dips): - if points[tip][1] < points[dip][1]: # 指尖在指间关节上方 + for tip, mcp in zip(finger_tips, finger_mcps): + # 放宽条件：指尖比掌指关节高（y值小），增加容差0.15 + if points[tip][1] < points[mcp][1] + 0.15: extended_fingers += 1 - # 至少3个手指伸直 - return extended_fingers >= 3 + # 放宽条件：至少2个手指伸直 + return extended_fingers >= 4 def _is_closed_fist(self, points): """检测握拳手势""" @@ -167,10 +206,11 @@ def _is_closed_fist(self, points): bent_fingers = 0 for tip, mcp in zip(finger_tips, finger_mcps): - if points[tip][1] > points[mcp][1]: # 指尖在掌指关节下方 + # 放宽条件：指尖在掌指关节下方或接近 + if points[tip][1] > points[mcp][1] - 0.1: bent_fingers += 1 - # 所有4个手指都弯曲 + # 放宽条件：至少3个手指弯曲 return bent_fingers >= 3 def _is_pointing_up(self, points): @@ -240,6 +280,79 @@ def get_command(self, gesture): """ return self.gesture_commands.get(gesture, "none") + def get_gesture_intensity(self, landmarks, gesture_type): + """ + 获取手势强度（用于精细控制） + + Args: + landmarks: 关键点数据 + gesture_type: 手势类型 + + Returns: + float: 手势强度 (0.0-1.0) + """ + if not landmarks or len(landmarks) < 21: + return 0.5 # 默认强度 + + if gesture_type == "pointing_up": + # 基于食指的角度计算强度 + index_tip = landmarks[8] + index_mcp = landmarks[5] + if index_tip['y'] < index_mcp['y']: + intensity = (index_mcp['y'] - index_tip['y']) * 2 + return min(max(intensity, 0.1), 1.0) + + elif gesture_type == "pointing_down": + # 基于食指的角度计算强度 + index_tip = landmarks[8] + index_mcp = landmarks[5] + if index_tip['y'] > index_mcp['y']: + intensity = (index_tip['y'] - index_mcp['y']) * 2 + return min(max(intensity, 0.1), 1.0) + + elif gesture_type in ["open_palm", "closed_fist"]: + # 基于手掌张开程度计算强度 + thumb_tip = landmarks[4] + pinky_tip = landmarks[20] + distance = math.sqrt( + (thumb_tip['x'] - pinky_tip['x']) ** 2 + + (thumb_tip['y'] - pinky_tip['y']) ** 2 + ) + if gesture_type == "open_palm": + return min(max(distance * 3, 0.1), 1.0) + else: + return min(max((0.2 - distance) * 5, 0.1), 1.0) + + return 0.5 # 默认强度 + + def get_hand_position(self, landmarks): + """ + 获取手部在画面中的位置 + + Args: + landmarks: 关键点数据 + + Returns: + dict: 包含手部中心位置和大小 + """ + if not landmarks or len(landmarks) < 21: + return None + + # 计算手部边界框 + x_coords = [p['x'] for p in landmarks] + y_coords = [p['y'] for p in landmarks] + + min_x, max_x = min(x_coords), max(x_coords) + min_y, max_y = min(y_coords), max(y_coords) + + return { + 'center_x': (min_x + max_x) / 2, + 'center_y': (min_y + max_y) / 2, + 'width': max_x - min_x, + 'height': max_y - min_y, + 'bbox': (min_x, min_y, max_x, max_y) + } + def release(self): """释放资源""" self.hands.close() \ No newline at end of file diff --git a/src/Drone_hand_gesture_project/main.py b/src/Drone_hand_gesture_project/main.py index b966155576..f42ab48f6e 100644 --- a/src/Drone_hand_gesture_project/main.py +++ b/src/Drone_hand_gesture_project/main.py @@ -1,166 +1,597 @@ +#!/usr/bin/env python3 +""" +手势控制无人机仿真系统 - 主程序（取消自动起飞版本） +集成：手势识别 + 无人机控制 + 3D仿真 +""" import cv2 import numpy as np import time +import threading import sys import os -from PIL import Image, ImageDraw, ImageFont +import json -# 添加路径 +# 添加项目路径 sys.path.append(os.path.dirname(os.path.abspath(__file__))) +# 导入自定义模块 from gesture_detector import GestureDetector from drone_controller import DroneController +from simulation_3d import Drone3DViewer +# 注意：physics_engine.py 是可选的，如果没有可以先注释掉 +try: + from physics_engine import PhysicsEngine -def cv2_add_chinese_text(img, text, position, text_color=(0, 255, 0), text_size=30): - """ - 在OpenCV图像上添加中文文字 - """ - if isinstance(img, np.ndarray): - img = Image.fromarray(cv2.cvtColor(img, cv2.COLOR_BGR2RGB)) + HAS_PHYSICS_ENGINE = True +except ImportError: + print("警告：未找到 physics_engine.py，使用简化的物理模拟") + HAS_PHYSICS_ENGINE = False - draw = ImageDraw.Draw(img) - # 尝试加载中文字体，如果失败使用默认字体 - try: - font = ImageFont.truetype("simsun.ttc", text_size, encoding="utf-8") - except: - try: - font = ImageFont.truetype("/usr/share/fonts/truetype/wqy/wqy-microhei.ttc", text_size, encoding="utf-8") - except: - font = ImageFont.load_default() +class IntegratedDroneSimulation: + """集成的无人机仿真系统""" - draw.text(position, text, text_color, font=font) + def __init__(self, config=None): + # 配置 + self.config = config or {} - return cv2.cvtColor(np.asarray(img), cv2.COLOR_RGB2BGR) + # 系统状态 + self.running = True + self.paused = False + # 初始化模块 + print("正在初始化手势检测器...") + self.gesture_detector = GestureDetector() -def create_test_frame(message="手势控制无人机 - 虚拟模式"): - """创建测试帧（支持中文）""" - # 创建白色背景 - frame = np.ones((480, 640, 3), dtype=np.uint8) * 255 + print("正在初始化无人机控制器...") + self.drone_controller = DroneController(simulation_mode=True) - # 添加标题 - frame = cv2_add_chinese_text(frame, message, (50, 50), (0, 0, 255), 30) + print("正在初始化3D仿真显示...") + self.viewer = Drone3DViewer( + width=self.config.get('window_width', 1024), + height=self.config.get('window_height', 768) + ) - # 添加手势说明 - gestures = [ - "手势指令对照表:", - "张开手掌 - 起飞", - "握拳 - 降落", - "胜利手势 - 前进", - "大拇指 - 后退", - "食指上指 - 上升", - "食指向下 - 下降", - "OK手势 - 悬停", - "大拇指向下 - 停止" - ] + # 初始化物理引擎（可选） + if HAS_PHYSICS_ENGINE: + print("正在初始化物理引擎...") + self.physics_engine = PhysicsEngine( + mass=self.config.get('drone_mass', 1.0), + gravity=self.config.get('gravity', 9.81) + ) + else: + self.physics_engine = None + + # 线程 + self.gesture_thread = None + self.simulation_thread = None + + # 数据共享 + self.current_frame = None + self.current_gesture = None + self.gesture_confidence = 0.0 + self.hand_landmarks = None + + # 控制参数（降低阈值以提高识别率） + self.control_intensity = 1.0 + self.last_command_time = time.time() + self.command_cooldown = 1.5 # 命令冷却时间（秒），从2.0降低到1.5 + + # 手势识别阈值（降低以提高灵敏度） + self.gesture_thresholds = { + 'open_palm': 0.6, # 降低到0.6 + 'closed_fist': 0.65, # 降低到0.65 + 'victory': 0.65, # 降低到0.65 + 'thumb_up': 0.65, # 降低到0.65 + 'thumb_down': 0.65, # 降低到0.65 + 'pointing_up': 0.6, # 降低到0.6 + 'pointing_down': 0.6, # 降低到0.6 + 'ok_sign': 0.7, # 稍微降低 + 'default': 0.6 # 默认阈值 + } + + # 初始化摄像头 + self.cap = self._initialize_camera() + + # 数据记录 + self.data_log = [] + self.log_file = "flight_log.json" + + # 不再自动起飞 - 完全由手势控制 + print("无人机初始化完成，等待手势指令...") + + print("无人机仿真系统初始化完成 ✓") + + def _initialize_camera(self): + """初始化摄像头""" + # 尝试多个摄像头ID，优先使用1，如果失败则尝试0 + camera_ids = [1, 0] # 优先使用摄像头1 + + for camera_id in camera_ids: + print(f"尝试打开摄像头 {camera_id}...") + cap = cv2.VideoCapture(camera_id) + + if cap.isOpened(): + # 设置摄像头参数 + cap.set(cv2.CAP_PROP_FRAME_WIDTH, 640) + cap.set(cv2.CAP_PROP_FRAME_HEIGHT, 480) + cap.set(cv2.CAP_PROP_FPS, 30) + + # 尝试读取一帧测试 + ret, test_frame = cap.read() + if ret: + # 获取实际参数 + width = int(cap.get(cv2.CAP_PROP_FRAME_WIDTH)) + height = int(cap.get(cv2.CAP_PROP_FRAME_HEIGHT)) + fps = cap.get(cv2.CAP_PROP_FPS) + + print(f"✅ 摄像头 {camera_id} 初始化成功: {width}x{height} @ {fps:.1f}fps") + return cap + else: + cap.release() + print(f"摄像头 {camera_id} 能打开但无法读取帧") + else: + print(f"摄像头 {camera_id} 无法打开") + + print("❌ 所有摄像头尝试失败，使用虚拟模式") + return None + + def _gesture_recognition_loop(self): + """手势识别循环""" + print("手势识别线程启动...") + + # 显示虚拟模式提示（如果摄像头未连接） + if self.cap is None: + print("⚠️ 使用虚拟摄像头模式，请连接摄像头进行真实手势识别") + + while self.running: + if self.paused: + time.sleep(0.1) + continue + + # 获取图像帧 + if self.cap and self.cap.isOpened(): + ret, frame = self.cap.read() + if ret: + frame = cv2.flip(frame, 1) # 镜像，更自然 + else: + # 创建虚拟帧 + frame = np.ones((480, 640, 3), dtype=np.uint8) * 255 + cv2.putText(frame, "Camera Error - Virtual Mode", (50, 50), + cv2.FONT_HERSHEY_SIMPLEX, 1, (0, 0, 255), 2) + cv2.putText(frame, "Connect camera for real gesture detection", (50, 100), + cv2.FONT_HERSHEY_SIMPLEX, 0.6, (0, 0, 0), 1) + else: + # 虚拟模式 + frame = np.ones((480, 640, 3), dtype=np.uint8) * 255 + cv2.putText(frame, "Virtual Camera Mode", (50, 50), + cv2.FONT_HERSHEY_SIMPLEX, 1, (0, 0, 255), 2) + cv2.putText(frame, "Gesture Commands:", (50, 100), + cv2.FONT_HERSHEY_SIMPLEX, 0.7, (0, 100, 0), 2) + cv2.putText(frame, "Open Palm - Takeoff", (50, 140), + cv2.FONT_HERSHEY_SIMPLEX, 0.5, (0, 0, 0), 1) + cv2.putText(frame, "Closed Fist - Land", (50, 170), + cv2.FONT_HERSHEY_SIMPLEX, 0.5, (0, 0, 0), 1) + cv2.putText(frame, "Victory - Forward", (50, 200), + cv2.FONT_HERSHEY_SIMPLEX, 0.5, (0, 0, 0), 1) + cv2.putText(frame, "Thumb Up - Backward", (50, 230), + cv2.FONT_HERSHEY_SIMPLEX, 0.5, (0, 0, 0), 1) + cv2.putText(frame, "Press 'q' to quit", (50, 280), + cv2.FONT_HERSHEY_SIMPLEX, 0.7, (0, 0, 0), 2) + + # 手势检测 + try: + processed_frame, gesture, confidence, landmarks = \ + self.gesture_detector.detect_gestures(frame, simulation_mode=True) + + # 更新共享数据 + self.current_frame = processed_frame + self.current_gesture = gesture + self.gesture_confidence = confidence + self.hand_landmarks = landmarks + + # 处理手势命令（使用降低的阈值） + self._process_gesture_command(gesture, confidence) + + # 显示手势识别窗口 + cv2.imshow('手势控制 - Gesture Control', processed_frame) + + except Exception as e: + print(f"手势检测错误: {e}") + self.current_frame = frame + self.current_gesture = None + + # 检查退出 + key = cv2.waitKey(1) & 0xFF + if key == ord('q'): + print("收到退出指令...") + self.running = False + break + elif key == ord('c'): + # 切换摄像头功能 + self._switch_camera() + elif key == ord('d'): # 调试模式 + self._debug_gesture_detection() + + print("手势识别线程结束") + + def _switch_camera(self): + """切换摄像头""" + if self.cap: + self.cap.release() + print("释放当前摄像头...") + + # 获取当前摄像头ID + current_id = 1 if self.cap is None else 0 + + print(f"切换到摄像头 {current_id}...") + self.cap = cv2.VideoCapture(current_id) + + if self.cap.isOpened(): + print(f"✅ 切换到摄像头 {current_id} 成功") + else: + print(f"❌ 切换到摄像头 {current_id} 失败") + self.cap = None + + def _debug_gesture_detection(self): + """调试手势检测""" + print("\n[手势调试信息]") + print(f"当前手势: {self.current_gesture}") + print(f"置信度: {self.gesture_confidence:.2f}") + print(f"冷却时间: {time.time() - self.last_command_time:.1f}s") + print(f"无人机解锁: {self.drone_controller.state['armed']}") + print(f"无人机模式: {self.drone_controller.state['mode']}") + print(f"无人机位置: ({self.drone_controller.state['position'][0]:.1f}, " + f"{self.drone_controller.state['position'][1]:.1f}, " + f"{self.drone_controller.state['position'][2]:.1f})") + + def _process_gesture_command(self, gesture, confidence): + """处理手势命令（使用降低的阈值）""" + current_time = time.time() - for i, text in enumerate(gestures): - y_pos = 90 + i * 25 - color = (0, 0, 255) if i == 0 else (0, 100, 0) - frame = cv2_add_chinese_text(frame, text, (50, y_pos), color, 20) + # 获取该手势的阈值（降低以提高识别率） + threshold = self.gesture_thresholds.get(gesture, self.gesture_thresholds['default']) - frame = cv2_add_chinese_text(frame, "按 'q' 键退出程序", (50, 430), (0, 0, 0), 20) + # 检查是否在冷却期内 + in_cooldown = current_time - self.last_command_time <= self.command_cooldown - return frame + # 只处理置信度高于阈值的手势且不在冷却期 + if (gesture not in ["no_hand", "hand_detected"] and + confidence > threshold and + not in_cooldown): + # 获取控制命令 + command = self.gesture_detector.get_command(gesture) -def main(): - print("=" * 60) - print(" 手势控制无人机系统 - 虚拟机版本") - print("=" * 60) - print("程序已启动，正在尝试显示窗口...") - print("如果看不到窗口，请检查虚拟机显示设置") - print("按 'q' 键退出程序") - print("按 'c' 键切换摄像头") - print("=" * 60) - - detector = GestureDetector() - controller = DroneController(simulation_mode=True) - - # 测试显示 - test_frame = create_test_frame("显示测试中...") - cv2.imshow('Test window', test_frame) - cv2.waitKey(1000) # 显示1秒 - - # 尝试打开摄像头 - cap = None - for cam_id in [1]:#需要视不同情况更改数字，选择摄像头 - cap = cv2.VideoCapture(cam_id) - if cap.isOpened(): - print(f"摄像头 {cam_id} 打开成功") - break + if command != "none": + # 计算手势强度（如果有手部关键点） + intensity = 1.0 + if self.hand_landmarks: + intensity = self.gesture_detector.get_gesture_intensity( + self.hand_landmarks, gesture + ) + + # 添加调试信息 + print( + f"🎯 检测到手势: {gesture} (置信度: {confidence:.2f}, 阈值: {threshold}) -> 执行: {command} (强度: {intensity:.2f})") + + # 发送命令到控制器 + self.drone_controller.send_command(command, intensity) + + # 记录命令 + self._log_command(gesture, command, confidence, intensity) + + # 更新最后命令时间 + self.last_command_time = current_time + elif gesture not in ["no_hand", "hand_detected"] and confidence > 0.3: + # 显示检测到但未触发的情况（仅调试用，可注释掉） + if in_cooldown: + # 冷却期内，不显示信息避免干扰 + pass + elif confidence < threshold: + # 置信度不足，显示信息 + print(f" [手势检测] {gesture} 置信度不足: {confidence:.2f} < {threshold}") + + def _simulation_loop(self): + """仿真主循环""" + print("3D仿真线程启动...") + + last_time = time.time() + frame_count = 0 + last_status_print = time.time() + + print("\n🎮 键盘提示：按 'R' 键重置无人机位置到原点") + print(" 按 'T' 键手动起飞") + print(" 按 'L' 键手动降落") + print(" 按 'H' 键悬停") + + # 按键防抖记录 + self._last_key_press = {} + + while self.running: + current_time = time.time() + dt = current_time - last_time + last_time = current_time + + # 每3秒打印一次状态 + if current_time - last_status_print > 3: + status = self.drone_controller.get_status_string() + print(f"[状态监控] {status}") + if self.current_gesture: + print(f"[状态监控] 当前手势: {self.current_gesture} (置信度: {self.gesture_confidence:.2f})") + last_status_print = current_time + + if dt <= 0: + dt = 0.016 + elif dt > 0.1: + dt = 0.1 + + if self.paused: + if not self.viewer.handle_events(): + self.running = False + time.sleep(0.01) + continue + + keys = pygame.key.get_pressed() + + # 检查重置键 R + if keys[pygame.K_r]: + if ('r' not in self._last_key_press or + current_time - self._last_key_press['r'] > 1.0): + print("🎮 键盘：重置无人机位置") + self.drone_controller.reset() + print(" 无人机已重置到原点位置") + self._last_key_press['r'] = current_time + + # 检查起飞键 T + if keys[pygame.K_t]: + if ('t' not in self._last_key_press or + current_time - self._last_key_press['t'] > 1.0): + print("🎮 键盘：起飞") + self.drone_controller.send_command("takeoff", 0.8) + self._last_key_press['t'] = current_time + + # 检查降落键 L + if keys[pygame.K_l]: + if ('l' not in self._last_key_press or + current_time - self._last_key_press['l'] > 1.0): + print("🎮 键盘：降落") + self.drone_controller.send_command("land", 0.5) + self._last_key_press['l'] = current_time + + # 检查悬停键 H + if keys[pygame.K_h]: + if ('h' not in self._last_key_press or + current_time - self._last_key_press['h'] > 1.0): + print("🎮 键盘：悬停") + self.drone_controller.send_command("hover") + self._last_key_press['h'] = current_time + + # 检查停止键 S + if keys[pygame.K_s]: + if ('s' not in self._last_key_press or + current_time - self._last_key_press['s'] > 1.0): + print("🎮 键盘：停止") + self.drone_controller.send_command("stop") + self._last_key_press['s'] = current_time + + if not self.viewer.handle_events(): + self.running = False + break + + if not self.running: + break + + drone_state = self.drone_controller.get_state() + self.drone_controller.update_physics(dt) + + if self.physics_engine and self.drone_controller.state['armed']: + control_input = self._get_control_input_from_state(drone_state) + physics_state = self.physics_engine.update(dt, control_input) + + trajectory = self.drone_controller.get_trajectory() + + drone_state_with_gesture = drone_state.copy() + if self.current_gesture: + drone_state_with_gesture['current_gesture'] = self.current_gesture + drone_state_with_gesture['gesture_confidence'] = self.gesture_confidence + + self.viewer.render(drone_state_with_gesture, trajectory) + + frame_count += 1 + if frame_count % 120 == 0: + fps = 1.0 / dt if dt > 0 else 0 + print(f"3D仿真帧率: {fps:.1f} FPS") + + print("3D仿真线程结束") + + def _get_control_input_from_state(self, drone_state): + """从无人机状态生成控制输入""" + control_input = { + 'throttle': 0.5, # 默认油门 + 'roll': 0.0, + 'pitch': 0.0, + 'yaw_rate': 0.0 + } + + # 如果检测到手部关键点，可以用于精细控制 + if self.hand_landmarks and self.current_gesture: + # 简单示例：根据手势调整控制 + if self.current_gesture == "pointing_up": + control_input['throttle'] = 0.8 + elif self.current_gesture == "pointing_down": + control_input['throttle'] = 0.2 + elif self.current_gesture == "victory": + control_input['pitch'] = 0.3 # 轻微前倾 + elif self.current_gesture == "thumb_up": + control_input['pitch'] = -0.3 # 轻微后倾 + + return control_input + + def _log_command(self, gesture, command, confidence, intensity): + """记录命令到日志""" + log_entry = { + 'timestamp': time.time(), + 'gesture': gesture, + 'command': command, + 'confidence': confidence, + 'intensity': intensity, + 'position': self.drone_controller.state['position'].tolist(), + 'battery': self.drone_controller.state['battery'], + 'armed': self.drone_controller.state['armed'], + 'mode': self.drone_controller.state['mode'] + } + self.data_log.append(log_entry) + + # 实时显示 + pos = self.drone_controller.state['position'] + print(f" 位置: ({pos[0]:.1f}, {pos[1]:.1f}, {pos[2]:.1f}) | " + f"电池: {self.drone_controller.state['battery']:.1f}%") + + def _save_log(self): + """保存日志到文件""" + if self.data_log: + try: + with open(self.log_file, 'w', encoding='utf-8') as f: + json.dump(self.data_log, f, indent=2, ensure_ascii=False) + print(f"飞行日志已保存到: {self.log_file} ({len(self.data_log)}条记录)") + except Exception as e: + print(f"保存日志失败: {e}") else: - cap = None + print("没有飞行记录需要保存") + + def run(self): + """运行主程序""" + print("=" * 60) + print(" 手势控制无人机仿真系统（完全手势控制版）") + print("=" * 60) + print("系统功能:") + print(" 1. 实时手势识别 (8种手势)") + print(" 2. 无人机控制仿真") + print(" 3. 3D可视化 (OpenGL渲染)") + print(" 4. 飞行数据记录") + print("=" * 60) + print("手势指令:") + print(" 张开手掌 - 起飞") + print(" 握拳 - 降落") + print(" 胜利手势 - 前进") + print(" 大拇指 - 后退") + print(" 食指上指 - 上升") + print(" 食指向下 - 下降") + print(" OK手势 - 悬停") + print(" 大拇指向下 - 停止") + print("=" * 60) + print("使用说明:") + print(" 手势控制窗口: 按 'q' 退出") + print(" 手势控制窗口: 按 'c' 切换摄像头") + print(" 手势控制窗口: 按 'd' 显示调试信息") + print(" 3D仿真窗口: 按 'ESC' 退出") + print(" 3D窗口按键控制:") + print(" G - 切换网格显示") + print(" T - 切换轨迹显示") + print(" A - 切换坐标轴显示") + print(" ↑↓←→ - 旋转视角") + print(" +/- - 缩放视角") + print(" 空格 - 重置视角") + print("=" * 60) + print("提示:") + print(" 1. 无人机初始在地面，等待手势指令") + print(" 2. 手势识别阈值已降低，更容易触发") + print(" 3. 做手势时保持手在摄像头中心") + print(" 4. 每个手势保持1.5秒以上") + print("=" * 60) + print("系统启动中...") - if cap is None: - print("使用虚拟摄像头模式") + try: + # 启动手势识别线程 + self.gesture_thread = threading.Thread( + target=self._gesture_recognition_loop, + name="GestureThread", + daemon=True + ) + self.gesture_thread.start() + + print("手势识别线程已启动") + print("3D仿真窗口即将打开...") + time.sleep(1) # 给手势窗口一点时间显示 + + # 主线程运行仿真 + self._simulation_loop() + + # 等待手势线程结束 + if self.gesture_thread.is_alive(): + self.gesture_thread.join(timeout=2.0) + + except KeyboardInterrupt: + print("\n系统被用户中断") + except Exception as e: + print(f"系统运行错误: {e}") + import traceback + traceback.print_exc() + finally: + # 清理资源 + self.running = False - last_command_time = time.time() - frame_count = 0 + if self.cap: + self.cap.release() + print("摄像头已释放") - while True: - frame_count += 1 + cv2.destroyAllWindows() + print("OpenCV窗口已关闭") - # 获取帧 - if cap and cap.isOpened(): - ret, frame = cap.read() - if ret: - frame = cv2.flip(frame, 1) - else: - frame = create_test_frame("摄像头错误 - 虚拟模式") - else: - # 虚拟模式 - 创建动态测试帧 - if frame_count % 30 == 0: # 每30帧切换消息 - messages = [ - "虚拟摄像头模式 - 请做出手势", - "手势检测已激活 - 虚拟机", - "手势识别系统准备就绪" - ] - message = messages[(frame_count // 30) % len(messages)] - frame = create_test_frame(message) - else: - frame = create_test_frame("虚拟摄像头模式 - 请做出手势") + # 保存日志 + self._save_log() - # 手势检测 - try: - processed_frame, gesture, confidence = detector.detect_gestures(frame) - except Exception as e: - print(f"手势检测错误: {e}") - processed_frame = frame - gesture = "no_hand" + print("无人机仿真系统已安全关闭 ✓") - # 处理命令 - current_time = time.time() - if (gesture not in ["no_hand", "hand_detected"] and - current_time - last_command_time > 2.0): - command = detector.get_command(gesture) - if command != "none": - print(f"检测到手势: {gesture} -> 执行: {command}") - controller.send_command(command) - last_command_time = current_time - - # 显示帧 - cv2.imshow('Main window', processed_frame) - - # 退出检测 - key = cv2.waitKey(1) & 0xFF - if key == ord('q'): - break - elif key == ord('c'): - print("切换摄像头...") - if cap: - cap.release() - cap = None - - # 清理 - if cap: - cap.release() - cv2.destroyAllWindows() - print("程序退出") +def load_config(): + """加载配置文件""" + config = { + 'camera_id': 1, # 默认使用摄像头1 + 'window_width': 1024, + 'window_height': 768, + 'drone_mass': 1.0, + 'gravity': 9.81, + 'simulation_fps': 60, + 'gesture_threshold': 0.6 # 降低默认阈值 + } + return config if __name__ == "__main__": - main() \ No newline at end of file + print("手势控制无人机仿真系统 - 启动（完全手势控制版）") + print("=" * 60) + + # 检查必要的模块 + try: + import pygame + + print("✅ Pygame 已安装") + except ImportError: + print("❌ 错误: Pygame 未安装!") + print("请运行: pip install pygame") + sys.exit(1) + + try: + import OpenGL + + print("✅ PyOpenGL 已安装") + except ImportError: + print("❌ 错误: PyOpenGL 未安装!") + print("请运行: pip install PyOpenGL PyOpenGL-accelerate") + sys.exit(1) + + # 加载配置 + config = load_config() + + # 创建并运行仿真系统 + try: + simulation = IntegratedDroneSimulation(config) + simulation.run() + except Exception as e: + print(f"系统启动失败: {e}") + import traceback + + traceback.print_exc() \ No newline at end of file