gesture control node started

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):
@@ -24,6 +29,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):
         try:
@@ -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)