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

80
autopilot.py
View File

@@ -1,4 +1,5 @@
from enum import Enum
from pathlib import Path
import math
import random
@@ -17,12 +18,16 @@ class Pilot:
def get_position(self) -> tuple[float, float]: pass
class PilotCommand:
velocity: float
dangle: float
stop: bool
def __init__(self, dangle: float = 0, stop: bool = False):
def __init__(self, dangle: float = 0, velocity: float = 100, stop: bool = False):
self.dangle = dangle
self.stop = stop
self.velocity = velocity
MOVE_KOF: float = 0.8274
class AutoPilot(Pilot):
@@ -118,11 +123,11 @@ class AutoPilot(Pilot):
for match in good_matches:
# Координаты точки в первом изображении
pt1 = kp1[match.queryIdx].pt
src_pts.append([center_x1 - pt1[0], pt1[1] - center_y1])
src_pts.append([pt1[0] - center_x1, center_y1 - pt1[1]])
# Координаты точки во втором изображении
pt2 = kp2[match.trainIdx].pt
dst_pts.append([center_x2 - pt2[0], pt2[1] - center_y2])
dst_pts.append([pt2[0] - center_x2, center_y2 - pt2[1]])
# Конвертируем в numpy массивы
src_pts = np.float32(src_pts).reshape(-1, 1, 2)
@@ -137,6 +142,7 @@ class AutoPilot(Pilot):
"""
# Используем RANSAC для оценки матрицы гомографии
H, mask = cv2.findHomography(src_pts, dst_pts, cv2.RANSAC, 5.0)
RH, rmask = cv2.findHomography(dst_pts, src_pts, cv2.RANSAC, 5.0)
if H is None:
return None
@@ -151,7 +157,9 @@ class AutoPilot(Pilot):
a21, a22 = H[1, 0], H[1, 1]
# Смещение (уже отцентрировано)
tx, ty = H[0, 2], H[1, 2]
tx, ty = RH[0, 2] * MOVE_KOF, RH[1, 2] * MOVE_KOF
print(" [Pilot] translate:", tx, ty)
# Вычисляем угол поворота
angle = -np.arctan2(a21, a11)
@@ -258,7 +266,7 @@ class AutoPilot(Pilot):
self.image_center = (width // 2, height // 2)
# Обновляем визуализацию детекции
if self.vis_manager:
if self.vis_manager is not None:
kp, _ = self.orb_detector.detectAndCompute(self.prev_image, None)
self.vis_manager.update_detection(self.prev_image, kp)
@@ -271,9 +279,16 @@ class AutoPilot(Pilot):
height, width = current_image.shape[:2]
self.image_center = (width // 2, height // 2)
# Расстояние до цели
distance_to_target = math.sqrt(
(self.points[self.target_idx][0] - self.x) ** 2 +
(self.points[self.target_idx][1] - self.y) ** 2
)
# Обнаруживаем и сопоставляем ключевые точки
src_pts, dst_pts, matches, kp1, kp2 = self.detect_and_match_keypoints(self.prev_image, current_image)
# Оцениваем смещение БПЛА
if src_pts is not None and dst_pts is not None:
# Оцениваем матрицу трансформации
transformation_info = self.estimate_transformation_matrix(src_pts, dst_pts)
@@ -284,49 +299,54 @@ class AutoPilot(Pilot):
# Выводим текущее состояние БПЛА
drone_state = self.get_drone_state()
self.vis_manager.update_drone_trajectory(drone_state['x'], drone_state['y'])
if self.vis_manager:
self.vis_manager.update_drone_trajectory(drone_state['x'], drone_state['y'])
print(f" [Pilot] Drone Position: ({drone_state['x']:.2f}, {drone_state['y']:.2f})")
print(f" [Pilot] Angle: {drone_state['angle_degrees']:.1f}°")
print(f" [Pilot] Target Index: {self.target_idx}")
print(f" [Pilot] Target Position: {self.points[self.target_idx]}")
print(f" [Pilot] Distance: {distance_to_target}")
# Обновляем визуализацию
if self.vis_manager:
# Обновляем детекцию ключевых точек
self.vis_manager.update_detection(current_image, kp2)
# Обновляем сопоставление точек
self.vis_manager.update_matches(self.prev_image, current_image,
kp1, kp2, matches, transformation_info)
# Обновляем предыдущее изображение
self.prev_image = current_image
return PilotCommand()
def act(self) -> tuple[float, float] | None:
"""
Возвращает угол поворота для управления дроном, чтобы он стремился к точке (0, 0).
Если дрон находится рядом с началом координат (в радиусе 1 метра), возвращает None.
"""
# Расстояние до цели (0, 0)
distance_to_target = math.sqrt(self.x**2 + self.y**2)
# Если дрон находится рядом с целью, останавливаемся
if distance_to_target < 30.0:
return None
# Обновляем сопоставление точек
self.vis_manager.update_matches(
self.prev_image,
current_image,
kp1, kp2, matches,
transformation_info)
# Пытаемся найти ориентир на картинке:
landmark_image = cv2.imread(Path('chunks') / f'chunk_{self.target_idx}.png', cv2.IMREAD_COLOR_RGB)
src_pts, dst_pts, matches, kp1, kp2 = self.detect_and_match_keypoints(current_image, landmark_image)
# if src_pts is not None and dst_pts is not None:
# # Оцениваем матрицу трансформации
# transformation_info = self.estimate_transformation_matrix(src_pts, dst_pts)
# if self.vis_manager:
# self.vis_manager.update_chunk_matches(current_image, landmark_image, kp1, kp2, matches, transformation_info)
if distance_to_target < 50:
self.target_idx += 1
if self.target_idx == len(self.points):
return PilotCommand(stop=True)
# Вычисляем угол к цели
target_angle = math.atan2(-self.y, -self.x) # Отрицательные координаты, так как движемся к (0,0)
target_angle = math.atan2(self.points[self.target_idx][1] - self.y, self.points[self.target_idx][0] - self.x)
# Вычисляем разность углов (направление поворота)
angle_diff = target_angle - self.angle
print(self.angle, target_angle, angle_diff)
# Нормализуем разность углов в диапазон [-π, π]
angle_diff %= 2 * math.pi
if angle_diff >= math.pi:
angle_diff -= 2 * math.pi
# Возвращаем угол поворота (положительный - поворот влево, отрицательный - вправо)
return max(min(0.1, angle_diff), -0.1), min(10., distance_to_target / 2)
self.prev_image = current_image
return PilotCommand(max(min(0.1, angle_diff), -0.1), min(50., distance_to_target / 2))
def reset_position(self, x: float = 0.0, y: float = 0.0, angle: float = 0.0):
"""Сбрасывает позицию и угол БПЛА"""