feat: drawing points

main.py

@@ -0,0 +1,30 @@
+from autopilot import AutoPilot, RandomPilot
+from simulator import Simulator
+from visualization import VisualizationManager
+from trajectory_drawer import TrajectoryDrawer
+from yandex_map import YandexMap
+
+# Создаем менеджер визуализации
+# viz_manager = VisualizationManager("Drone Autopilot - Global Map & Detection")
+
+# Создаем симулятор с AutoPilot для обработки изображений
+# Передаем менеджер визуализации в автопилот
+# simulator = Simulator(RandomPilot(), AutoPilot(viz_manager=viz_manager), viz_manager=viz_manager)
+
+yandexMap = YandexMap()
+yandexMap.savePhoto('map.jpg')
+# yandexMap.destroy()
+
+trajectoryDrawer = TrajectoryDrawer('map.jpg')
+trajectoryDrawer.on_complete_trajectory = lambda x: print(x)
+trajectoryDrawer.show()
+
+# Использование
+# if __name__ == "__main__":
+#     # Укажите путь к вашему изображению
+#     image_path = "map.jpg"  # Замените на путь к вашему изображению
+    
+#     drawer = TrajectoryDrawer(image_path)
+#     drawer.show()
+# Запускаем симуляцию
+# simulator.loop()

old/simulator.py

@@ -59,7 +59,9 @@ class Simulator:
         action.perform()
 
         # Режим спутника
-        action.click(self.driver.find_element(By.CLASS_NAME, '_key_satellite'))
+        sleep(1)
+        action.move_to_element_with_offset(self.driver.find_element(By.XPATH, "//div[@class='rounded-controls']/div[@class='rounded-controls__child'][5]//button"), -500, -500)
+        action.click()
         action.perform()
 
         self.angle = 0

simulator.py

