gesture control node started
This commit is contained in:
moscovskayaliza
@@ -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)
|
||||
|
||||
|
||||
Reference in New Issue
Block a user