From 387c8175e02199ac0dc7f2352fed4740b2242346 Mon Sep 17 00:00:00 2001 From: moscovskayaliza Date: Thu, 25 Jun 2026 16:10:59 +0300 Subject: [PATCH] gesture control node started --- .../base_gesture_control_node.py | 143 +++++++++++++++++- 1 file changed, 138 insertions(+), 5 deletions(-) diff --git a/unity_robot_controller/base_gesture_control_node.py b/unity_robot_controller/base_gesture_control_node.py index 895846e..c899161 100644 --- a/unity_robot_controller/base_gesture_control_node.py +++ b/unity_robot_controller/base_gesture_control_node.py @@ -9,6 +9,11 @@ import cv2 import numpy as np from sensor_msgs.msg import Image +from skeleton.mediapipe_detector import MediaPipeDetector +from gesture_control.special_gestures import SpecialGestureDetector +from gesture_control.arm_control import ArmController +from ml_gestures_dynamic.predict import DynamicGesturePredictor + class BaseGestureControlNode(Node): def __init__(self, RobotController): @@ -23,6 +28,56 @@ class BaseGestureControlNode(Node): # subscriptions self.create_subscription(Image, 'image', self.image_cb, 10) + + #ros params? + ARM_CONTROL = { + 'linear_arm': 'right', + 'angular_arm': 'left', + 'max_speed_linear': 1.0, + 'max_speed_angular': 1.0, + 'dead_zone': 0.2, + 'debug': False + } + + SPECIAL_GESTURE_MODE = 'geometric' # 'ml' или 'geometric' + ML_GESTURE_MODEL = "path/to/special_gestures_rf.pkl" + ML_GESTURE_CLASSES ['dome', 'cross', 'none'] + + DG_ENABLED = True + DG_MODEL_PATH = "path/to/dynamic_model.h5" + DG_CLASSES_PATH = "path/to/dynamic_model_classes.pkl" + DG_WINDOW_SIZE = 14 + DG_THRESH = 0.8 + + # gestures + self.detector = MediaPipeDetector() + + self.arm_control = ArmController(ARM_CONTROL, mirror=True) + + self.special_detector = SpecialGestureDetector( + mode=SPECIAL_GESTURE_MODE, + model_path=ML_GESTURE_MODEL, + class_names=ML_GESTURE_CLASSES + ) + + self.dynamic_predictor = None + if DG_ENABLED: + self.dynamic_predictor = DynamicGesturePredictor( + DG_MODEL_PATH, + DG_CLASSES_PATH, + DG_WINDOW_SIZE, + DG_THRESH + ) + + # Состояния управления + self.enabled = False # разрешено ли управление жестами + self.cross_cooldown = 0 + self.CROSS_COOLDOWN_FRAMES = 30 # сколько кадров блокировки после переключения ждем + self.dome_processed = False # предотвращает многократное срабатывание тушителя + self.last_dynamic = None # последний жест распознанный + self.dynamic_counter = 0 # отображение последнего распознанного динамического жеста в течение 30 кадров + + self.image_pub = self.create_publisher(Image, 'skeleton_image', 10) def image_cb(self, msg): @@ -38,9 +93,89 @@ class BaseGestureControlNode(Node): while rclpy.ok(): if not self.cv_image is None: - - # DO DETECTION HERE - self.cv_image + frame = self.cv_image.copy() + + # Детекция скелета + result = self.detector.detect(frame) + if result['success']: + landmarks = result['landmarks'] + pose_landmarks = result.get('pose_landmarks') + else: + landmarks = None + pose_landmarks = None + + vis_image = frame.copy() + if landmarks is not None: + + # Распознавание статичного жеста + special = self.special_detector.predict(landmarks) + + # Вкл/выкл управления жестами + if special == 'cross' and self.cross_cooldown == 0: + self.enabled = not self.enabled + self.cross_cooldown = self.CROSS_COOLDOWN_FRAMES + self.get_logger().info( + f"Gesture control {'ENABLED' if self.enabled else 'DISABLED'}" + ) + if self.cross_cooldown > 0: + self.cross_cooldown -= 1 + + # Команда тушения + if self.enabled: + if special == 'dome' and not self.dome_processed: + self.URC.send_fire_ext_burst_cmd(): + self.dome_processed = True + elif special != 'dome': + self.dome_processed = False + + # Скорости + linear, angular = self.arm_control.compute_speeds(landmarks) + if self.enabled: + self.URC.send_speed_cmd(linear, angular) + else: + self.URC.send_speed_cmd(0.0, 0.0) + + # Распознавание динамических жестов + if self.dynamic_predictor: + self.dynamic_predictor.add_frame(landmarks) + dynamic_gesture = self.dynamic_predictor.predict() + if dynamic_gesture: + self.last_dynamic = dynamic_gesture + self.dynamic_counter = 30 + self.get_logger().info(f"Dynamic gesture: {dynamic_gesture}") + ''' + Дальше можно уже отсылать команду в зависимости от жеста. Доступны: 'wave_left', 'wave_right' + ''' + # Отрисовка + # Скелет + vis_image = self.detector.draw_landmarks(vis_image, pose_landmarks) + + # Имена жестов и т.п. + cv2.putText(vis_image, f"Move: {'ON' if self.enabled else 'OFF'}", + (10, 30), cv2.FONT_HERSHEY_SIMPLEX, 1, + (0, 255, 0) if self.enabled else (0, 0, 255), 2) + cv2.putText(vis_image, f"L:{linear:.2f} A:{angular:.2f}", + (10, 60), cv2.FONT_HERSHEY_SIMPLEX, 0.7, + (255, 255, 0), 2) + if special != 'none': + cv2.putText(vis_image, f"Special: {special}", + (10, 90), cv2.FONT_HERSHEY_SIMPLEX, 0.7, + (255, 255, 0), 2) + if self.dynamic_counter > 0 and self.last_dynamic: + cv2.putText(vis_image, f"Dynamic: {self.last_dynamic}", + (10, 120), cv2.FONT_HERSHEY_SIMPLEX, 0.7, + (0, 255, 255), 2) + self.dynamic_counter -= 1 + else: + # Скелета нет - стоим + self.URC.send_speed_cmd(0.0, 0.0) + cv2.putText(vis_image, "No pose detected", + (10, 30), cv2.FONT_HERSHEY_SIMPLEX, 1, + (0, 0, 255), 2) + + # Паблишер изображения со скелетом + image_msg = self.bridge.cv2_to_imgmsg(vis_image, encoding='bgr8') + self.image_pub.publish(image_msg) # SELECT COMMAND @@ -54,8 +189,6 @@ class BaseGestureControlNode(Node): rclpy.shutdown() - - def main(args=None): rclpy.init(args=args)