@@ -0,0 +1,173 @@
+import math
+from io import BytesIO
+from time import sleep
+
+from PIL import Image
+from selenium import webdriver
+from selenium.webdriver.common.by import By
+from selenium.webdriver.common.action_chains import ActionChains
+
+from autopilot import Pilot
+from visualization import VisualizationManager, SimMode
+
+import os
+
+class Simulator:
+    driver: webdriver.Chrome
+    mode: SimMode
+
+    operatorPilot: Pilot
+    autonomePilot: Pilot
+
+    angle: float
+    
+    # Менеджер визуализации
+    viz_manager: VisualizationManager
+    current_x: float
+    current_y: float
+
+    def __init__(self, operatorPilot: Pilot, autonomePilot: Pilot, viz_manager: VisualizationManager = None):
+        self.mode = SimMode.OPERATOR
+        self.operatorPilot = operatorPilot
+        self.autonomePilot = autonomePilot
+
+        # Инициализация переменных для траектории
+        self.current_x = 0.0
+        self.current_y = 0.0
+        
+        # Создаем менеджер визуализации
+        self.viz_manager = viz_manager
+        
+        # Передаем менеджер визуализации в автопилот, если он поддерживает это
+        if hasattr(self.autonomePilot, 'viz_manager'):
+            self.autonomePilot.viz_manager = self.viz_manager
+        
+        # Создаем папку для изображений, если её нет
+        os.makedirs('./images', exist_ok=True)
+
+        options = webdriver.ChromeOptions()
+        # 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)
+
+        action = ActionChains(self.driver)
+
+        # Закрытие левой панели
+        action.click(self.driver.find_element(By.CLASS_NAME, 'sidebar-toggle-button'))
+        action.move_to_element_with_offset(self.driver.find_element(By.XPATH, "//div[@class='rounded-controls']/div[@class='rounded-controls__child'][5]//button"), 5, 5)
+        action.perform()
+
+        # Режим спутника
+        action.click(self.driver.find_element(By.CLASS_NAME, '_key_satellite'))
+        action.perform()
+        
+        self.driver.get_screenshot_as_png()
+
+        self.angle = 0
+
+    def rotate_image_like_drone(self, image: Image.Image, angle: float) -> Image.Image:
+        """
+        Поворачивает картинку как будто съемка ведется с летящего дрона.
+        Выделяет концентрический квадрат, поворачивает его и извлекает результат.
+        """
+        # Получаем размеры изображения
+        width, height = image.size
+        square_size = min(width, height)
+        cropped_image = image.crop((0, 0, square_size, square_size))
+        cropped_image = cropped_image.rotate(angle / math.pi * 180, expand=True)
+        
+        # Определяем размер концентрического квадрата (80% от минимальной стороны)
+        local_square_size = int(square_size / 2 ** 0.5)
+        
+        # Вычисляем координаты для центрирования квадрата
+        left = (cropped_image.width - local_square_size) // 2
+        top = (cropped_image.height - local_square_size) // 2
+        right = left + local_square_size
+        bottom = top + local_square_size
+        
+        # Вырезаем концентрический квадрат
+        final_image = cropped_image.crop((left, top, right, bottom))
+
+        return final_image
+
+    def update_trajectory(self, dx: float, dy: float):
+        """
+        Обновляет траекторию полета беспилотника
+        """
+        # Обновляем текущие координаты
+        self.current_x += dx
+        self.current_y += dy
+
+    def update_map(self):
+        """
+        Обновляет карту траектории полета
+        """
+        self.viz_manager.update_global_map(self.current_x, self.current_y, self.mode)
+
+    def loop(self):
+
+        html = self.driver.find_element(By.TAG_NAME, 'html')
+        action = ActionChains(self.driver)
+
+        # Добавляем начальную точку в траекторию
+        self.update_trajectory(0, 0)
+        self.viz_manager.update_global_map(self.current_x, self.current_y, self.mode)
+
+        for i in range(1000):
+            signal = None
+            if self.mode == SimMode.OPERATOR:
+                signal = self.operatorPilot.act()
+                if signal is None:
+                    self.mode = SimMode.AUTONOME
+                    print("Режим возвращения домой!")
+
+            if self.mode == SimMode.AUTONOME:
+                signal = self.autonomePilot.act()
+
+            if signal is None:
+                break
+
+            dangle, velocity = signal
+            drone_x, drone_y = self.autonomePilot.get_position()
+            self.viz_manager.update_error_plot(i, drone_x, drone_y, self.current_x, self.current_y)
+
+            # Сдвиг камеры
+            action = ActionChains(self.driver)
+            action.move_to_element_with_offset(html, 200, 200)
+            action.click_and_hold()
+
+            self.angle += dangle
+            dx = math.cos(self.angle) * velocity
+            dy = math.sin(self.angle) * velocity
+            action.move_by_offset(-dx, dy)
+            action.release()
+            action.perform()
+
+            # Обновляем траекторию
+            self.update_trajectory(dx, dy)
+
+            # Загрузка скриншота
+            png = self.driver.get_screenshot_as_png()
+            im = Image.open(BytesIO(png))
+            im = im.crop([0, 80, im.width-80, im.height-60])
+            
+            # Применяем поворот как будто съемка с дрона
+            rotated_im = self.rotate_image_like_drone(im, math.pi / 2 - self.angle)
+            
+            # Передаем изображение в AutoPilot для анализа
+            self.autonomePilot.handle(rotated_im)
+            
+            # Обновляем визуализацию каждые несколько итераций для производительности
+            self.update_map()
+            self.viz_manager.update_display()
+
+        # Финальное обновление карты
+        self.update_map()
+        self.viz_manager.update_display()
+        print("last position: ", self.driver.current_url)
+        print(f"Финальная позиция: ({self.current_x:.2f}, {self.current_y:.2f})")
+        
+        # Показываем карту до закрытия, но не поднимаем на передний план
+        self.viz_manager.show_final()
+        print("Симуляция завершена. Окно визуализации остается открытым для анализа.")

trajectory_drawer.py

