fix: return to center

autopilot.py

@@ -9,7 +9,7 @@ random.seed(1)
 class Pilot:
     def __init__(self): pass
     def handle(self, image: Image): pass
-    def act(self) -> float | None: pass
+    def act(self) -> tuple[float, float] | None: pass
 
 class AutoPilot(Pilot):
     prev_image: np.ndarray | None
@@ -89,11 +89,11 @@ class AutoPilot(Pilot):
         for match in good_matches:
             # Координаты точки в первом изображении
             pt1 = kp1[match.queryIdx].pt
-            src_pts.append([pt1[0] - center_x1, pt1[1] - center_y1])
+            src_pts.append([center_x1 - pt1[0], pt1[1] - center_y1])
             
             # Координаты точки во втором изображении
             pt2 = kp2[match.trainIdx].pt
-            dst_pts.append([pt2[0] - center_x2, pt2[1] - center_y2])
+            dst_pts.append([center_x2 - pt2[0], pt2[1] - center_y2])
         
         # Конвертируем в numpy массивы
         src_pts = np.float32(src_pts).reshape(-1, 1, 2)
@@ -125,7 +125,7 @@ class AutoPilot(Pilot):
         tx, ty = H[0, 2], H[1, 2]
         
         # Вычисляем угол поворота
-        angle = np.arctan2(a21, a11)
+        angle = -np.arctan2(a21, a11)
         
         # Вычисляем масштаб
         scale_x = np.sqrt(a11**2 + a21**2)
@@ -150,8 +150,8 @@ class AutoPilot(Pilot):
         scale = transformation_info['scale']
         
         # Координаты уже отцентрированы, поэтому используем их напрямую
-        dx_meters = tx
+        dx_meters = ty
-        dy_meters = ty
+        dy_meters = tx
         
         # Применяем поворот к смещению (учитываем текущий угол БПЛА)
         cos_angle = math.cos(self.angle)
@@ -159,8 +159,8 @@ class AutoPilot(Pilot):
         
         # Поворачиваем смещение в глобальные координаты
         # Обратите внимание: dy_meters инвертирован, так как в изображениях Y направлен вниз
-        dx_global = dx_meters * cos_angle - (-dy_meters) * sin_angle
+        dx_global = dx_meters * cos_angle - dy_meters * sin_angle
-        dy_global = dx_meters * sin_angle + (-dy_meters) * cos_angle
+        dy_global = dx_meters * sin_angle + dy_meters * cos_angle
         
         # Обновляем координаты БПЛА
         self.x += dx_global
@@ -276,7 +276,7 @@ class AutoPilot(Pilot):
         # Обновляем предыдущее изображение
         self.prev_image = current_image
 
