feat: main loop

main.py

@@ -3,25 +3,98 @@ 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
 
-def main():
-    global trajectoryDrawer
+import cv2
+import matplotlib.pyplot as plt
+import numpy as np
 
+import autopilot
+
+def make_global_photo(filename):
     yandexMap = YandexMap()
-    yandexMap.savePhoto('map.jpg')
+    yandexMap.save_photo()
     yandexMap.destroy()
 
-
-    trajectoryDrawer = TrajectoryDrawer('map.jpg')
-    trajectoryDrawer.on_complete_trajectory = onCompleteTrajectory
+def get_trajectory_points(bg_img: str) -> list[(float, float)]:
+    trajectoryDrawer = TrajectoryDrawer(bg_img)
     trajectoryDrawer.show()
     trajectoryDrawer.wait()
-    onCompleteTrajectory(trajectoryDrawer.points)
+    points = list(map(lambda p: [p[0] / trajectoryDrawer.img.shape[1], p[1] / trajectoryDrawer.img.shape[0]], trajectoryDrawer.points))
+    return points
 
-def onCompleteTrajectory(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()