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

fix: return to center

Browse Source
This commit is contained in:
russian_proger
2025-06-30 02:36:45 +03:00
parent 2d85d78def
commit 2c202af51f
3 changed files with 56 additions and 28 deletions
Download Patch File Download Diff File Expand all files Collapse all files

28
autopilot.py
View File

@@ -9,7 +9,7 @@ random.seed(1)
class Pilot:
def __init__(self): pass
def handle(self, image: Image): pass
def act(self) -> float | None: pass
def act(self) -> tuple[float, float] | None: pass
class AutoPilot(Pilot):
prev_image: np.ndarray | None
@@ -89,11 +89,11 @@ class AutoPilot(Pilot):
for match in good_matches:
# Координаты точки в первом изображении
pt1 = kp1[match.queryIdx].pt
src_pts.append([pt1[0] - center_x1, pt1[1] - center_y1])
src_pts.append([center_x1 - pt1[0], pt1[1] - center_y1])
# Координаты точки во втором изображении
pt2 = kp2[match.trainIdx].pt
dst_pts.append([pt2[0] - center_x2, pt2[1] - center_y2])
dst_pts.append([center_x2 - pt2[0], pt2[1] - center_y2])
# Конвертируем в numpy массивы
src_pts = np.float32(src_pts).reshape(-1, 1, 2)
@@ -125,7 +125,7 @@ class AutoPilot(Pilot):
tx, ty = H[0, 2], H[1, 2]
# Вычисляем угол поворота
angle = np.arctan2(a21, a11)
angle = -np.arctan2(a21, a11)
# Вычисляем масштаб
scale_x = np.sqrt(a11**2 + a21**2)
@@ -150,8 +150,8 @@ class AutoPilot(Pilot):
scale = transformation_info['scale']
# Координаты уже отцентрированы, поэтому используем их напрямую
dx_meters = tx
dy_meters = ty
dx_meters = ty
dy_meters = tx
# Применяем поворот к смещению (учитываем текущий угол БПЛА)
cos_angle = math.cos(self.angle)
@@ -159,8 +159,8 @@ class AutoPilot(Pilot):
# Поворачиваем смещение в глобальные координаты
# Обратите внимание: dy_meters инвертирован, так как в изображениях Y направлен вниз
dx_global = dx_meters * cos_angle - (-dy_meters) * sin_angle
dy_global = dx_meters * sin_angle + (-dy_meters) * cos_angle
dx_global = dx_meters * cos_angle - dy_meters * sin_angle
dy_global = dx_meters * sin_angle + dy_meters * cos_angle
# Обновляем координаты БПЛА
self.x += dx_global
@@ -276,7 +276,7 @@ class AutoPilot(Pilot):
# Обновляем предыдущее изображение
self.prev_image = current_image
def act(self) -> float | None:
def act(self) -> tuple[float, float] | None:
"""
Возвращает угол поворота для управления дроном, чтобы он стремился к точке (0, 0).
Если дрон находится рядом с началом координат (в радиусе 1 метра), возвращает None.
@@ -284,8 +284,8 @@ class AutoPilot(Pilot):
# Расстояние до цели (0, 0)
distance_to_target = math.sqrt(self.x**2 + self.y**2)
# Если дрон находится рядом с целью (в радиусе 1 метра), останавливаемся
if distance_to_target < 1.0:
# Если дрон находится рядом с целью, останавливаемся
if distance_to_target < 10.0:
return None
# Вычисляем угол к цели
@@ -302,7 +302,7 @@ class AutoPilot(Pilot):
angle_diff -= 2 * math.pi
# Возвращаем угол поворота (положительный - поворот влево, отрицательный - вправо)
return max(min(0.05, angle_diff / 2), -0.05)
return max(min(0.1, angle_diff), -0.1), min(10., distance_to_target / 2)
def reset_position(self, x: float = 0.0, y: float = 0.0, angle: float = 0.0):
"""Сбрасывает позицию и угол БПЛА"""
@@ -318,12 +318,12 @@ class RandomPilot(Pilot):
def __init__(self, velocity: float = 1):
self.counter = 0
def act(self) -> float | None:
def act(self) -> tuple[float, float] | None:
self.counter += 1
if self.counter > 40:
return None
return 1 / (self.counter + 20)
return 1 / (self.counter + 20), 10.0
# def _test():
# randomPilot = RandomPilot()

15
simulator.py
View File

@@ -107,26 +107,27 @@ class Simulator:
html = self.driver.find_element(By.TAG_NAME, 'html')
action = ActionChains(self.driver)
velocity = 10
# Добавляем начальную точку в траекторию
self.update_trajectory(0, 0)
self.viz_manager.update_global_map(self.current_x, self.current_y, self.mode)
for i in range(1000):
dangle = None
signal = None
if self.mode == SimMode.OPERATOR:
dangle = self.operatorPilot.act()
if dangle is None:
signal = self.operatorPilot.act()
if signal is None:
self.mode = SimMode.AUTONOME
print("Режим возвращения домой!")
if self.mode == SimMode.AUTONOME:
dangle = self.autonomePilot.act()
signal = self.autonomePilot.act()
if dangle is None:
if signal is None:
break
dangle, velocity = signal
# Сдвиг камеры
action = ActionChains(self.driver)
action.move_to_element_with_offset(html, 200, 200)
@@ -148,7 +149,7 @@ class Simulator:
im = im.crop([0, 80, im.width-80, im.height-60])
# Применяем поворот как будто съемка с дрона
rotated_im = self.rotate_image_like_drone(im, self.angle + math.pi / 2)
rotated_im = self.rotate_image_like_drone(im, math.pi / 2 - self.angle)
# Передаем изображение в AutoPilot для анализа
self.autonomePilot.handle(rotated_im)

41
visualization.py
View File

@@ -27,6 +27,7 @@ class VisualizationManager:
def __init__(self, window_title="Drone Autopilot Visualization"):
self.window_title = window_title
self.fig = None
self.ax_drone_view = None
self.ax_global_map = None
self.ax_detection = None
self.ax_matches = None
@@ -43,6 +44,9 @@ class VisualizationManager:
self.keypoints = []
self.matches = []
# Данные для вида БПЛА
self.drone_view_image = None
self._setup_window()
def _setup_window(self):
@@ -54,11 +58,16 @@ class VisualizationManager:
# Открываем окно на полный экран
self.fig.canvas.manager.window.state('zoomed')
# Создаем сетку 2x2
gs = self.fig.add_gridspec(2, 2, hspace=0.3, wspace=0.3)
# Создаем сетку 2x2 с разными размерами колонок
gs = self.fig.add_gridspec(2, 2, hspace=0.3, wspace=0.3, width_ratios=[1, 1])
# Глобальная карта (левый верхний угол, занимает 2x1)
self.ax_global_map = self.fig.add_subplot(gs[0, :])
# Вид БПЛА (левый верхний угол)
self.ax_drone_view = self.fig.add_subplot(gs[0, 0])
self.ax_drone_view.set_title('Вид БПЛА')
self.ax_drone_view.axis('off')
# Глобальная карта (левый нижний угол)
self.ax_global_map = self.fig.add_subplot(gs[1, 0])
self.ax_global_map.set_title('Global Map - Траектория полета беспилотника')
self.ax_global_map.set_xlabel('X координата')
self.ax_global_map.set_ylabel('Y координата')
@@ -66,8 +75,8 @@ class VisualizationManager:
self.ax_global_map.axhline(y=0, color='k', linestyle='-', alpha=0.3)
self.ax_global_map.axvline(x=0, color='k', linestyle='-', alpha=0.3)
# Детекция ключевых точек (правый нижний угол)
self.ax_detection = self.fig.add_subplot(gs[1, 0])
# Детекция ключевых точек (правый верхний угол)
self.ax_detection = self.fig.add_subplot(gs[0, 1])
self.ax_detection.set_title('Keypoint Detection')
self.ax_detection.axis('off')
@@ -100,7 +109,7 @@ class VisualizationManager:
if len(self.trajectory_x) > 1:
# Разделяем траекторию по режимам
operator_indices = [i for i, m in enumerate(self.trajectory_modes) if m == SimMode.OPERATOR]
autonome_indices = [i for i, m in enumerate(self.trajectory_modes) if m == SimMode.AUTONOME]
autonome_indices = operator_indices[-1:] + [i for i, m in enumerate(self.trajectory_modes) if m == SimMode.AUTONOME]
# Рисуем траекторию оператора (синий цвет)
if len(operator_indices) > 1:
@@ -139,6 +148,24 @@ class VisualizationManager:
self.ax_global_map.set_xlim(x_min - margin, x_max + margin)
self.ax_global_map.set_ylim(y_min - margin, y_max + margin)
def update_drone_view(self, image: np.ndarray):
"""Обновляет вид БПЛА"""
self.drone_view_image = image.copy()
self.ax_drone_view.clear()
self.ax_drone_view.set_title('Вид БПЛА')
if image is not None:
# Конвертируем BGR в RGB для matplotlib
if len(image.shape) == 3 and image.shape[2] == 3:
image_rgb = cv2.cvtColor(image, cv2.COLOR_BGR2RGB)
else:
image_rgb = image
self.ax_drone_view.imshow(image_rgb)
self.ax_drone_view.axis('off')
def update_detection(self, image: np.ndarray, keypoints):
"""Обновляет визуализацию детекции ключевых точек"""
self.current_frame = image.copy()