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

feat: add zoom

Browse Source
This commit is contained in:
russian_proger
2025-12-19 23:26:25 +03:00
parent d04d4a0cdc
commit 48885d567a
3 changed files with 42 additions and 14 deletions
Download Patch File Download Diff File Expand all files Collapse all files

29
autopilot.py
View File

@@ -42,6 +42,7 @@ class AutoPilot(Pilot):
angle: float
x: float # Координата X БПЛА
y: float # Координата Y БПЛА
zoom: float # Коэффициент увеличения
# Ориентиры
points: list[tuple[float,float]] # Координаты
@@ -65,6 +66,7 @@ class AutoPilot(Pilot):
self.angle = 0.0
self.x = 0.0
self.y = 0.0
self.zoom = 1.0
self.frame_count = 0
self.image_center = (0, 0) # Будет обновлено при получении первого изображения
self.vis_manager = viz_manager # Менеджер визуализации
@@ -238,7 +240,8 @@ class AutoPilot(Pilot):
mat: np.ndarray,
x_source: float,
y_source: float,
angle_source: float
angle_source: float,
zoom_source: float
) -> tuple[float, float, float] | None:
""" Возвращает новые координаты и поворот на основе матрицы преобразования """
@@ -247,6 +250,8 @@ class AutoPilot(Pilot):
# Вычисляем угол поворота
rotation = -np.arctan2(mat[1, 0], mat[0, 0])
scale = np.hypot(mat[0, 0], mat[0, 1])
print("Scale: ", scale)
# print("[HOMOGRAPHY]", mat)
# print("[ROTATION]", rotation)
@@ -266,13 +271,16 @@ class AutoPilot(Pilot):
dy_global = dx_meters * sin_angle + dy_meters * cos_angle
# Обновляем координаты БПЛА
x = x_source + dx_global
y = y_source + dy_global
print("[Matrix Transformation]: ")
print(mat)
zoom = zoom_source / scale
x = x_source + dx_global * zoom
y = y_source + dy_global * zoom
# Нормализуем угол в диапазоне [-π, π]
angle = math.atan2(math.sin(angle_global), math.cos(angle_global))
return x, y, angle
return x, y, angle, zoom
def update_drone_position(self, transformation_info: dict):
"""
@@ -281,9 +289,9 @@ class AutoPilot(Pilot):
"""
homography = transformation_info['homography']
self.x, self.y, self.angle = self.calc_position(
self.x, self.y, self.angle, self.zoom = self.calc_position(
homography,
self.x, self.y, self.angle
self.x, self.y, self.angle, self.zoom
)
if self.reserved_pos is not None:
@@ -305,7 +313,7 @@ class AutoPilot(Pilot):
def get_position_by_chunk(self) -> tuple[float, float, float] | None:
def get_position_by_chunk(self) -> tuple[float, float, float, float] | None:
# Пытаемся найти ориентир на картинке:
current_image = self.prev_image
landmark_image = cv2.imread(Path('chunks') / f'chunk_{self.target_idx}.png', cv2.IMREAD_COLOR_RGB)
@@ -338,10 +346,11 @@ class AutoPilot(Pilot):
# Считаем абсолютную позу относительно координат ориентира
landmark_world_x, landmark_world_y = self.points[self.target_idx]
landmark_angle = 0
landmark_zoom = 1
homography = landmark_transform['homography']
# homography = np.linalg.inv(homography)
# print(f" [Pilot] Landmark correction applied (inliers={landmark_transform['inliers']}, ratio={ratio:.2f}, rmse={rmse:.2f})")
return self.calc_position(homography, landmark_world_x, landmark_world_y, landmark_angle)
print(f" [Pilot] Landmark correction applied (inliers={landmark_transform['inliers']}, ratio={ratio:.2f}, rmse={rmse:.2f})")
return self.calc_position(homography, landmark_world_x, landmark_world_y, landmark_angle, landmark_zoom)
return None
@@ -437,7 +446,7 @@ class AutoPilot(Pilot):
)
if self.reserved_pos is not None:
self.x, self.y, self.angle = self.reserved_pos
self.x, self.y, self.angle, self.zoom = self.reserved_pos
self.reserved_pos = None
# Вычисляем угол к цели

9
main.py
View File

@@ -4,6 +4,7 @@ from trajectory_drawer import TrajectoryDrawer
from yandex_map import YandexMap
from time import sleep
from pathlib import Path
import random
import cv2
import matplotlib.pyplot as plt
@@ -106,8 +107,16 @@ def main():
proc_time = np.array([])
zoom_next_event = random.randint(5, 10)
for i in range(10000000000):
print(f"Image #{i}")
if i == zoom_next_event:
r = random.randint(0, 1)
direction = ['up', 'down'][r]
simulator.change_zoom(direction)
zoom_next_event = i + random.randint(20, 40)
photo = simulator.get_photo()
command = pilot.handle(photo)

18
simulator.py
View File

@@ -31,6 +31,7 @@ class Simulator:
self.angle = 0.0
self.current_x = 0.0
self.current_y = 0.0
self.zoom = 1
# Создаем папку для изображений, если её нет
os.makedirs('./images', exist_ok=True)
@@ -68,8 +69,8 @@ class Simulator:
Обновляет траекторию полета беспилотника
"""
# Обновляем текущие координаты
self.current_x += dx
self.current_y += dy
self.current_x += dx * self.zoom
self.current_y += dy * self.zoom
def update_map(self):
"""
@@ -88,14 +89,23 @@ class Simulator:
self.angle += dangle
# print(f" [Simulator] angle: {self.angle / math.pi * 180:.1f}°")
velocity = max(velocity, 10)
dx = math.cos(math.pi / 2 + self.angle) * velocity
dy = math.sin(math.pi / 2 + self.angle) * velocity
dx = math.cos(math.pi / 2 + self.angle) * velocity / self.zoom
dy = math.sin(math.pi / 2 + self.angle) * velocity / self.zoom
# print(" [Simulator] dx, dy:", [dx, dy])
self.update_trajectory(dx, dy)
action.move_by_offset(-dx, dy)
action.release()
action.perform()
# print(f" [Simulator] Position: {self.current_x}, {self.current_y}")
def change_zoom(self, direction: str = 'down'):
""" Изменение масштаба """
self.yandexMap.scroll(0.5, 0.5, 2, direction == 'down')
sleep(0.4)
if direction == 'down':
self.zoom /= 2
else:
self.zoom *= 2
def get_photo(self) -> Image.Image:
png = self.yandexMap.driver.get_screenshot_as_png()