261 lines
7.8 KiB
Python
261 lines
7.8 KiB
Python
from google_map import GoogleMap
|
||
from pathlib import Path
|
||
from position import Position
|
||
from simulator import Simulator
|
||
from time import sleep
|
||
from trajectory_drawer import TrajectoryDrawer
|
||
from utility import cv2_to_pil
|
||
from vision_chunk import VisionChunk
|
||
from visualization import VisualizationManager
|
||
from yandex_map import YandexMap
|
||
|
||
import argparse
|
||
import autopilot
|
||
import constants
|
||
import cv2
|
||
import matplotlib.pyplot as plt
|
||
import numpy as np
|
||
import pickle
|
||
import random
|
||
import utility
|
||
|
||
def make_global_photo(filename, initial_zoom=13):
|
||
google_map = GoogleMap(initial_zoom=initial_zoom)
|
||
google_map.save_photo(filename)
|
||
google_map.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 build(name: str):
|
||
|
||
# Создание папки с информацией о маршруте
|
||
dir = Path('trajectories')
|
||
if not dir.exists(): dir.mkdir()
|
||
dir /= name
|
||
assert not dir.exists()
|
||
dir.mkdir()
|
||
dir_chunks = dir / 'chunks'
|
||
dir_chunks.mkdir()
|
||
|
||
# make_global_photo('map.jpg', 15)
|
||
points = get_trajectory_points('map.jpg')
|
||
google_map = GoogleMap(initial_zoom=15)
|
||
|
||
# Начнём симуляцию полёта с первой точки
|
||
google_map.make_as_center(*points[0])
|
||
sleep(3)
|
||
google_map.scroll(0.5, 0.5, 10, True)
|
||
sleep(2)
|
||
geo = google_map.get_geolocation()
|
||
google_map.open(geo['lat'], geo['lon'], 18)
|
||
sleep(20)
|
||
|
||
width, height = google_map.get_size()
|
||
points_coords = np.array(list(map(lambda p: [
|
||
(p[0] - points[0][0]) * width, (points[0][1] - p[1]) * height
|
||
], points)))
|
||
|
||
points_coords *= constants.GOOGLE_PIXEL_RATIO_15
|
||
points_coords_pixel = points_coords / constants.GOOGLE_PIXEL_RATIO_18
|
||
|
||
pilot = autopilot.AutoPilot(points_coords_pixel)
|
||
simulator = Simulator(google_map)
|
||
pilot.target_idx = 0
|
||
|
||
pilot.pos = simulator.pos.copy()
|
||
command = pilot.make_command()
|
||
|
||
positions: list[Position] = []
|
||
|
||
print("points_coords_pixel:", points_coords_pixel)
|
||
for i in range(100000):
|
||
|
||
if command.stop:
|
||
break
|
||
|
||
# simulator.handle(command.dangle, command.velocity)
|
||
chunk = simulator.get_chunk()
|
||
pilot.pos = simulator.pos.copy()
|
||
command = pilot.make_command()
|
||
command.velocity /= constants.GOOGLE_PIXEL_RATIO_18
|
||
|
||
print("Position:", simulator.pos)
|
||
|
||
# Save Image
|
||
chunk.save_image(dir_chunks / f"chunk_{len(positions)}.png")
|
||
positions.append(simulator.pos)
|
||
|
||
simulator.handle(command.dangle, command.velocity)
|
||
sleep(1.5)
|
||
|
||
data = {
|
||
'points': points_coords,
|
||
'chunk_positions': positions,
|
||
'initial_geolocation': geo
|
||
}
|
||
|
||
file_positions = dir / 'positions.pkl'
|
||
with file_positions.open('wb') as file:
|
||
pickle.dump(data, file)
|
||
|
||
with file_positions.open('rb') as file:
|
||
r = pickle.load(file)
|
||
print(r)
|
||
|
||
sleep(15)
|
||
google_map.destroy()
|
||
|
||
def run(name: str):
|
||
dir = Path('trajectories')
|
||
assert dir.exists()
|
||
dir /= name
|
||
assert dir.exists()
|
||
dir_chunks = dir / 'chunks'
|
||
file_positions = dir / 'positions.pkl'
|
||
|
||
with file_positions.open('rb') as file:
|
||
data = pickle.load(file)
|
||
|
||
initial_geolocation = data['initial_geolocation']
|
||
chunks: list[VisionChunk] = []
|
||
for i in range(len(data['chunk_positions'])):
|
||
chunk = VisionChunk.load_image(dir_chunks / f"chunk_{i}.png")
|
||
chunk.pos = data['chunk_positions'][i]
|
||
|
||
points = data['points'] / constants.YANDEX_PIXEL_RATIO_18
|
||
|
||
yandex_map = YandexMap(initial_geolocation['lat'], initial_geolocation['lon'], 18)
|
||
sleep(10)
|
||
|
||
vis_manager = VisualizationManager()
|
||
width, height = yandex_map.get_size()
|
||
pilot = autopilot.AutoPilot(points, chunks, vis_manager)
|
||
simulator = Simulator(yandex_map)
|
||
pilot.target_idx = 0
|
||
|
||
chunk = simulator.get_chunk()
|
||
command = pilot.handle(chunk)
|
||
|
||
vis_manager.update_display()
|
||
vis_manager.pause(1)
|
||
|
||
vis_manager.set_target_points(points)
|
||
|
||
proc_time = np.array([])
|
||
|
||
zoom_next_event = random.randint(5, 10)
|
||
|
||
errors = []
|
||
chunk_errors = []
|
||
chunk_improves = []
|
||
|
||
last_chunk_index = 0
|
||
|
||
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)
|
||
|
||
|
||
# if i > 0:
|
||
# simulator.set_pitch(np.sin(i / 10) * 5)
|
||
# simulator.set_roll(np.sin(i / 15) * 5)
|
||
# simulator.set_zoom(1.0 + np.sin(i / 10) * 0.3)
|
||
|
||
if command.stop:
|
||
break
|
||
|
||
# simulator.handle(command.dangle, command.velocity)
|
||
chunk = simulator.get_chunk()
|
||
command = pilot.handle(chunk)
|
||
command.velocity /= constants.YANDEX_PIXEL_RATIO_18
|
||
|
||
proc_time = np.append(proc_time, command.proccessing_time)
|
||
|
||
# Save Image
|
||
chunk.save_image(Path('images') / f"photo_{i}.png")
|
||
|
||
vis_manager.update_display()
|
||
vis_manager.pause(0.2)
|
||
|
||
vis_manager.set_target_index(pilot.target_idx)
|
||
vis_manager.update_drone_trajectory(pilot.pos.x, pilot.pos.y)
|
||
vis_manager.update_global_map(simulator.pos.x, simulator.pos.y)
|
||
vis_manager.update_error_plot(i, pilot.pos.x, pilot.pos.y, simulator.pos.x, simulator.pos.y)
|
||
|
||
errors.append(np.hypot(pilot.pos.x - simulator.pos.x, pilot.pos.y - simulator.pos.y))
|
||
if last_chunk_index != pilot.target_idx:
|
||
last_chunk_index = pilot.target_idx
|
||
chunk_errors.append(errors[-1])
|
||
chunk_improves.append(errors[-1] - errors[max(len(errors) - 2, 0)])
|
||
|
||
vis_manager.update_display()
|
||
vis_manager.pause(0.2)
|
||
|
||
last_proc_times = proc_time[-10:]
|
||
print("Average FPS:", 1 / last_proc_times.mean())
|
||
print("Pilot coords:", pilot.pos)
|
||
print("Simulator coords:", simulator.pos)
|
||
simulator.handle(command.dangle, command.velocity)
|
||
|
||
print("Errors:", errors)
|
||
print("MSE:", (np.array(errors) ** 2).mean())
|
||
print("RMSE:", (np.array(errors) ** 2).mean() ** 0.5)
|
||
print("Chunk errors:", chunk_errors)
|
||
print("Chunk error improves:", chunk_improves)
|
||
print("Average FPS:", 1 / proc_time.mean())
|
||
vis_manager.show_final()
|
||
|
||
|
||
def main(mode: str, name: str):
|
||
if name is None:
|
||
name = utility.generate_folder_name()
|
||
|
||
if mode == 'build' or mode == 'standalone':
|
||
build(name)
|
||
|
||
if mode == 'run' or mode == 'standalone':
|
||
run(name)
|
||
|
||
def parse_args():
|
||
"""Парсер аргументов командной строки"""
|
||
parser = argparse.ArgumentParser(description='Обработка траекторий')
|
||
|
||
# Добавляем обязательный аргумент --mode
|
||
parser.add_argument(
|
||
'--mode',
|
||
type=str,
|
||
required=True,
|
||
choices=['standalone', 'build', 'run'],
|
||
help='Режим работы: standalone, build или run'
|
||
)
|
||
|
||
# Добавляем опциональный аргумент --name
|
||
parser.add_argument(
|
||
'--name',
|
||
type=str,
|
||
help='Название траектории (обязательно для режимов build и run)'
|
||
)
|
||
|
||
# Парсим аргументы
|
||
args = parser.parse_args()
|
||
|
||
# Проверяем, что для build и run указан --name
|
||
if args.mode in ['build', 'run'] and not args.name:
|
||
parser.error(f"--name обязателен для режима {args.mode}")
|
||
|
||
return args
|
||
|
||
if __name__ == "__main__":
|
||
args = parse_args()
|
||
main(args.mode, args.name)
|
||
|