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

feat: chunks from google

Browse Source
This commit is contained in:
russian_proger
2026-01-11 13:54:37 +03:00
parent 31c0f13361
commit 3ee3599b87
11 changed files with 410 additions and 180 deletions
Download Patch File Download Diff File Expand all files Collapse all files

252
main.py
View File

@@ -1,23 +1,28 @@
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
from vision_chunk import VisionChunk
from utility import cv2_to_pil
import random
import argparse
import autopilot
import constants
import cv2
import matplotlib.pyplot as plt
import numpy as np
import pickle
import random
import utility
import autopilot
def make_global_photo(filename):
yandexMap = YandexMap()
yandexMap.save_photo(filename)
yandexMap.destroy()
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)
@@ -26,97 +31,111 @@ def get_trajectory_points(bg_img: str) -> list[(float, float)]:
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')
def build(name: str):
# Получаем траекторию от пользователя
# points = get_trajectory_points('map.jpg')
# Создание папки с информацией о маршруте
dir = Path('trajectories')
if not dir.exists(): dir.mkdir()
dir /= name
assert not dir.exists()
dir.mkdir()
dir_chunks = dir / 'chunks'
dir_chunks.mkdir()
# Trajectory #1
# points = [[np.float64(0.5384504359393909), np.float64(0.4084520767967683)], [np.float64(0.4451750568707629), np.float64(0.38213330305374654)], [np.float64(0.49266070439660997), np.float64(0.2789637099811013)], [np.float64(0.36377108968359656), np.float64(0.3263375027185404)], [np.float64(0.3535955937852008), np.float64(0.4337180995900692)]]
# Trajectory #2
# points = [[np.float64(0.29197731306713737), np.float64(0.3452870198135161)], [np.float64(0.33494051797147517), np.float64(0.2010601397017569)], [np.float64(0.39768940934491587), np.float64(0.25369768718780034)], [np.float64(0.4027771572941138), np.float64(0.4158213334448144)], [np.float64(0.2914120077394487), np.float64(0.5547844588079692)]]
# Trajectory #3
# points = [[np.float64(0.2755834585641664), np.float64(0.45687862048392835)], [np.float64(0.295934450360958), np.float64(0.5021469113219258)], [np.float64(0.32872215936689997), np.float64(0.4810918923275084)], [np.float64(0.3649017003389739), np.float64(0.5295184360146684)], [np.float64(0.3999506306556705), np.float64(0.49477765467387963)]]
# Trajectory #4
# points = [[np.float64(0.42143223310783934), np.float64(0.6663760594783815)], [np.float64(0.4253893704016599), np.float64(0.5537317078582484)], [np.float64(0.5124463908657128), np.float64(0.5621537154560153)], [np.float64(0.5124463908657128), np.float64(0.6684815613778233)], [np.float64(0.42143223310783934), np.float64(0.6663760594783815)]]
# Trajectory #5
# points = [[np.float64(0.5983728006743884), np.float64(0.7348048712102382)], [np.float64(0.5966768846913225), np.float64(0.5453097002604814)], [np.float64(0.6345523416464622), np.float64(0.7190136069644251)], [np.float64(0.6402053949233488), np.float64(0.5495207040593649)], [np.float64(0.5983728006743884), np.float64(0.7348048712102382)]]
# Trajectory #6
# points = [[np.float64(0.4406526142492536), np.float64(0.28106921188054296)], [np.float64(0.38581799746345413), np.float64(0.2968604761263561)], [np.float64(0.3931669667234066), np.float64(0.353709027411283)], [np.float64(0.4248240650739713), np.float64(0.35265627646156217)], [np.float64(0.40616898926024564), np.float64(0.3179154951207735)]]
# Trajectory #7
# points = [[np.float64(0.5491912371654754), np.float64(0.7505961354560512)], [np.float64(0.5537136797869846), np.float64(0.6863783275230781)], [np.float64(0.5017055896396284), np.float64(0.6653233085286606)], [np.float64(0.5520177638039186), np.float64(0.6042637534448503)], [np.float64(0.5593667330638712), np.float64(0.516885424618018)]]
# Trajectory #8
# points =
# Trajectory #9
# points =
# Trajectory #10
# points =
print(points)
# Для каждой точки сделаем приближенный снимок
yandexMap = YandexMap()
chunks: list[VisionChunk] = []
plt.ion()
for i in range(len(points)):
point = points[i]
yandexMap.scroll(point[0], point[1], 5, True)
sleep(1)
cv2_img = yandexMap.make_screenshot(point[0], point[1], 0.2, 0.2)
img = cv2_to_pil(cv2_img)
chunk = VisionChunk(img)
Path('chunks').mkdir(exist_ok=True)
chunk.save_image(Path('.') / 'chunks' / f'chunk_{i}.png')
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)):
chunk = VisionChunk.load_image(Path('chunks') / f'chunk_{i}.png')
chunks.append(chunk)
kp, des = chunk.compute_keypoints()
plt.subplot(1, len(points), i+1)
plt.imshow(chunk.image)
kp_coords = np.array([j.pt for j in kp])
if len(kp_coords) > 0:
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)
# make_global_photo('map.jpg', 15)
points = get_trajectory_points('map.jpg')
google_map = GoogleMap(initial_zoom=15)
# Начнём симуляцию полёта с первой точки
yandexMap.scroll(points[0][0], points[0][1], 5, True)
sleep(0.2)
yandexMap.make_as_center(*points[0])
sleep(1)
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)
vis_manager = VisualizationManager()
width, height = yandexMap.get_size()
# print(width, height)
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 *= 2 ** 4
pilot = autopilot.AutoPilot(points_coords, chunks, vis_manager)
simulator = Simulator(yandexMap)
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()
@@ -125,7 +144,7 @@ def main():
vis_manager.update_display()
vis_manager.pause(1)
vis_manager.set_target_points(points_coords)
vis_manager.set_target_points(points)
proc_time = np.array([])
@@ -157,6 +176,7 @@ def main():
# 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)
@@ -194,7 +214,47 @@ def main():
print("Average FPS:", 1 / proc_time.mean())
vis_manager.show_final()
if __name__ == "__main__":
main()
#TODO
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)