-    def act(self) -> float | None:
+    def act(self) -> tuple[float, float] | None:
         """
         Возвращает угол поворота для управления дроном, чтобы он стремился к точке (0, 0).
         Если дрон находится рядом с началом координат (в радиусе 1 метра), возвращает None.
@@ -284,8 +284,8 @@ class AutoPilot(Pilot):
         # Расстояние до цели (0, 0)
         distance_to_target = math.sqrt(self.x**2 + self.y**2)
         
-        # Если дрон находится рядом с целью (в радиусе 1 метра), останавливаемся
+        # Если дрон находится рядом с целью, останавливаемся
-        if distance_to_target < 1.0:
+        if distance_to_target < 10.0:
             return None
         
         # Вычисляем угол к цели
@@ -302,7 +302,7 @@ class AutoPilot(Pilot):
             angle_diff -= 2 * math.pi
 
         # Возвращаем угол поворота (положительный - поворот влево, отрицательный - вправо)
-        return max(min(0.05, angle_diff / 2), -0.05)
+        return max(min(0.1, angle_diff), -0.1), min(10., distance_to_target / 2)
 
     def reset_position(self, x: float = 0.0, y: float = 0.0, angle: float = 0.0):
         """Сбрасывает позицию и угол БПЛА"""
@@ -318,12 +318,12 @@ class RandomPilot(Pilot):
     def __init__(self, velocity: float = 1):
         self.counter = 0
 
-    def act(self) -> float | None:
+    def act(self) -> tuple[float, float] | None:
         self.counter += 1
         if self.counter > 40:
             return None
         
-        return 1 / (self.counter + 20)
+        return 1 / (self.counter + 20), 10.0
 
 # def _test():
 #     randomPilot = RandomPilot()

simulator.py

@@ -107,26 +107,27 @@ class Simulator:
 
         html = self.driver.find_element(By.TAG_NAME, 'html')
         action = ActionChains(self.driver)
-        velocity = 10
 
         # Добавляем начальную точку в траекторию
         self.update_trajectory(0, 0)
         self.viz_manager.update_global_map(self.current_x, self.current_y, self.mode)
 
         for i in range(1000):
-            dangle = None
+            signal = None
             if self.mode == SimMode.OPERATOR:
-                dangle = self.operatorPilot.act()
+                signal = self.operatorPilot.act()
-                if dangle is None:
+                if signal is None:
                     self.mode = SimMode.AUTONOME
                     print("Режим возвращения домой!")
 
             if self.mode == SimMode.AUTONOME:
-                dangle = self.autonomePilot.act()
+                signal = self.autonomePilot.act()
 
-            if dangle is None:
+            if signal is None:
                 break
 
+            dangle, velocity = signal
+
             # Сдвиг камеры
             action = ActionChains(self.driver)
             action.move_to_element_with_offset(html, 200, 200)
@@ -148,7 +149,7 @@ class Simulator:
             im = im.crop([0, 80, im.width-80, im.height-60])
             
             # Применяем поворот как будто съемка с дрона
-            rotated_im = self.rotate_image_like_drone(im, self.angle + math.pi / 2)
+            rotated_im = self.rotate_image_like_drone(im, math.pi / 2 - self.angle)
             
             # Передаем изображение в AutoPilot для анализа
             self.autonomePilot.handle(rotated_im)

visualization.py

@@ -27,6 +27,7 @@ class VisualizationManager:
     def __init__(self, window_title="Drone Autopilot Visualization"):
         self.window_title = window_title
         self.fig = None
+        self.ax_drone_view = None
         self.ax_global_map = None
         self.ax_detection = None
         self.ax_matches = None
@@ -43,6 +44,9 @@ class VisualizationManager:
         self.keypoints = []
         self.matches = []
         
+        # Данные для вида БПЛА
+        self.drone_view_image = None
+        
         self._setup_window()
     
     def _setup_window(self):
@@ -54,11 +58,16 @@ class VisualizationManager:
         # Открываем окно на полный экран
         self.fig.canvas.manager.window.state('zoomed')
         
-        # Создаем сетку 2x2
+        # Создаем сетку 2x2 с разными размерами колонок
-        gs = self.fig.add_gridspec(2, 2, hspace=0.3, wspace=0.3)
+        gs = self.fig.add_gridspec(2, 2, hspace=0.3, wspace=0.3, width_ratios=[1, 1])
         
-        # Глобальная карта (левый верхний угол, занимает 2x1)
+        # Вид БПЛА (левый верхний угол)
-        self.ax_global_map = self.fig.add_subplot(gs[0, :])
+        self.ax_drone_view = self.fig.add_subplot(gs[0, 0])
+        self.ax_drone_view.set_title('Вид БПЛА')
+        self.ax_drone_view.axis('off')
+        
+        # Глобальная карта (левый нижний угол)
+        self.ax_global_map = self.fig.add_subplot(gs[1, 0])
         self.ax_global_map.set_title('Global Map - Траектория полета беспилотника')
         self.ax_global_map.set_xlabel('X координата')
         self.ax_global_map.set_ylabel('Y координата')
@@ -66,8 +75,8 @@ class VisualizationManager:
         self.ax_global_map.axhline(y=0, color='k', linestyle='-', alpha=0.3)
         self.ax_global_map.axvline(x=0, color='k', linestyle='-', alpha=0.3)
         
-        # Детекция ключевых точек (правый нижний угол)
+        # Детекция ключевых точек (правый верхний угол)
-        self.ax_detection = self.fig.add_subplot(gs[1, 0])
+        self.ax_detection = self.fig.add_subplot(gs[0, 1])
         self.ax_detection.set_title('Keypoint Detection')
         self.ax_detection.axis('off')
         
@@ -100,7 +109,7 @@ class VisualizationManager:
         if len(self.trajectory_x) > 1:
             # Разделяем траекторию по режимам
             operator_indices = [i for i, m in enumerate(self.trajectory_modes) if m == SimMode.OPERATOR]
-            autonome_indices = [i for i, m in enumerate(self.trajectory_modes) if m == SimMode.AUTONOME]
+            autonome_indices = operator_indices[-1:] + [i for i, m in enumerate(self.trajectory_modes) if m == SimMode.AUTONOME]
             
             # Рисуем траекторию оператора (синий цвет)
             if len(operator_indices) > 1:
@@ -139,6 +148,24 @@ class VisualizationManager:
             self.ax_global_map.set_xlim(x_min - margin, x_max + margin)
             self.ax_global_map.set_ylim(y_min - margin, y_max + margin)
     
+    def update_drone_view(self, image: np.ndarray):
+        """Обновляет вид БПЛА"""
+        self.drone_view_image = image.copy()
+        
+        self.ax_drone_view.clear()
+        self.ax_drone_view.set_title('Вид БПЛА')
+        
+        if image is not None:
+            # Конвертируем BGR в RGB для matplotlib
+            if len(image.shape) == 3 and image.shape[2] == 3:
+                image_rgb = cv2.cvtColor(image, cv2.COLOR_BGR2RGB)
+            else:
+                image_rgb = image
+            
+            self.ax_drone_view.imshow(image_rgb)
+        
+        self.ax_drone_view.axis('off')
+    
     def update_detection(self, image: np.ndarray, keypoints):
         """Обновляет визуализацию детекции ключевых точек"""
         self.current_frame = image.copy()