You cannot select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
131 lines
4.5 KiB
Python
131 lines
4.5 KiB
Python
import sys
|
|
import os
|
|
|
|
sys.path.insert(0, os.path.join(os.path.dirname(__file__), 'submodules', 'gesture_detection'))
|
|
|
|
import cv2
|
|
from config import Config
|
|
from skeleton.mediapipe_detector import MediaPipeDetector
|
|
from gesture_control.special_gestures import SpecialGestureDetector
|
|
from ml_gestures_dynamic.predict import DynamicGesturePredictor
|
|
|
|
from geom_controll.arm_control import ArmController
|
|
from robot.simulated_robot import DummySimRobot
|
|
from robot.debug_robot import DebugRobot
|
|
|
|
def main():
|
|
cfg = Config()
|
|
detector = MediaPipeDetector()
|
|
special_detector = SpecialGestureDetector(
|
|
mode=cfg.SPECIAL_GESTURE_MODE,
|
|
model_path=cfg.ML_GESTURE_MODEL,
|
|
class_names=cfg.ML_GESTURE_CLASSES
|
|
)
|
|
|
|
dynamic_predictor = None
|
|
|
|
if cfg.DYNAMIC_GESTURE['enabled']:
|
|
dynamic_predictor = DynamicGesturePredictor(
|
|
cfg.DYNAMIC_GESTURE['model_path'],
|
|
cfg.DYNAMIC_GESTURE['classes_path'],
|
|
cfg.DYNAMIC_GESTURE['window_size'],
|
|
cfg.DYNAMIC_GESTURE['threshold']
|
|
)
|
|
|
|
arm_control = ArmController(cfg.ARM_CONTROL, mirror=cfg.MIRROR_CAMERA)
|
|
if cfg.ROBOT_MODE == 'simulator':
|
|
robot = DummySimRobot(cfg)
|
|
else:
|
|
robot = DebugRobot()
|
|
|
|
cap = cv2.VideoCapture(cfg.CAMERA_ID)
|
|
if not cap.isOpened():
|
|
print("Ошибка: не удалось открыть камеру")
|
|
sys.exit(1)
|
|
|
|
enabled = False
|
|
# включение управления жестами
|
|
cross_cooldown = 0
|
|
CROSS_COOLDOWN_FRAMES = 30
|
|
last_dynamic = None
|
|
dynamic_counter = 0
|
|
dome_processed = False
|
|
|
|
while True:
|
|
ret, frame = cap.read()
|
|
if not ret:
|
|
break
|
|
if cfg.MIRROR_CAMERA:
|
|
frame = cv2.flip(frame, 1)
|
|
|
|
result = detector.detect(frame)
|
|
if result['success']:
|
|
landmarks = result['landmarks']
|
|
|
|
# Статический жест
|
|
|
|
# Вкл/выкл управления жестами
|
|
if special == 'cross' and cross_cooldown == 0:
|
|
enabled = not enabled
|
|
cross_cooldown = CROSS_COOLDOWN_FRAMES
|
|
if cross_cooldown > 0:
|
|
cross_cooldown -= 1
|
|
|
|
# Тушение
|
|
if special == 'dome' and not dome_processed:
|
|
robot.send_fire_ext_burst_cmd()
|
|
dome_processed = True
|
|
elif special != 'dome':
|
|
dome_processed = False
|
|
|
|
# Скорости из рук
|
|
linear, angular = arm_control.compute_speeds(landmarks)
|
|
if enabled:
|
|
robot.send_speed_cmd(linear, angular)
|
|
else:
|
|
robot.send_speed_cmd(0.0, 0.0)
|
|
|
|
# Динамический жест
|
|
dynamic_gesture = None
|
|
if dynamic_predictor:
|
|
dynamic_predictor.add_frame(landmarks)
|
|
dynamic_gesture = dynamic_predictor.predict()
|
|
if dynamic_gesture:
|
|
last_dynamic = dynamic_gesture
|
|
dynamic_counter = 30
|
|
|
|
if dynamic_gesture == 'wave_left':
|
|
robot.reset_position()
|
|
elif dynamic_gesture == 'wave_right':
|
|
robot.restart_game()
|
|
|
|
# Отрисовка
|
|
vis = detector.draw_landmarks(frame, result['pose_landmarks'])
|
|
cv2.putText(vis, f"Move: {'ON' if enabled else 'OFF'}", (10, 30), cv2.FONT_HERSHEY_SIMPLEX, 1, (0,255,0) if enabled else (0,0,255), 2)
|
|
cv2.putText(vis, 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, f"Special: {special}", (10, 90), cv2.FONT_HERSHEY_SIMPLEX, 0.7, (255,255,0), 2)
|
|
if dynamic_counter > 0 and last_dynamic:
|
|
cv2.putText(vis, f"Dynamic: {last_dynamic}", (10, 120), cv2.FONT_HERSHEY_SIMPLEX, 0.7, (0,255,255), 2)
|
|
dynamic_counter -= 1
|
|
|
|
else:
|
|
vis = frame
|
|
cv2.putText(vis, "No pose detected", (10, 30), cv2.FONT_HERSHEY_SIMPLEX, 1, (0,0,255), 2)
|
|
|
|
cv2.imshow('Camera', vis)
|
|
|
|
if not robot.step():
|
|
break
|
|
|
|
if cv2.waitKey(1) & 0xFF == ord('q'):
|
|
break
|
|
|
|
cap.release()
|
|
cv2.destroyAllWindows()
|
|
robot.quit()
|
|
|
|
if __name__ == '__main__':
|
|
main()
|
|
|