From 8b6bb3e22c4c1277d7d2ef3ab81e8ae53e20b676 Mon Sep 17 00:00:00 2001 From: russian_proger Date: Mon, 30 Jun 2025 01:03:23 +0300 Subject: [PATCH] feat: build map and realize act method --- autopilot.py | 54 ++++++++++++++----- simulator.py | 147 +++++++++++++++++++++++++++++++++++++++++++++++++-- 2 files changed, 185 insertions(+), 16 deletions(-) diff --git a/autopilot.py b/autopilot.py index 85a4d46..8992e6e 100644 --- a/autopilot.py +++ b/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() diff --git a/simulator.py b/simulator.py index 63e4c8c..c251fd1 100644 --- a/simulator.py +++ b/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()