feat: main loop

autopilot.py

@@ -1,8 +1,11 @@
+from enum import Enum
+import math
 import random
+
 import cv2
 import numpy as np
 from PIL import Image
-import math
+
 from visualization import VisualizationManager
 
 random.seed(1)
@@ -13,25 +16,42 @@ class Pilot:
     def act(self) -> tuple[float, float] | None: pass
     def get_position(self) -> tuple[float, float]: pass
 
+class PilotCommand:
+    dangle: float
+    stop: bool
+
+    def __init__(self, dangle: float = 0, stop: bool = False):
+        self.dangle = dangle
+        self.stop = stop
+
 class AutoPilot(Pilot):
-    prev_image: np.ndarray | None
+
+    # Состояние одометрии
     angle: float
     x: float  # Координата X БПЛА
     y: float  # Координата Y БПЛА
+
+    # Ориентиры
+    points: list[tuple[float,float]] # Координаты
+    keypoints: list[any]             # Ключевые точки ориентиров
+    target_idx: int                  # Текущая целевая метка
+
+    # Всякие вспомогательные штуки
+    prev_image: np.ndarray | None
     orb_detector: cv2.ORB
     bf_matcher: cv2.BFMatcher
     frame_count: int
     image_center: tuple  # Центр изображения (x, y)
-    viz_manager: VisualizationManager  # Менеджер визуализации (опционально)
+    vis_manager: VisualizationManager  # Менеджер визуализации (опционально)
 
-    def __init__(self, viz_manager=None):
+    def __init__(self, points = [], keypoints = [], viz_manager=None):
         self.prev_image = None
         self.angle = 0.0
         self.x = 0.0
         self.y = 0.0
         self.frame_count = 0
         self.image_center = (0, 0)  # Будет обновлено при получении первого изображения
-        self.viz_manager = viz_manager  # Менеджер визуализации
+        self.vis_manager = viz_manager  # Менеджер визуализации
 
         # Инициализация ORB детектора
         self.orb_detector = cv2.ORB_create(
@@ -48,7 +68,11 @@ class AutoPilot(Pilot):
         # Инициализация матчера для сопоставления ключевых точек
         self.bf_matcher = cv2.BFMatcher(cv2.NORM_HAMMING, crossCheck=True)
 
-    def get_position(self):
+        self.points = points
+        self.keypoints = keypoints
+        self.target_idx = 0
+
+    def get_position(self) -> tuple[float, float]:
         return self.x, self.y
 
     def image_to_numpy(self, image: Image.Image) -> np.ndarray:
@@ -224,9 +248,9 @@ class AutoPilot(Pilot):
         
         return img_matches
 
-    def handle(self, image: Image):
+    def handle(self, image: Image) -> PilotCommand:
         self.frame_count += 1
-        
+
         if self.prev_image is None:
             self.prev_image = self.image_to_numpy(image)
             # Вычисляем центр изображения
@@ -234,11 +258,11 @@ class AutoPilot(Pilot):
             self.image_center = (width // 2, height // 2)
             
             # Обновляем визуализацию детекции
-            if self.viz_manager:
+            if self.vis_manager:
                 kp, _ = self.orb_detector.detectAndCompute(self.prev_image, None)
-                self.viz_manager.update_detection(self.prev_image, kp)
+                self.vis_manager.update_detection(self.prev_image, kp)
             
-            return
+            return PilotCommand()
         
         # Конвертируем текущее изображение
         current_image = self.image_to_numpy(image)
@@ -260,27 +284,21 @@ class AutoPilot(Pilot):
                 
                 # Выводим текущее состояние БПЛА
                 drone_state = self.get_drone_state()
-                self.viz_manager.update_drone_trajectory(drone_state['x'], drone_state['y'])
+                self.vis_manager.update_drone_trajectory(drone_state['x'], drone_state['y'])
                 print(f"  [Pilot] Drone Position: ({drone_state['x']:.2f}, {drone_state['y']:.2f})")
                 print(f"  [Pilot] Angle: {drone_state['angle_degrees']:.1f}°")
                 
                 # Обновляем визуализацию
-                if self.viz_manager:
+                if self.vis_manager:
                     # Обновляем детекцию ключевых точек
-                    self.viz_manager.update_detection(current_image, kp2)
+                    self.vis_manager.update_detection(current_image, kp2)
                     # Обновляем сопоставление точек
-                    self.viz_manager.update_matches(self.prev_image, current_image, 
+                    self.vis_manager.update_matches(self.prev_image, current_image, 
                                                    kp1, kp2, matches, transformation_info)
-                
-                # Визуализация (опционально, если нет менеджера визуализации)
-                if not self.viz_manager:
-                    img_matches = self.visualize_matches(self.prev_image, current_image, 
-                                                       kp1, kp2, matches, transformation_info)
-                    cv2.imshow('Drone Tracking', img_matches)
-                    cv2.waitKey(1)
         
         # Обновляем предыдущее изображение
         self.prev_image = current_image
+        return PilotCommand()
 
     def act(self) -> tuple[float, float] | None:
         """