@@ -0,0 +1,83 @@
+import matplotlib.pyplot as plt
+import matplotlib.image as mpimg
+from matplotlib.path import Path
+import matplotlib.patches as patches
+import numpy as np
+
+class TrajectoryDrawer:
+    def __init__(self, image_path):
+        self.image_path = image_path
+        self.points = []
+        self.fig, self.ax = plt.subplots(figsize=(12, 8))
+        
+        # Загрузка фонового изображения
+        self.img = mpimg.imread(image_path)
+        self.ax.imshow(self.img)
+        
+        # Настройка графика
+        self.ax.set_title('Нарисуйте траекторию движения (кликните для точек, Enter для завершения)')
+        self.ax.set_xlabel('X координата')
+        self.ax.set_ylabel('Y координата')
+        
+        # Подключение обработчиков событий
+        self.fig.canvas.mpl_connect('button_press_event', self.on_click)
+        self.fig.canvas.mpl_connect('key_press_event', self.on_key)
+        
+        self.line, = self.ax.plot([], [], 'ro-', linewidth=2, markersize=6)
+        
+    def on_click(self, event):
+        if event.inaxes == self.ax:
+            # Добавляем точку только если клик в области графика
+            x, y = event.xdata, event.ydata
+            self.points.append((x, y))
+            self.update_plot()
+            
+    def on_key(self, event):
+        if event.key == 'enter':
+            self.complete_trajectory()
+        elif event.key == 'escape':
+            self.clear_trajectory()
+            
+    def update_plot(self):
+        if self.points:
+            x_coords = [p[0] for p in self.points]
+            y_coords = [p[1] for p in self.points]
+            self.line.set_data(x_coords, y_coords)
+            self.fig.canvas.draw()
+            
+    def complete_trajectory(self):
+        if len(self.points) > 1:
+            print("Траектория завершена!")
+            print(f"Количество точек: {len(self.points)}")
+            print("Координаты точек:")
+            for i, (x, y) in enumerate(self.points):
+                print(f"Точка {i+1}: ({x:.2f}, {y:.2f})")
+                
+            # Можно сохранить координаты в файл
+            # self.save_trajectory()
+            self.on_complete_trajectory("GOOD")
+            
+    def clear_trajectory(self):
+        self.points = []
+        self.line.set_data([], [])
+        self.fig.canvas.draw()
+        print("Траектория очищена")
+        
+    def save_trajectory(self):
+        # Сохранение координат в файл
+        with open('trajectory_coordinates.txt', 'w') as f:
+            for i, (x, y) in enumerate(self.points):
+                f.write(f"{i+1},{x:.4f},{y:.4f}\n")
+        print("Координаты сохранены в trajectory_coordinates.txt")
+        
+    def show(self):
+        plt.tight_layout()
+        plt.show()
+
+# Использование
+if __name__ == "__main__":
+    # Укажите путь к вашему изображению
+    image_path = "map.jpg"  # Замените на путь к вашему изображению
+    
+    drawer = TrajectoryDrawer(image_path)
+    drawer.show()

visualization.py

