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

feat: add chunks to autopilot, test for estimation

Browse Source
This commit is contained in:
russian_proger
2025-10-01 20:47:06 +03:00
parent 7a1a4050c8
commit 4d3e0d0e59
6 changed files with 297 additions and 55 deletions
Download Patch File Download Diff File Expand all files Collapse all files

68
main.py
View File

@@ -29,27 +29,27 @@ 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)]
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)
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()
@@ -66,12 +66,14 @@ np.float64(0.5699957136274587)]
fastThreshold=20
)
img = cv2.imread(Path('chunks') / f'chunk_{i}.png')
kp: list[cv2.KeyPoint]
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')
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()
@@ -79,21 +81,45 @@ np.float64(0.5699957136274587)]
# Начнём симуляцию полёта с первой точки
yandexMap.scroll(points[0][0], points[0][1], 5, True)
sleep(1)
yandexMap.make_as_center(*points[0])
sleep(5)
vis_manager = VisualizationManager()
pilot = autopilot.AutoPilot(points, keypoints, vis_manager)
width, height = yandexMap.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, keypoints, vis_manager)
simulator = Simulator(yandexMap)
pilot.target_idx = 0
while True:
photo = simulator.get_photo()
command = pilot.handle(photo)
print("DEBUG SIZE:", yandexMap.get_size())
print("DEBUG SIZE:", photo.size)
vis_manager.update_display()
vis_manager.pause(10)
for i in range(10000000000):
photo = simulator.get_photo()
command = pilot.handle(photo)
# Save Image
photo.save(Path('images') / f"photo_{i}.png")
vis_manager.update_display()
vis_manager.pause(1)
if command.stop:
break
simulator.handle(command.dangle)
simulator.handle(command.dangle, command.velocity)
vis_manager.update_display()
vis_manager.pause(0.2)
vis_manager.pause(50)
sleep(30)
if __name__ == "__main__":