|
|
import cv2
|
|
|
import sys
|
|
|
from config import Config
|
|
|
from skeleton.mediapipe_detector import MediaPipeDetector
|
|
|
from gesture_control.arm_control import ArmController
|
|
|
from gesture_control.special_gestures import SpecialGestureDetector
|
|
|
from gesture_control.state import ControlState
|
|
|
from robot.map_simulator import MapSimulator
|
|
|
from robot.dummy import DummyRobot
|
|
|
|
|
|
def main():
|
|
|
cfg = Config()
|
|
|
|
|
|
# Детектор
|
|
|
detector = MediaPipeDetector()
|
|
|
|
|
|
# Состояние управления
|
|
|
state = ControlState()
|
|
|
|
|
|
# Специальные жесты
|
|
|
special_detector = SpecialGestureDetector(
|
|
|
mode=cfg.SPECIAL_GESTURE_MODE,
|
|
|
model_path=cfg.ML_GESTURE_MODEL,
|
|
|
class_names=cfg.ML_GESTURE_CLASSES
|
|
|
)
|
|
|
|
|
|
# Динамические жесты
|
|
|
if cfg.DYNAMIC_GESTURE['enabled']:
|
|
|
from ml_gestures_dynamic.predict import DynamicGesturePredictor
|
|
|
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 = MapSimulator(cfg)
|
|
|
else:
|
|
|
robot = DummyRobot()
|
|
|
|
|
|
# Камера
|
|
|
cap = cv2.VideoCapture(cfg.CAMERA_ID)
|
|
|
if not cap.isOpened():
|
|
|
print("Ошибка: не удалось открыть камеру")
|
|
|
sys.exit(1)
|
|
|
|
|
|
print("Управление: крест руками = СТОП, домик = ПУСК")
|
|
|
print("Линейная скорость: правая рука в сторону, угловая: левая рука")
|
|
|
print("В симуляторе: R – перезапуск после Game Over/победы")
|
|
|
print("q – выход")
|
|
|
|
|
|
last_dynamic_gesture = None
|
|
|
dynamic_gesture_counter = 0
|
|
|
|
|
|
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 cfg.DYNAMIC_GESTURE['enabled']:
|
|
|
dynamic_predictor.add_frame(landmarks)
|
|
|
gesture = dynamic_predictor.predict()
|
|
|
if gesture:
|
|
|
action = cfg.DYNAMIC_GESTURE['actions'].get(gesture)
|
|
|
if action == 'reset':
|
|
|
robot.reset()
|
|
|
elif action == 'restart':
|
|
|
robot.reset()
|
|
|
last_dynamic_gesture = gesture
|
|
|
dynamic_gesture_counter = 30
|
|
|
|
|
|
vis_frame = detector.draw_landmarks(frame, result['pose_landmarks'])
|
|
|
|
|
|
special = special_detector.predict(landmarks)
|
|
|
state.update(special)
|
|
|
|
|
|
if state.enabled:
|
|
|
linear, angular = arm_control.compute_speeds(landmarks)
|
|
|
else:
|
|
|
linear, angular = 0.0, 0.0
|
|
|
|
|
|
robot.set_speeds(linear, angular)
|
|
|
|
|
|
# Отрисовка на видео
|
|
|
cv2.putText(vis_frame, f"State: {'ON' if state.enabled else 'OFF'}",
|
|
|
(10, 30), cv2.FONT_HERSHEY_SIMPLEX, 1,
|
|
|
(0,255,0) if state.enabled else (0,0,255), 2)
|
|
|
cv2.putText(vis_frame, 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_frame, f"Special: {special}", (10, 90),
|
|
|
cv2.FONT_HERSHEY_SIMPLEX, 0.7, (255,255,0), 2)
|
|
|
|
|
|
if dynamic_gesture_counter > 0 and last_dynamic_gesture:
|
|
|
cv2.putText(vis_frame, f"Dynamic: {last_dynamic_gesture}", (10, 120),
|
|
|
cv2.FONT_HERSHEY_SIMPLEX, 0.7, (0, 255, 255), 2)
|
|
|
dynamic_gesture_counter -= 1
|
|
|
|
|
|
else:
|
|
|
vis_frame = frame
|
|
|
cv2.putText(vis_frame, "No pose detected", (10, 30),
|
|
|
cv2.FONT_HERSHEY_SIMPLEX, 1, (0,0,255), 2)
|
|
|
|
|
|
cv2.imshow('Camera', vis_frame)
|
|
|
|
|
|
if cfg.ROBOT_MODE == 'simulator':
|
|
|
if not robot.step():
|
|
|
break
|
|
|
else:
|
|
|
robot.step()
|
|
|
|
|
|
if cv2.waitKey(1) & 0xFF == ord('q'):
|
|
|
break
|
|
|
|
|
|
cap.release()
|
|
|
cv2.destroyAllWindows()
|
|
|
robot.quit()
|
|
|
|
|
|
if __name__ == '__main__':
|
|
|
main()
|