@@ -0,0 +1,277 @@
+#!/usr/bin/env python3
+"""
+Модуль для управления общим окном визуализации
+"""
+
+import matplotlib.pyplot as plt
+import matplotlib.patches as patches
+import numpy as np
+from enum import Enum
+import cv2
+from PIL import Image
+import matplotlib
+
+# Настройки matplotlib
+matplotlib.use('TkAgg')
+plt.rcParams['figure.raise_window'] = False
+
+class SimMode(Enum):
+    OPERATOR = 1
+    AUTONOME = 2
+
+class VisualizationManager:
+    """
+    Менеджер для управления общим окном визуализации
+    """
+    
+    def __init__(self, window_title="Drone Autopilot Visualization"):
+        self.window_title = window_title
+        self.fig = None
+        self.ax_error_plot = None  # График погрешности позиции
+        self.ax_global_map = None
+        self.ax_detection = None
+        self.ax_matches = None
+        
+        # Данные для глобальной карты
+        self.trajectory_x = []
+        self.trajectory_y = []
+        self.trajectory_modes = []
+        self.current_x = 0.0
+        self.current_y = 0.0
+        
+        # Данные для траектории БПЛА (его собственное видение)
+        self.drone_trajectory_x = []
+        self.drone_trajectory_y = []
+        
+        # Данные для графика погрешности
+        self.error_times = []
+        self.position_errors = []
+        
+        # Данные для детекции
+        self.current_frame = None
+        self.keypoints = []
+        self.matches = []
+        
+        self._setup_window()
+    
+    def _setup_window(self):
+        """Настраивает общее окно с несколькими областями"""
+        plt.ion()
+        self.fig = plt.figure(figsize=(16, 10))
+        self.fig.canvas.manager.window.title(self.window_title)
+        
+        # Открываем окно на полный экран
+        self.fig.canvas.manager.window.state('zoomed')
+        
+        # Создаем сетку 2x2 с разными размерами колонок
+        gs = self.fig.add_gridspec(2, 2, hspace=0.3, wspace=0.3, width_ratios=[1, 1])
+        
+        # График погрешности позиции (левый верхний угол)
+        self.ax_error_plot = self.fig.add_subplot(gs[0, 0])
+        self.ax_error_plot.set_title('Погрешность позиции от времени')
+        self.ax_error_plot.set_xlabel('Время (кадры)')
+        self.ax_error_plot.set_ylabel('Погрешность (метры)')
+        self.ax_error_plot.grid(True, alpha=0.3)
+        
+        # Глобальная карта (левый нижний угол)
+        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 координата')
+        self.ax_global_map.grid(True, alpha=0.3)
+        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[0, 1])
+        self.ax_detection.set_title('Keypoint Detection')
+        self.ax_detection.axis('off')
+        
+        # Сопоставление точек (правый нижний угол)
+        self.ax_matches = self.fig.add_subplot(gs[1, 1])
+        self.ax_matches.set_title('Feature Matching')
+        self.ax_matches.axis('off')
+        
+        # Настройки окна
+        self.fig.canvas.manager.window.attributes('-topmost', False)
+        
+        plt.tight_layout()
+    
+    def update_global_map(self, x: float, y: float, mode: SimMode):
+        """Обновляет глобальную карту"""
+        self.current_x = x
+        self.current_y = y
+        self.trajectory_x.append(x)
+        self.trajectory_y.append(y)
+        self.trajectory_modes.append(mode)
+        
+        self.ax_global_map.clear()
+        self.ax_global_map.set_title('Global Map - Траектория полета беспилотника')
+        self.ax_global_map.set_xlabel('X координата')
+        self.ax_global_map.set_ylabel('Y координата')
+        self.ax_global_map.grid(True, alpha=0.3)
+        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)
+        
+        if len(self.trajectory_x) > 1:
+            # Разделяем траекторию по режимам
+            operator_indices = [i for i, m in enumerate(self.trajectory_modes) if m == SimMode.OPERATOR]
+            autonome_indices = operator_indices[-1:] + [i for i, m in enumerate(self.trajectory_modes) if m == 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_global_map.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_global_map.plot(autonome_x, autonome_y, 'r-', linewidth=2, label='Автономный режим')
+            
+            # Рисуем траекторию БПЛА (пунктирная линия, тонкая)
+            if len(self.drone_trajectory_x) > 1:
+                self.ax_global_map.plot(self.drone_trajectory_x, self.drone_trajectory_y, 
+                                      'g--', linewidth=1, alpha=0.7, label='Данные по одометрии')
+            
+            # Рисуем начальную точку (зеленая)
+            self.ax_global_map.plot(self.trajectory_x[0], self.trajectory_y[0], 'go', markersize=8, label='Начальная точка')
+            
+            # Рисуем текущую позицию (черная)
+            self.ax_global_map.plot(self.current_x, self.current_y, 'ko', markersize=6, label='Текущая позиция')
+            
+            # Рисуем целевую точку (0, 0) - желтая
+            self.ax_global_map.plot(0, 0, 'yo', markersize=8, label='Цель (0, 0)')
+            
+            self.ax_global_map.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)
+            
+            # Учитываем также траекторию БПЛА при масштабировании
+            if len(self.drone_trajectory_x) > 0:
+                x_min = min(x_min, min(self.drone_trajectory_x))
+                x_max = max(x_max, max(self.drone_trajectory_x))
+                y_min = min(y_min, min(self.drone_trajectory_y))
+                y_max = max(y_max, max(self.drone_trajectory_y))
+            
+            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_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_trajectory(self, drone_x: float, drone_y: float):
+        """Обновляет траекторию БПЛА (его собственное видение позиции)"""
+        self.drone_trajectory_x.append(drone_x)
+        self.drone_trajectory_y.append(drone_y)
+    
+    def update_error_plot(self, frame_count: int, drone_x: float, drone_y: float, true_x: float, true_y: float):
+        """Обновляет график погрешности позиции"""
+        # Вычисляем погрешность как расстояние между реальной и предполагаемой позицией
+        error = np.sqrt((drone_x - true_x)**2 + (drone_y - true_y)**2)
+        
+        self.error_times.append(frame_count)
+        self.position_errors.append(error)
+        
+        self.ax_error_plot.clear()
+        self.ax_error_plot.set_title('Погрешность позиции от времени')
+        self.ax_error_plot.set_xlabel('Время (кадры)')
+        self.ax_error_plot.set_ylabel('Погрешность (метры)')
+        self.ax_error_plot.grid(True, alpha=0.3)
+        
+        if len(self.error_times) > 1:
+            self.ax_error_plot.plot(self.error_times, self.position_errors, 'b-', linewidth=2)
+            
+            # Автоматически масштабируем оси
+            if len(self.position_errors) > 0:
+                margin = 0.1
+                error_min, error_max = min(self.position_errors), max(self.position_errors)
+                if error_max > error_min:
+                    self.ax_error_plot.set_ylim(0, error_max + margin)
+                else:
+                    self.ax_error_plot.set_ylim(0, 1)
+    
+    def update_detection(self, image: np.ndarray, keypoints):
+        """Обновляет визуализацию детекции ключевых точек"""
+        self.current_frame = image.copy()
+        self.keypoints = keypoints
+        
+        self.ax_detection.clear()
+        self.ax_detection.set_title('Keypoint Detection')
+        
+        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_detection.imshow(image_rgb)
+            
+            # Рисуем ключевые точки
+            if keypoints:
+                kp_coords = np.array([kp.pt for kp in keypoints])
+                self.ax_detection.scatter(kp_coords[:, 0], kp_coords[:, 1], 
+                                        c='red', s=20, alpha=0.7, marker='o')
+            
+            self.ax_detection.axis('off')
+    
+    def update_matches(self, img1: np.ndarray, img2: np.ndarray, 
+                      kp1, kp2, matches, transformation_info=None):
+        """Обновляет визуализацию сопоставления точек"""
+        self.ax_matches.clear()
+        self.ax_matches.set_title('Feature Matching')
+        
+        if img1 is not None and img2 is not None and matches:
+            # Рисуем сопоставления
+            img_matches = cv2.drawMatches(img1, kp1, img2, kp2, matches, None,
+                                         flags=cv2.DrawMatchesFlags_NOT_DRAW_SINGLE_POINTS)
+            
+            # Конвертируем BGR в RGB
+            if len(img_matches.shape) == 3 and img_matches.shape[2] == 3:
+                img_matches_rgb = cv2.cvtColor(img_matches, cv2.COLOR_BGR2RGB)
+            else:
+                img_matches_rgb = img_matches
+            
+            self.ax_matches.imshow(img_matches_rgb)
+            
+            # Добавляем информацию о трансформации
+            if transformation_info:
+                tx, ty = transformation_info['translation']
+                angle = transformation_info['rotation']
+                
+                info_text = f"Translation: ({tx:.2f}, {ty:.2f})"
+                info_text2 = f"Rotation: {angle:.2f} rad ({np.degrees(angle):.1f}°)"
+                
+                self.ax_matches.text(10, 30, info_text, fontsize=8, color='green', 
+                                   bbox=dict(boxstyle="round,pad=0.3", facecolor="white", alpha=0.8))
+                self.ax_matches.text(10, 90, info_text2, fontsize=8, color='green',
+                                   bbox=dict(boxstyle="round,pad=0.3", facecolor="white", alpha=0.8))
+        
+        self.ax_matches.axis('off')
+    
+    def update_display(self):
+        """Обновляет отображение всех областей"""
+        self.fig.canvas.draw()
+        self.fig.canvas.flush_events()
+        plt.pause(0.01)
+    
+    def close(self):
+        """Закрывает окно"""
+        plt.close(self.fig)
+    
+    def show_final(self):
+        """Показывает финальное состояние окна"""
+        plt.ioff()
+        print("Симуляция завершена. Окно визуализации остается открытым для анализа.")
+        plt.pause(100000)
+
+    def pause(self, duration: float):
+        plt.pause(duration) 

