From 040299ca4c950061ed96c0867a88428030372dc1 Mon Sep 17 00:00:00 2001 From: russian_proger Date: Tue, 16 Sep 2025 21:29:26 +0300 Subject: [PATCH] feat: drawing points --- a.ipynb | 0 old/autopilot.py => autopilot.py | 0 main.py | 30 ++++ old/simulator.py | 4 +- simulator.py | 173 +++++++++++++++++++ trajectory_drawer.py | 83 +++++++++ visualization.py | 277 +++++++++++++++++++++++++++++++ yandex_map.py | 40 +++++ 8 files changed, 606 insertions(+), 1 deletion(-) create mode 100644 a.ipynb rename old/autopilot.py => autopilot.py (100%) create mode 100644 main.py create mode 100644 simulator.py create mode 100644 trajectory_drawer.py create mode 100644 visualization.py create mode 100644 yandex_map.py diff --git a/a.ipynb b/a.ipynb new file mode 100644 index 0000000..e69de29 diff --git a/old/autopilot.py b/autopilot.py similarity index 100% rename from old/autopilot.py rename to autopilot.py diff --git a/main.py b/main.py new file mode 100644 index 0000000..2b774c3 --- /dev/null +++ b/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() diff --git a/old/simulator.py b/old/simulator.py index 71aab45..d37c8f5 100644 --- a/old/simulator.py +++ b/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 diff --git a/simulator.py b/simulator.py new file mode 100644 index 0000000..dcea4ce --- /dev/null +++ b/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("Симуляция завершена. Окно визуализации остается открытым для анализа.") diff --git a/trajectory_drawer.py b/trajectory_drawer.py new file mode 100644 index 0000000..e648211 --- /dev/null +++ b/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() \ No newline at end of file diff --git a/visualization.py b/visualization.py new file mode 100644 index 0000000..de882b0 --- /dev/null +++ b/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) diff --git a/yandex_map.py b/yandex_map.py new file mode 100644 index 0000000..fd901cc --- /dev/null +++ b/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()