100 lines
3.2 KiB
Python
100 lines
3.2 KiB
Python
from autopilot import AutoPilot, RandomPilot
|
||
from simulator import Simulator
|
||
from visualization import VisualizationManager
|
||
from trajectory_drawer import TrajectoryDrawer
|
||
from yandex_map import YandexMap
|
||
from time import sleep
|
||
from pathlib import Path
|
||
|
||
import cv2
|
||
import matplotlib.pyplot as plt
|
||
import numpy as np
|
||
|
||
import autopilot
|
||
|
||
def make_global_photo(filename):
|
||
yandexMap = YandexMap()
|
||
yandexMap.save_photo()
|
||
yandexMap.destroy()
|
||
|
||
def get_trajectory_points(bg_img: str) -> list[(float, float)]:
|
||
trajectoryDrawer = TrajectoryDrawer(bg_img)
|
||
trajectoryDrawer.show()
|
||
trajectoryDrawer.wait()
|
||
points = list(map(lambda p: [p[0] / trajectoryDrawer.img.shape[1], p[1] / trajectoryDrawer.img.shape[0]], trajectoryDrawer.points))
|
||
return points
|
||
|
||
def main():
|
||
# Скриншот местности
|
||
# make_global_photo('map.jpg')
|
||
|
||
# Получаем траекторию от пользователя
|
||
# points = get_trajectory_points('map.jpg')
|
||
# print(points)
|
||
points = [np.float64(0.5443937502226799), np.float64(0.4030424838774785)], [np.float64(0.18517133490120316), np.float64(0.4586935604608052)], [np.float64(0.1641887171838272), np.float64(0.5586383510594329)], [np.float64(0.587198290366127),
|
||
np.float64(0.5699957136274587)]
|
||
|
||
# Для каждой точки сделаем приближенный снимок
|
||
yandexMap = YandexMap()
|
||
keypoints: list[(any, any)] = []
|
||
|
||
plt.ion()
|
||
# for i in range(len(points)):
|
||
# point = points[i]
|
||
# yandexMap.scroll(point[0], point[1], 5, True)
|
||
# sleep(1)
|
||
# img = yandexMap.make_screenshot(point[0], point[1], 0.2, 0.2)
|
||
# Path('chunks').mkdir(exist_ok=True)
|
||
# cv2.imwrite(Path('.') / 'chunks' / f'chunk_{i}.png', img)
|
||
# plt.subplot(1, len(points), i+1)
|
||
# plt.imshow(img)
|
||
# plt.pause(0.25)
|
||
# yandexMap.scroll(point[0], point[1], 5, False)
|
||
|
||
plt.tight_layout()
|
||
|
||
# Выделим на каждой картинке ключевые точки
|
||
for i in range(len(points)):
|
||
orb = cv2.ORB_create(
|
||
nfeatures=1000,
|
||
scaleFactor=1.2,
|
||
nlevels=8,
|
||
edgeThreshold=31,
|
||
firstLevel=0,
|
||
WTA_K=2,
|
||
patchSize=31,
|
||
fastThreshold=20
|
||
)
|
||
img = cv2.imread(Path('chunks') / f'chunk_{i}.png')
|
||
kp, des = orb.detectAndCompute(img, None)
|
||
keypoints.append((kp, des))
|
||
|
||
plt.subplot(1, len(points), i+1)
|
||
kp_coords = np.array([j.pt for j in kp])
|
||
plt.scatter(kp_coords[:, 0], kp_coords[:, 1], c='red', s=20, alpha=0.7, marker='o')
|
||
plt.pause(0.2)
|
||
plt.ioff()
|
||
|
||
plt.show(block=True)
|
||
|
||
# Начнём симуляцию полёта с первой точки
|
||
yandexMap.scroll(points[0][0], points[0][1], 5, True)
|
||
|
||
vis_manager = VisualizationManager()
|
||
pilot = autopilot.AutoPilot(points, keypoints, vis_manager)
|
||
simulator = Simulator(yandexMap)
|
||
|
||
while True:
|
||
photo = simulator.get_photo()
|
||
command = pilot.handle(photo)
|
||
|
||
if command.stop:
|
||
break
|
||
|
||
simulator.handle(command.dangle)
|
||
vis_manager.update_display()
|
||
|
||
sleep(30)
|
||
|
||
if __name__ == "__main__":
|
||
main() |