zenloger/autopilot
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

feat: build map and realize act method

Browse Source
This commit is contained in:
russian_proger
2025-06-30 01:03:23 +03:00
parent 09e9fc884d
commit 8b6bb3e22c
2 changed files with 185 additions and 16 deletions
Download Patch File Download Diff File Expand all files Collapse all files

147
simulator.py
View File

@@ -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()