feat: build map and realize act method

autopilot.py

@@ -209,18 +209,18 @@ class AutoPilot(Pilot):
             transformation_info = self.estimate_transformation_matrix(src_pts, dst_pts)
             
             if transformation_info:
-                print(f"Frame {self.frame_count}:")
-                print(f"  Translation: {transformation_info['translation']}")
-                print(f"  Rotation: {transformation_info['rotation']:.4f} rad")
-                print(f"  Scale: {transformation_info['scale']:.4f}")
+                # print(f"Frame {self.frame_count}:")
+                # print(f"  Translation: {transformation_info['translation']}")
+                # print(f"  Rotation: {transformation_info['rotation']:.4f} rad")
+                # print(f"  Scale: {transformation_info['scale']:.4f}")
                 
                 # Обновляем позицию и угол БПЛА
                 self.update_drone_position(transformation_info)
                 
                 # Выводим текущее состояние БПЛА
                 drone_state = self.get_drone_state()
-                print(f"  Drone Position: ({drone_state['x']:.2f}, {drone_state['y']:.2f})")
-                print(f"  Drone Angle: {drone_state['angle_degrees']:.1f}°")
+                print(f"  [Pilot] Drone Position: ({drone_state['x']:.2f}, {drone_state['y']:.2f})")
+                print(f"  [Pilot] Angle: {drone_state['angle_degrees']:.1f}°")
                 
                 # Визуализация (опционально)
                 img_matches = self.visualize_matches(self.prev_image, current_image, 
@@ -231,9 +231,33 @@ class AutoPilot(Pilot):
         # Обновляем предыдущее изображение
         self.prev_image = current_image
 
-    def act(self) -> float:
-        """Возвращает угол поворота для управления дроном"""
-        return self.angle
+    def act(self) -> float | None:
+        """
+        Возвращает угол поворота для управления дроном, чтобы он стремился к точке (0, 0).
+        Если дрон находится рядом с началом координат (в радиусе 1 метра), возвращает None.
+        """
+        # Расстояние до цели (0, 0)
+        distance_to_target = math.sqrt(self.x**2 + self.y**2)
+        
+        # Если дрон находится рядом с целью (в радиусе 1 метра), останавливаемся
+        if distance_to_target < 1.0:
+            return None
+        
+        # Вычисляем угол к цели
+        target_angle = math.atan2(-self.y, -self.x)  # Отрицательные координаты, так как движемся к (0,0)
+        
+        # Вычисляем разность углов (направление поворота)
+        angle_diff = target_angle - self.angle
+
+        print(self.angle, target_angle, angle_diff)
+        
+        # Нормализуем разность углов в диапазон [-π, π]
+        angle_diff %= 2 * math.pi
+        if angle_diff >= math.pi:
+            angle_diff -= 2 * math.pi
+
+        # Возвращаем угол поворота (положительный - поворот влево, отрицательный - вправо)
+        return max(min(0.05, angle_diff / 2), -0.05)
 
     def reset_position(self, x: float = 0.0, y: float = 0.0, angle: float = 0.0):
         """Сбрасывает позицию и угол БПЛА"""
@@ -244,11 +268,17 @@ class AutoPilot(Pilot):
 
 
 class RandomPilot(Pilot):
+    counter: int
+    
     def __init__(self, velocity: float = 1):
-        pass
+        self.counter = 0
 
-    def act(self) -> float:
-        return 0.1
+    def act(self) -> float | None:
+        self.counter += 1
+        if self.counter > 40:
+            return None
+        
+        return 1 / (self.counter + 20)
 
 # def _test():
 #     randomPilot = RandomPilot()

simulator.py

@@ -10,6 +10,9 @@ from selenium.webdriver.common.action_chains import ActionChains
 from autopilot import Pilot
 
 from enum import Enum
+import matplotlib.pyplot as plt
+import numpy as np
+import os
 
 class SimMode(Enum):
     OPERATOR = 1
@@ -23,14 +26,43 @@ class Simulator:
     autonomePilot: Pilot
 
     angle: float
+    
+    # Переменные для отслеживания траектории
+    trajectory_x: list
+    trajectory_y: list
+    trajectory_modes: list  # Для отслеживания режимов полета
+    current_x: float
+    current_y: float
+    fig: plt.Figure
+    ax: plt.Axes
 
     def __init__(self, operatorPilot: Pilot, autonomePilot: Pilot):
         self.mode = SimMode.OPERATOR
         self.operatorPilot = operatorPilot
         self.autonomePilot = autonomePilot
 
+        # Инициализация переменных для траектории
+        self.trajectory_x = []
+        self.trajectory_y = []
+        self.trajectory_modes = []
+        self.current_x = 0.0
+        self.current_y = 0.0
+        
+        # Создание окна для карты
+        plt.ion()  # Включаем интерактивный режим
+        self.fig, self.ax = plt.subplots(figsize=(10, 8))
+        self.ax.set_title('Global Map - Траектория полета беспилотника')
+        self.ax.set_xlabel('X координата')
+        self.ax.set_ylabel('Y координата')
+        self.ax.grid(True, alpha=0.3)
+        self.ax.axhline(y=0, color='k', linestyle='-', alpha=0.3)
+        self.ax.axvline(x=0, color='k', linestyle='-', alpha=0.3)
+        
+        # Создаем папку для изображений, если её нет
+        os.makedirs('./images', exist_ok=True)
+
         options = webdriver.ChromeOptions()
-        # options.add_experimental_option("detach", True)
+        options.add_experimental_option("detach", True)
         self.driver = webdriver.Chrome(options)
         self.driver.get("https://yandex.ru/maps/43/kazan/?ll=49.103814%2C55.794258&z=14")
         sleep(2)
@@ -73,18 +105,110 @@ class Simulator:
 
         return final_image
 
+    def update_trajectory(self, dx: float, dy: float):
+        """
+        Обновляет траекторию полета беспилотника
+        """
+        # Обновляем текущие координаты
+        self.current_x += dx
+        self.current_y += dy
+        
+        # Добавляем точку в траекторию
+        self.trajectory_x.append(self.current_x)
+        self.trajectory_y.append(self.current_y)
+        self.trajectory_modes.append(self.mode)
+
+    def update_map(self):
+        """
+        Обновляет карту траектории полета
+        """
+        self.ax.clear()
+        self.ax.set_title('Global Map - Траектория полета беспилотника')
+        self.ax.set_xlabel('X координата')
+        self.ax.set_ylabel('Y координата')
+        self.ax.grid(True, alpha=0.3)
+        self.ax.axhline(y=0, color='k', linestyle='-', alpha=0.3)
+        self.ax.axvline(x=0, color='k', linestyle='-', alpha=0.3)
+        
+        if len(self.trajectory_x) > 1:
+            # Разделяем траекторию по режимам
+            operator_indices = [i for i, mode in enumerate(self.trajectory_modes) if mode == SimMode.OPERATOR]
+            autonome_indices = [i for i, mode in enumerate(self.trajectory_modes) if mode == SimMode.AUTONOME]
+            
+            # Рисуем траекторию оператора (синий цвет)
+            if len(operator_indices) > 1:
+                operator_x = [self.trajectory_x[i] for i in operator_indices]
+                operator_y = [self.trajectory_y[i] for i in operator_indices]
+                self.ax.plot(operator_x, operator_y, 'b-', linewidth=2, label='Режим оператора')
+            
+            # Рисуем траекторию автономного режима (красный цвет)
+            if len(autonome_indices) > 1:
+                autonome_x = [self.trajectory_x[i] for i in autonome_indices]
+                autonome_y = [self.trajectory_y[i] for i in autonome_indices]
+                self.ax.plot(autonome_x, autonome_y, 'r-', linewidth=2, label='Автономный режим')
+            
+            # Рисуем начальную точку (зеленая)
+            self.ax.plot(self.trajectory_x[0], self.trajectory_y[0], 'go', markersize=8, label='Начальная точка')
+            
+            # Рисуем текущую позицию (черная)
+            self.ax.plot(self.current_x, self.current_y, 'ko', markersize=6, label='Текущая позиция')
+            
+            # Рисуем целевую точку (0, 0) - желтая
+            self.ax.plot(0, 0, 'yo', markersize=8, label='Цель (0, 0)')
+            
+            self.ax.legend()
+        
+        # Автоматически масштабируем оси
+        if len(self.trajectory_x) > 0:
+            margin = 50  # Отступ от краев
+            x_min, x_max = min(self.trajectory_x), max(self.trajectory_x)
+            y_min, y_max = min(self.trajectory_y), max(self.trajectory_y)
+            
+            # Учитываем целевую точку (0, 0)
+            x_min = min(x_min, 0)
+            x_max = max(x_max, 0)
+            y_min = min(y_min, 0)
+            y_max = max(y_max, 0)
+            
+            self.ax.set_xlim(x_min - margin, x_max + margin)
+            self.ax.set_ylim(y_min - margin, y_max + margin)
+        
+        plt.draw()
+        plt.pause(0.25)  # Небольшая пауза для обновления окна
+
     def loop(self):
 
         html = self.driver.find_element(By.TAG_NAME, 'html')
         action = ActionChains(self.driver)
         velocity = 10
 
-        for i in range(100):
+        # Добавляем начальную точку в траекторию
+        self.update_trajectory(0, 0)
+        sleep(2)
+
+        for i in range(1000):
+            dangle = None
+            if self.mode == SimMode.OPERATOR:
+                dangle = self.operatorPilot.act()
+                if dangle is None:
+                    self.mode = SimMode.AUTONOME
+                    print("Режим возвращения домой!")
+
+            if self.mode == SimMode.AUTONOME:
+                dangle = self.autonomePilot.act()
+
+            if dangle is None:
+                break
+
+
             # Сдвиг камеры
             action = ActionChains(self.driver)
             action.move_to_element_with_offset(html, 200, 200)
             action.click_and_hold()
-            dangle = self.operatorPilot.act()
+
+            print(f"  [Simulator] Drone Position: ({self.current_x:.2f}, {self.current_y:.2f})")
+            print(f"  [Simulator] Angle: {self.angle * 180 / math.pi:.1f}°")
+
             self.angle += dangle
             dx = math.cos(self.angle) * velocity
             dy = math.sin(self.angle) * velocity
@@ -92,6 +216,9 @@ class Simulator:
             action.release()
             action.perform()
 
+            # Обновляем траекторию
+            self.update_trajectory(dx, dy)
+
             # Загрузка скриншота
             png = self.driver.get_screenshot_as_png()
             im = Image.open(BytesIO(png))
@@ -104,6 +231,18 @@ class Simulator:
             self.autonomePilot.handle(rotated_im)
             
             rotated_im.save(f"./images/{i}.png")
-            sleep(0.25)
+            
+            # Обновляем карту каждые несколько итераций для производительности
+            if i % 5 == 0:
+                self.update_map()
+            
+            
 
+        # Финальное обновление карты
+        self.update_map()
         print("last position: ", self.driver.current_url)
+        print(f"Финальная позиция: ({self.current_x:.2f}, {self.current_y:.2f})")
+        
+        # Показываем карту до закрытия
+        plt.ioff()
+        plt.show()