yandex_map.py

@@ -0,0 +1,40 @@
+import math
+from io import BytesIO
+from time import sleep
+import os
+
+from PIL import Image
+from selenium import webdriver
+from selenium.webdriver.common.by import By
+from selenium.webdriver.common.action_chains import ActionChains
+
+class YandexMap:
+    def __init__(self):
+        options = webdriver.ChromeOptions()
+        # 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=10")
+        sleep(5)
+
+        action = ActionChains(self.driver)
+
+        # Закрытие левой панели
+        action.click(self.driver.find_element(By.CLASS_NAME, 'sidebar-toggle-button'))
+        action.move_to_element_with_offset(self.driver.find_element(By.XPATH, "//div[@class='rounded-controls']/div[@class='rounded-controls__child'][5]//button"), 5, 5)
+        action.perform()
+
+        # Режим спутника
+        action.click(self.driver.find_element(By.CLASS_NAME, '_key_satellite'))
+        action.perform()
+
+        html = self.driver.find_element(By.TAG_NAME, 'html')
+        action.move_to_element_with_offset(html, 200, 200)
+        action.perform()
+
+        sleep(5)
+
+    def savePhoto(self, filename: str) -> bytes:
+        return self.driver.save_screenshot(filename)
+    
+    def destroy(self):
+        self.driver.close()