From 5814e0c0a0805a91a76b8dac95229dd684a3543b Mon Sep 17 00:00:00 2001 From: Arrebol01 <1245298208@qq.com> Date: Fri, 19 Dec 2025 22:04:01 +0800 Subject: [PATCH 1/3] =?UTF-8?q?=E6=96=B0=E6=B7=BB=E5=B8=A7=E7=8E=87?= =?UTF-8?q?=E6=8E=A7=E5=88=B6?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- src/Drone_hand_gesture_project/main.py | 31 ++++++++++++++++++-------- 1 file changed, 22 insertions(+), 9 deletions(-) diff --git a/src/Drone_hand_gesture_project/main.py b/src/Drone_hand_gesture_project/main.py index edcb92f282..6b322cea30 100644 --- a/src/Drone_hand_gesture_project/main.py +++ b/src/Drone_hand_gesture_project/main.py @@ -12,11 +12,13 @@ # 导入自定义模块 try: from gesture_detector_enhanced import EnhancedGestureDetector + print("✅ 导入增强版手势检测器 (机器学习)") HAS_ENHANCED_DETECTOR = True except ImportError: print("⚠️ 未找到增强版检测器,使用原始手势检测器") from gesture_detector import GestureDetector + HAS_ENHANCED_DETECTOR = False from drone_controller import DroneController @@ -246,11 +248,11 @@ def _gesture_recognition_loop(self): # 虚拟模式 frame = np.ones((480, 640, 3), dtype=np.uint8) * 255 cv2.putText(frame, "虚拟摄像头模式", (50, 50), - cv2.FONT_HERSHEY_SIMPLEX, 1, (0, 0, 255), 2) + cv2.FONT_HERSHEY_SIMPLEX, 1, (0, 0, 255), 2) cv2.putText(frame, f"手势指令 ({mode_text}):", (50, 100), - cv2.FONT_HERSHEY_SIMPLEX, 0.7, (0, 100, 0), 2) + cv2.FONT_HERSHEY_SIMPLEX, 0.7, (0, 100, 0), 2) cv2.putText(frame, "张开手掌 - 起飞", (50, 140), - cv2.FONT_HERSHEY_SIMPLEX, 0.5, (0, 0, 0), 1) + cv2.FONT_HERSHEY_SIMPLEX, 0.5, (0, 0, 0), 1) cv2.putText(frame, "握拳 - 降落", (50, 170), cv2.FONT_HERSHEY_SIMPLEX, 0.5, (0, 0, 0), 1) cv2.putText(frame, "胜利手势 - 前进", (50, 200), @@ -428,6 +430,10 @@ def _simulation_loop(self): frame_count = 0 last_status_print = time.time() + # 帧率控制 + target_fps = 60 + frame_delay = 1.0 / target_fps + print("\n🎮 键盘提示:按 'R' 键重置无人机位置到原点") print(" 按 'T' 键手动起飞") print(" 按 'L' 键手动降落") @@ -437,10 +443,16 @@ def _simulation_loop(self): self._last_key_press = {} while self.running: + start_time = time.time() current_time = time.time() dt = current_time - last_time last_time = current_time + if dt <= 0: + dt = frame_delay + elif dt > 0.1: + dt = 0.1 + # 每3秒打印一次状态 if current_time - last_status_print > 3: status = self.drone_controller.get_status_string() @@ -449,11 +461,6 @@ def _simulation_loop(self): 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 @@ -526,9 +533,15 @@ def _simulation_loop(self): self.viewer.render(drone_state_with_gesture, trajectory) + # 控制帧率,避免CPU占用过高 + elapsed = time.time() - start_time + sleep_time = frame_delay - elapsed + if sleep_time > 0: + time.sleep(sleep_time) + frame_count += 1 if frame_count % 120 == 0: - fps = 1.0 / dt if dt > 0 else 0 + fps = 1.0 / (time.time() - start_time) if start_time > 0 else 0 print(f"3D仿真帧率: {fps:.1f} FPS") print("3D仿真线程结束") From 515a93d6b41e3f19fcae33e5646dfe32ef345539 Mon Sep 17 00:00:00 2001 From: Arrebol01 <1245298208@qq.com> Date: Sat, 20 Dec 2025 11:33:44 +0800 Subject: [PATCH 2/3] =?UTF-8?q?=E6=B7=BB=E5=8A=A0=E8=AF=86=E5=88=AB?= =?UTF-8?q?=E5=86=B7=E5=8D=B4?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- src/Drone_hand_gesture_project/main.py | 30 ++++++++++++++++++-------- 1 file changed, 21 insertions(+), 9 deletions(-) diff --git a/src/Drone_hand_gesture_project/main.py b/src/Drone_hand_gesture_project/main.py index 6b322cea30..3bfbea354e 100644 --- a/src/Drone_hand_gesture_project/main.py +++ b/src/Drone_hand_gesture_project/main.py @@ -385,10 +385,17 @@ def _process_gesture_command(self, gesture, confidence): # 检查是否在冷却期内 in_cooldown = current_time - self.last_command_time <= self.command_cooldown + # 检查是否是重复手势(避免频繁处理同一个手势) + same_gesture = (gesture == self.current_gesture and + hasattr(self, 'last_processed_gesture') and + gesture == self.last_processed_gesture and + current_time - getattr(self, 'last_processed_time', 0) < 2.0) + # 只处理置信度高于阈值的手势且不在冷却期 if (gesture not in ["no_hand", "hand_detected"] and confidence > threshold and - not in_cooldown): + not in_cooldown and + not same_gesture): # 获取控制命令 command = self.gesture_detector.get_command(gesture) @@ -411,16 +418,21 @@ def _process_gesture_command(self, gesture, confidence): # 记录命令 self._log_command(gesture, command, confidence, intensity) - # 更新最后命令时间 + # 更新最后命令时间和手势状态 self.last_command_time = current_time + self.last_processed_gesture = gesture + self.last_processed_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}") + # 只在调试模式下显示检测到但未触发的情况 + debug_mode = False # 可以设为True启用详细调试 + if debug_mode: + if in_cooldown: + print( + f" [冷却中] {gesture} 冷却时间剩余: {self.command_cooldown - (current_time - self.last_command_time):.1f}s") + elif same_gesture: + print(f" [重复手势] {gesture} 已处理过,冷却中") + elif confidence < threshold: + print(f" [置信度不足] {gesture} 置信度: {confidence:.2f} < 阈值: {threshold}") def _simulation_loop(self): """仿真主循环""" From d499889d77b95a85c2fc00ccad12d5c1e1cdeb8d Mon Sep 17 00:00:00 2001 From: Arrebol01 <1245298208@qq.com> Date: Sun, 21 Dec 2025 15:04:22 +0800 Subject: [PATCH 3/3] =?UTF-8?q?=E6=B7=BB=E5=8A=A0ROS2=E5=B0=81=E8=A3=85?= =?UTF-8?q?=E5=90=8E=E7=9A=84=E5=BF=85=E8=A6=81=E6=96=87=E4=BB=B6?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../nodes/gesture_control_node.py | 240 +++++++++ .../nodes/visualization_node.py | 469 ++++++++++++++++++ .../launch/drone_gesture.launch.py | 34 ++ .../drone_gesture_ros2/package.xml | 32 ++ .../drone_gesture_ros2/setup.py | 35 ++ 5 files changed, 810 insertions(+) create mode 100644 src/Drone_hand_gesture_project/drone_gesture_ros2/drone_gesture_control/nodes/gesture_control_node.py create mode 100644 src/Drone_hand_gesture_project/drone_gesture_ros2/drone_gesture_control/nodes/visualization_node.py create mode 100644 src/Drone_hand_gesture_project/drone_gesture_ros2/launch/drone_gesture.launch.py create mode 100644 src/Drone_hand_gesture_project/drone_gesture_ros2/package.xml create mode 100644 src/Drone_hand_gesture_project/drone_gesture_ros2/setup.py diff --git a/src/Drone_hand_gesture_project/drone_gesture_ros2/drone_gesture_control/nodes/gesture_control_node.py b/src/Drone_hand_gesture_project/drone_gesture_ros2/drone_gesture_control/nodes/gesture_control_node.py new file mode 100644 index 0000000000..4875162d55 --- /dev/null +++ b/src/Drone_hand_gesture_project/drone_gesture_ros2/drone_gesture_control/nodes/gesture_control_node.py @@ -0,0 +1,240 @@ +#!/usr/bin/env python3 +""" +手势控制节点 - 简化版本,避免复杂的机器学习依赖 +""" +import rclpy +from rclpy.node import Node +import sys +import os + +# 添加项目路径 +sys.path.insert(0, os.path.join(os.path.dirname(__file__), '..')) + +class SimpleGestureDetector: + """简化的手势检测器,避免复杂的依赖""" + def __init__(self): + self.gesture_commands = { + "open_palm": "takeoff", + "closed_fist": "land", + "pointing_up": "up", + "pointing_down": "down", + "victory": "forward", + "thumb_up": "backward", + "thumb_down": "stop", + "ok_sign": "hover", + } + + def detect_gestures(self, image, simulation_mode=False): + """模拟手势检测""" + import numpy as np + import cv2 + + # 在图像上绘制虚拟手势信息 + height, width = image.shape[:2] + cv2.putText(image, "虚拟手势检测模式", (10, 30), + cv2.FONT_HERSHEY_SIMPLEX, 1, (0, 255, 0), 2) + cv2.putText(image, "按 'q' 退出", (10, 70), + cv2.FONT_HERSHEY_SIMPLEX, 0.7, (0, 255, 0), 2) + + # 模拟手势 + gestures = list(self.gesture_commands.keys()) + import random + gesture = random.choice(gestures) + confidence = random.uniform(0.7, 0.95) + + cv2.putText(image, f"手势: {gesture}", (10, 110), + cv2.FONT_HERSHEY_SIMPLEX, 1, (0, 255, 0), 2) + cv2.putText(image, f"置信度: {confidence:.2f}", (10, 150), + cv2.FONT_HERSHEY_SIMPLEX, 0.7, (0, 255, 0), 2) + + return image, gesture, confidence, None + + def get_command(self, gesture): + return self.gesture_commands.get(gesture, "none") + +class SimpleDroneController: + """简化的无人机控制器""" + def __init__(self, simulation_mode=True): + self.simulation_mode = simulation_mode + self.position = [0.0, 0.0, 0.0] + self.battery = 100.0 + self.armed = False + self.mode = "DISARMED" + + def send_command(self, command, intensity=1.0): + print(f"[仿真] 执行命令: {command}, 强度: {intensity}") + + if command == "takeoff": + self.armed = True + self.mode = "TAKEOFF" + self.position[1] = 2.0 # 起飞到2米高度 + elif command == "land": + self.mode = "LAND" + self.position[1] = 0.0 + self.armed = False + elif command == "up": + self.position[1] += 0.5 * intensity + elif command == "down": + self.position[1] = max(0, self.position[1] - 0.5 * intensity) + elif command == "forward": + self.position[2] += 0.5 * intensity + elif command == "backward": + self.position[2] -= 0.5 * intensity + elif command == "left": + self.position[0] -= 0.5 * intensity + elif command == "right": + self.position[0] += 0.5 * intensity + elif command == "hover": + self.mode = "HOVER" + elif command == "stop": + self.mode = "STOP" + + def get_state(self): + return { + 'position': self.position, + 'battery': self.battery, + 'armed': self.armed, + 'mode': self.mode + } + +from std_msgs.msg import String +from geometry_msgs.msg import Twist +import time + +class GestureControlNode(Node): + def __init__(self): + super().__init__('gesture_control_node') + + # ROS参数 + self.declare_parameter('simulation_mode', True) + self.declare_parameter('camera_id', 0) + self.declare_parameter('command_cooldown', 1.5) + + # 获取参数 + simulation_mode = self.get_parameter('simulation_mode').value + camera_id = self.get_parameter('camera_id').value + self.command_cooldown = self.get_parameter('command_cooldown').value + + # 使用简化的组件 + self.gesture_detector = SimpleGestureDetector() + self.drone_controller = SimpleDroneController(simulation_mode) + + # ROS发布器 + self.gesture_pub = self.create_publisher(String, 'gesture_detected', 10) + self.command_pub = self.create_publisher(String, 'gesture_command', 10) + self.velocity_pub = self.create_publisher(Twist, 'cmd_vel', 10) + + # 定时器 + self.gesture_timer = self.create_timer(0.5, self.gesture_callback) + self.status_timer = self.create_timer(2.0, self.status_callback) + + # 状态 + self.current_gesture = "none" + self.gesture_confidence = 0.0 + self.last_command_time = time.time() + + self.get_logger().info('🎮 手势控制节点已启动 (简化版本)') + self.get_logger().info(f'📊 模式: {"仿真" if simulation_mode else "真实"}') + self.get_logger().info('💡 提示: 这是简化版本,用于测试ROS2包结构') + + def gesture_callback(self): + """手势检测回调""" + try: + # 创建虚拟图像 + import numpy as np + import cv2 + + # 创建虚拟摄像头图像 + frame = np.ones((480, 640, 3), dtype=np.uint8) * 100 + cv2.putText(frame, "虚拟摄像头模式", (50, 50), + cv2.FONT_HERSHEY_SIMPLEX, 1, (0, 0, 255), 2) + cv2.putText(frame, "手势指令:", (50, 100), + cv2.FONT_HERSHEY_SIMPLEX, 0.7, (0, 100, 0), 2) + cv2.putText(frame, "按 'q' 退出", (50, 400), + cv2.FONT_HERSHEY_SIMPLEX, 0.7, (0, 0, 0), 2) + + # 检测手势 + processed_frame, gesture, confidence, _ = \ + self.gesture_detector.detect_gestures(frame, simulation_mode=True) + + # 更新状态 + self.current_gesture = gesture + self.gesture_confidence = confidence + + # 发布手势 + gesture_msg = String() + gesture_msg.data = f"{gesture}:{confidence:.2f}" + self.gesture_pub.publish(gesture_msg) + + # 处理命令 + current_time = time.time() + if current_time - self.last_command_time > self.command_cooldown: + if confidence > 0.7: + command = self.gesture_detector.get_command(gesture) + if command != "none": + # 执行命令 + intensity = min(max(confidence, 0.5), 1.0) + self.drone_controller.send_command(command, intensity) + + # 发布命令 + command_msg = String() + command_msg.data = command + self.command_pub.publish(command_msg) + + # 发布速度指令 + if command in ['forward', 'backward', 'up', 'down', 'left', 'right']: + twist = Twist() + speed = 0.3 * intensity + + if command == 'forward': + twist.linear.x = speed + elif command == 'backward': + twist.linear.x = -speed + elif command == 'up': + twist.linear.z = speed + elif command == 'down': + twist.linear.z = -speed + elif command == 'left': + twist.linear.y = speed + elif command == 'right': + twist.linear.y = -speed + + self.velocity_pub.publish(twist) + + self.last_command_time = current_time + self.get_logger().info(f'🎯 手势: {gesture} -> 命令: {command}') + + # 显示图像 + cv2.imshow('Gesture Control (Virtual Mode)', processed_frame) + cv2.waitKey(1) + + except Exception as e: + self.get_logger().error(f'手势检测错误: {e}') + + def status_callback(self): + """状态更新回调""" + state = self.drone_controller.get_state() + self.get_logger().info( + f'📊 状态 | 位置: ({state["position"][0]:.1f}, {state["position"][1]:.1f}, {state["position"][2]:.1f}) | ' + f'电池: {state["battery"]:.1f}% | 模式: {state["mode"]}' + ) + + def destroy_node(self): + """清理资源""" + import cv2 + cv2.destroyAllWindows() + super().destroy_node() + +def main(args=None): + rclpy.init(args=args) + node = GestureControlNode() + try: + rclpy.spin(node) + except KeyboardInterrupt: + node.get_logger().info('🛑 收到中断信号,正在关闭...') + finally: + node.destroy_node() + rclpy.shutdown() + +if __name__ == '__main__': + main() diff --git a/src/Drone_hand_gesture_project/drone_gesture_ros2/drone_gesture_control/nodes/visualization_node.py b/src/Drone_hand_gesture_project/drone_gesture_ros2/drone_gesture_control/nodes/visualization_node.py new file mode 100644 index 0000000000..c51c726df0 --- /dev/null +++ b/src/Drone_hand_gesture_project/drone_gesture_ros2/drone_gesture_control/nodes/visualization_node.py @@ -0,0 +1,469 @@ +#!/usr/bin/env python3 +""" +可视化节点 - 包含3D无人机显示 +""" +import rclpy +from rclpy.node import Node +from std_msgs.msg import String +import time +import threading + +class VisualizationNode(Node): + def __init__(self): + super().__init__('visualization_node') + + # 订阅器 + self.gesture_sub = self.create_subscription( + String, 'gesture_detected', self.gesture_callback, 10) + self.command_sub = self.create_subscription( + String, 'gesture_command', self.command_callback, 10) + + # 状态 + self.current_gesture = "none" + self.current_command = "none" + self.gesture_history = [] + self.command_history = [] + self.start_time = time.time() + + # 无人机模拟状态 + self.drone_position = [0.0, 2.0, 0.0] # [x, y, z] + self.drone_orientation = [0.0, 0.0, 0.0] # [roll, pitch, yaw] + self.drone_battery = 100.0 + self.drone_armed = True + self.drone_mode = "HOVER" + + # 启动3D可视化线程 + self.viz_thread = threading.Thread(target=self.run_3d_visualization, daemon=True) + self.viz_thread.start() + + # 状态定时器 + self.status_timer = self.create_timer(2.0, self.update_drone_state) + + self.get_logger().info('👁️ 3D可视化节点已启动') + self.get_logger().info('🚀 3D无人机显示已启用') + + def gesture_callback(self, msg): + """手势回调""" + data = msg.data.split(':') + if len(data) >= 1: + self.current_gesture = data[0] + confidence = data[1] if len(data) > 1 else "0.0" + + # 记录历史 + current_time = time.time() - self.start_time + self.gesture_history.append((current_time, self.current_gesture, confidence)) + + # 只保留最近10个 + if len(self.gesture_history) > 10: + self.gesture_history.pop(0) + + self.get_logger().info(f'👋 检测到手势: {self.current_gesture} (置信度: {confidence})') + + # 根据手势更新无人机位置(模拟) + self.update_drone_by_gesture(self.current_gesture) + + def command_callback(self, msg): + """命令回调""" + self.current_command = msg.data + + # 记录历史 + current_time = time.time() - self.start_time + self.command_history.append((current_time, self.current_command)) + + # 只保留最近10个 + if len(self.command_history) > 10: + self.command_history.pop(0) + + self.get_logger().info(f'🎯 执行命令: {self.current_command}') + + # 根据命令更新无人机状态 + self.update_drone_by_command(self.current_command) + + def update_drone_by_gesture(self, gesture): + """根据手势更新无人机位置(模拟)""" + import numpy as np + + # 模拟手势对无人机的影响 + gesture_effects = { + "pointing_up": [0, 0.1, 0], # 向上 + "pointing_down": [0, -0.1, 0], # 向下 + "victory": [0, 0, 0.1], # 向前 + "thumb_up": [0, 0, -0.1], # 向后 + "open_palm": [0.1, 0, 0], # 向右 + "closed_fist": [-0.1, 0, 0], # 向左 + } + + if gesture in gesture_effects: + effect = gesture_effects[gesture] + self.drone_position[0] += effect[0] + self.drone_position[1] += effect[1] + self.drone_position[2] += effect[2] + + # 限制高度 + self.drone_position[1] = max(0.0, min(10.0, self.drone_position[1])) + + def update_drone_by_command(self, command): + """根据命令更新无人机状态""" + if command == "takeoff": + self.drone_armed = True + self.drone_mode = "TAKEOFF" + self.drone_position[1] = 3.0 # 起飞到3米高度 + elif command == "land": + self.drone_mode = "LAND" + self.drone_position[1] = 0.0 + elif command == "hover": + self.drone_mode = "HOVER" + elif command == "stop": + self.drone_mode = "STOP" + + def update_drone_state(self): + """定期更新无人机状态""" + # 模拟电池消耗 + self.drone_battery = max(0, self.drone_battery - 0.01) + if self.drone_battery < 20.0: + self.get_logger().warning(f'🔋 电池电量低: {self.drone_battery:.1f}%') + + # 模拟轻微晃动 + import numpy as np + self.drone_orientation[2] = np.sin(time.time() * 0.5) * 0.1 # 偏航 + + def run_3d_visualization(self): + """运行3D可视化""" + try: + # 尝试导入OpenGL和Pygame + import OpenGL.GL as gl + import OpenGL.GLU as glu + import pygame + import numpy as np + + # 初始化Pygame + pygame.init() + width, height = 1024, 768 + screen = pygame.display.set_mode((width, height), pygame.DOUBLEBUF | pygame.OPENGL) + pygame.display.set_caption("无人机3D仿真系统") + + # OpenGL设置 + gl.glEnable(gl.GL_DEPTH_TEST) + gl.glEnable(gl.GL_LIGHTING) + gl.glEnable(gl.GL_LIGHT0) + + # 光源 + gl.glLightfv(gl.GL_LIGHT0, gl.GL_POSITION, [5.0, 5.0, 5.0, 1.0]) + gl.glLightfv(gl.GL_LIGHT0, gl.GL_AMBIENT, [0.2, 0.2, 0.2, 1.0]) + gl.glLightfv(gl.GL_LIGHT0, gl.GL_DIFFUSE, [0.8, 0.8, 0.8, 1.0]) + + # 投影 + glu.gluPerspective(45, (width / height), 0.1, 100.0) + + # 相机初始位置 + camera_distance = 10.0 + camera_angle_x = 30.0 + camera_angle_y = -30.0 + + clock = pygame.time.Clock() + + self.get_logger().info('🎮 3D窗口已打开') + self.get_logger().info('💡 控制提示:') + self.get_logger().info(' ESC - 退出') + self.get_logger().info(' ↑↓←→ - 旋转视角') + self.get_logger().info(' +/- - 缩放视角') + self.get_logger().info(' 空格 - 重置视角') + + running = True + while running and rclpy.ok(): + # 处理事件 + for event in pygame.event.get(): + if event.type == pygame.QUIT: + running = False + elif event.type == pygame.KEYDOWN: + if event.key == pygame.K_ESCAPE: + running = False + elif event.key == pygame.K_UP: + camera_angle_y = min(89, camera_angle_y + 5) + elif event.key == pygame.K_DOWN: + camera_angle_y = max(-89, camera_angle_y - 5) + elif event.key == pygame.K_LEFT: + camera_angle_x -= 5 + elif event.key == pygame.K_RIGHT: + camera_angle_x += 5 + elif event.key == pygame.K_PLUS or event.key == pygame.K_EQUALS: + camera_distance = max(5, camera_distance - 1) + elif event.key == pygame.K_MINUS: + camera_distance = min(50, camera_distance + 1) + elif event.key == pygame.K_SPACE: + camera_distance = 10.0 + camera_angle_x = 30.0 + camera_angle_y = -30.0 + + # 清除缓冲区 + gl.glClear(gl.GL_COLOR_BUFFER_BIT | gl.GL_DEPTH_BUFFER_BIT) + gl.glClearColor(0.1, 0.1, 0.15, 1.0) + + # 设置模型视图矩阵 + gl.glLoadIdentity() + + # 计算相机位置 + cam_x = camera_distance * np.sin(np.radians(camera_angle_y)) * np.cos(np.radians(camera_angle_x)) + cam_y = camera_distance * np.cos(np.radians(camera_angle_y)) + cam_z = camera_distance * np.sin(np.radians(camera_angle_y)) * np.sin(np.radians(camera_angle_x)) + + glu.gluLookAt(cam_x, cam_y, cam_z, # 相机位置 + 0, 0, 0, # 观察点 + 0, 1, 0) # 上方向 + + # 绘制地面网格 + self.draw_grid() + + # 绘制坐标轴 + self.draw_axes() + + # 绘制无人机 + self.draw_drone() + + # 绘制状态信息 + self.draw_status_overlay(width, height) + + # 交换缓冲区 + pygame.display.flip() + clock.tick(60) + + pygame.quit() + self.get_logger().info('3D窗口已关闭') + + except ImportError as e: + self.get_logger().warning(f'无法加载3D库: {e}') + self.get_logger().info('切换到2D终端显示模式') + self.run_terminal_display() + except Exception as e: + self.get_logger().error(f'3D可视化错误: {e}') + self.run_terminal_display() + + def draw_grid(self): + """绘制地面网格""" + import OpenGL.GL as gl + + gl.glBegin(gl.GL_LINES) + gl.glColor3f(0.3, 0.3, 0.3) + + for i in range(-20, 21): + # X方向线 + gl.glVertex3f(i, 0, -20) + gl.glVertex3f(i, 0, 20) + # Z方向线 + gl.glVertex3f(-20, 0, i) + gl.glVertex3f(20, 0, i) + + gl.glEnd() + + def draw_axes(self): + """绘制坐标轴""" + import OpenGL.GL as gl + + gl.glLineWidth(2.0) + gl.glBegin(gl.GL_LINES) + + # X轴 (红色) + gl.glColor3f(1.0, 0.0, 0.0) + gl.glVertex3f(0, 0, 0) + gl.glVertex3f(3, 0, 0) + + # Y轴 (绿色) + gl.glColor3f(0.0, 1.0, 0.0) + gl.glVertex3f(0, 0, 0) + gl.glVertex3f(0, 3, 0) + + # Z轴 (蓝色) + gl.glColor3f(0.0, 0.0, 1.0) + gl.glVertex3f(0, 0, 0) + gl.glVertex3f(0, 0, 3) + + gl.glEnd() + gl.glLineWidth(1.0) + + def draw_drone(self): + """绘制无人机""" + import OpenGL.GL as gl + import numpy as np + + # 无人机位置和姿态 + x, y, z = self.drone_position + roll, pitch, yaw = self.drone_orientation + + gl.glPushMatrix() + gl.glTranslatef(x, y, z) + gl.glRotatef(np.degrees(yaw), 0, 1, 0) + gl.glRotatef(np.degrees(pitch), 1, 0, 0) + gl.glRotatef(np.degrees(roll), 0, 0, 1) + + # 根据状态设置颜色 + if self.drone_armed: + if self.drone_mode == "TAKEOFF": + color = (0.0, 1.0, 0.0) # 绿色:起飞 + elif self.drone_mode == "LAND": + color = (1.0, 0.5, 0.0) # 橙色:降落 + elif self.drone_mode == "HOVER": + color = (0.0, 0.8, 1.0) # 青色:悬停 + else: + color = (0.0, 0.6, 0.0) # 深绿:飞行 + else: + color = (0.5, 0.5, 0.5) # 灰色:未解锁 + + gl.glColor3f(*color) + + # 绘制无人机机身(立方体) + size = 0.3 + self.draw_cube(size) + + # 绘制机臂 + arm_length = 0.8 + arm_positions = [ + (-arm_length/2, 0, 0), + (arm_length/2, 0, 0), + (0, 0, -arm_length/2), + (0, 0, arm_length/2) + ] + + gl.glColor3f(0.3, 0.3, 0.3) + for arm_x, arm_y, arm_z in arm_positions: + gl.glPushMatrix() + gl.glTranslatef(arm_x, arm_y, arm_z) + self.draw_cylinder(0.03, 0.05) + gl.glPopMatrix() + + gl.glPopMatrix() + + def draw_cube(self, size): + """绘制立方体""" + import OpenGL.GL as gl + + s = size / 2 + vertices = [ + (-s, -s, -s), (s, -s, -s), (s, s, -s), (-s, s, -s), + (-s, -s, s), (s, -s, s), (s, s, s), (-s, s, s) + ] + + faces = [ + (0,1,2,3), (1,5,6,2), (5,4,7,6), + (4,0,3,7), (3,2,6,7), (1,0,4,5) + ] + + gl.glBegin(gl.GL_QUADS) + for face in faces: + for vertex in face: + gl.glVertex3fv(vertices[vertex]) + gl.glEnd() + + def draw_cylinder(self, radius, height): + """绘制圆柱体""" + import OpenGL.GL as gl + import numpy as np + + slices = 8 + gl.glBegin(gl.GL_QUAD_STRIP) + for i in range(slices + 1): + angle = 2 * np.pi * i / slices + x = np.cos(angle) * radius + z = np.sin(angle) * radius + gl.glVertex3f(x, -height/2, z) + gl.glVertex3f(x, height/2, z) + gl.glEnd() + + def draw_status_overlay(self, width, height): + """绘制状态信息覆盖层""" + import OpenGL.GL as gl + import pygame + + # 切换到2D模式 + gl.glMatrixMode(gl.GL_PROJECTION) + gl.glPushMatrix() + gl.glLoadIdentity() + gl.glOrtho(0, width, 0, height, -1, 1) + + gl.glMatrixMode(gl.GL_MODELVIEW) + gl.glPushMatrix() + gl.glLoadIdentity() + + # 禁用3D特性 + gl.glDisable(gl.GL_DEPTH_TEST) + gl.glDisable(gl.GL_LIGHTING) + + # 绘制半透明背景 + gl.glColor4f(0.0, 0.0, 0.0, 0.5) + gl.glBegin(gl.GL_QUADS) + gl.glVertex2f(10, height - 200) + gl.glVertex2f(400, height - 200) + gl.glVertex2f(400, height - 10) + gl.glVertex2f(10, height - 10) + gl.glEnd() + + # 创建字体 + font = pygame.font.SysFont(None, 24) + + # 状态信息 + status_lines = [ + f"无人机状态: {self.drone_mode}", + f"位置: X={self.drone_position[0]:.2f} Y={self.drone_position[1]:.2f} Z={self.drone_position[2]:.2f}", + f"电池: {self.drone_battery:.1f}%", + f"解锁: {'是' if self.drone_armed else '否'}", + f"当前手势: {self.current_gesture}", + f"当前命令: {self.current_command}", + f"运行时间: {time.time() - self.start_time:.0f}秒" + ] + + # 渲染文本 + y_offset = height - 40 + for line in status_lines: + text_surface = font.render(line, True, (255, 255, 255)) + text_data = pygame.image.tostring(text_surface, "RGBA", True) + + gl.glRasterPos2d(20, y_offset) + gl.glDrawPixels(text_surface.get_width(), text_surface.get_height(), + gl.GL_RGBA, gl.GL_UNSIGNED_BYTE, text_data) + y_offset -= 30 + + # 恢复3D设置 + gl.glEnable(gl.GL_DEPTH_TEST) + gl.glEnable(gl.GL_LIGHTING) + + gl.glMatrixMode(gl.GL_PROJECTION) + gl.glPopMatrix() + gl.glMatrixMode(gl.GL_MODELVIEW) + gl.glPopMatrix() + + def run_terminal_display(self): + """终端显示模式(备用)""" + import time + + self.get_logger().info('切换到终端显示模式') + + while rclpy.ok(): + # 显示状态信息 + status = ( + f"\n{'='*60}\n" + f"无人机3D仿真系统 (终端模式)\n" + f"{'='*60}\n" + f"位置: X={self.drone_position[0]:.2f} Y={self.drone_position[1]:.2f} Z={self.drone_position[2]:.2f}\n" + f"电池: {self.drone_battery:.1f}%\n" + f"模式: {self.drone_mode}\n" + f"解锁: {'是' if self.drone_armed else '否'}\n" + f"当前手势: {self.current_gesture}\n" + f"当前命令: {self.current_command}\n" + f"{'='*60}\n" + ) + + print(status) + time.sleep(2) + +def main(args=None): + rclpy.init(args=args) + node = VisualizationNode() + try: + rclpy.spin(node) + except KeyboardInterrupt: + node.get_logger().info('🛑 3D可视化节点正在关闭...') + finally: + node.destroy_node() + rclpy.shutdown() + +if __name__ == '__main__': + main() diff --git a/src/Drone_hand_gesture_project/drone_gesture_ros2/launch/drone_gesture.launch.py b/src/Drone_hand_gesture_project/drone_gesture_ros2/launch/drone_gesture.launch.py new file mode 100644 index 0000000000..52c1db86aa --- /dev/null +++ b/src/Drone_hand_gesture_project/drone_gesture_ros2/launch/drone_gesture.launch.py @@ -0,0 +1,34 @@ +from launch import LaunchDescription +from launch_ros.actions import Node + +def generate_launch_description(): + return LaunchDescription([ + # 手势控制节点 + Node( + package='drone_gesture_control', + executable='gesture_control_node', + name='gesture_control_node', + output='screen', + parameters=[ + {'simulation_mode': True}, + {'camera_id': 1}, + {'ml_model_path': 'dataset/models/gesture_svm.pkl'} + ] + ), + + # 可视化节点 + Node( + package='drone_gesture_control', + executable='visualization_node', + name='visualization_node', + output='screen' + ), + + # 可选的:状态监控节点 + Node( + package='drone_gesture_control', + executable='status_monitor_node', + name='status_monitor_node', + output='screen' + ) + ]) diff --git a/src/Drone_hand_gesture_project/drone_gesture_ros2/package.xml b/src/Drone_hand_gesture_project/drone_gesture_ros2/package.xml new file mode 100644 index 0000000000..96803b27a4 --- /dev/null +++ b/src/Drone_hand_gesture_project/drone_gesture_ros2/package.xml @@ -0,0 +1,32 @@ + + + + drone_gesture_control + 0.0.1 + Drone hand gesture control package using ROS 2 + arrebol + Apache-2.0 + + ament_python + + rclpy + std_msgs + geometry_msgs + sensor_msgs + + python3-opencv + python3-mediapipe + python3-numpy + python3-scikit-learn + python3-pygame + python3-pyopengl + + ament_copyright + ament_flake8 + ament_pep257 + python3-pytest + + + ament_python + + diff --git a/src/Drone_hand_gesture_project/drone_gesture_ros2/setup.py b/src/Drone_hand_gesture_project/drone_gesture_ros2/setup.py new file mode 100644 index 0000000000..f6248c998d --- /dev/null +++ b/src/Drone_hand_gesture_project/drone_gesture_ros2/setup.py @@ -0,0 +1,35 @@ +from setuptools import setup +import os +from glob import glob + +package_name = 'drone_gesture_control' + +setup( + name=package_name, + version='0.0.1', + packages=[package_name, f'{package_name}.nodes', f'{package_name}.scripts'], + data_files=[ + ('share/ament_index/resource_index/packages', + ['resource/' + package_name]), + ('share/' + package_name, ['package.xml']), + (os.path.join('share', package_name, 'launch'), glob('launch/*.launch.py')), + (os.path.join('share', package_name, 'config'), glob('config/*.yaml')), + ], + install_requires=['setuptools'], + zip_safe=True, + maintainer='arrebol', + maintainer_email='arrebol@example.com', + description='Drone hand gesture control using ROS 2', + license='Apache-2.0', + tests_require=['pytest'], + entry_points={ + 'console_scripts': [ + 'gesture_control_node = drone_gesture_control.nodes.gesture_control_node:main', + 'visualization_node = drone_gesture_control.nodes.visualization_node:main', + 'visualization_node_fixed = drone_gesture_control.nodes.visualization_node_fixed:main', + 'status_monitor_node = drone_gesture_control.nodes.status_monitor_node:main', + 'train_gesture_model = drone_gesture_control.scripts.train_gesture_model:main', + 'collect_gesture_data = drone_gesture_control.scripts.gesture_data_collector:main', + ], + }, +)