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

ref: comment output

Browse Source
This commit is contained in:
russian_proger
2025-12-19 22:35:49 +03:00
parent f0ac60c8ef
commit d04d4a0cdc
3 changed files with 64 additions and 26 deletions
Download Patch File Download Diff File Expand all files Collapse all files

73
autopilot.py
View File

@@ -7,6 +7,8 @@ import cv2
import numpy as np import numpy as np
from PIL import Image from PIL import Image
from timer import Timer
from visualization import VisualizationManager from visualization import VisualizationManager
random.seed(1) random.seed(1)
@@ -21,11 +23,18 @@ class PilotCommand:
velocity: float velocity: float
dangle: float dangle: float
stop: bool stop: bool
proccessing_time: float
def __init__(self, dangle: float = 0, velocity: float = 100, stop: bool = False): def __init__(self,
dangle: float = 0,
velocity: float = 100,
stop: bool = False,
processing_time: float = 0.
):
self.dangle = dangle self.dangle = dangle
self.stop = stop self.stop = stop
self.velocity = velocity self.velocity = velocity
self.proccessing_time = processing_time
class AutoPilot(Pilot): class AutoPilot(Pilot):
@@ -40,12 +49,16 @@ class AutoPilot(Pilot):
target_idx: int # Текущая целевая метка target_idx: int # Текущая целевая метка
# Всякие вспомогательные штуки # Всякие вспомогательные штуки
timer: Timer
prev_image: np.ndarray | None prev_image: np.ndarray | None
orb_detector: cv2.ORB orb_detector: cv2.ORB
bf_matcher: cv2.BFMatcher bf_matcher: cv2.BFMatcher
frame_count: int frame_count: int
image_center: tuple # Центр изображения (x, y) image_center: tuple # Центр изображения (x, y)
vis_manager: VisualizationManager # Менеджер визуализации (опционально) vis_manager: VisualizationManager # Менеджер визуализации (опционально)
# Положение на основе ориентира
reserved_pos: tuple[float, float, float] | None
def __init__(self, points = [], keypoints = [], viz_manager=None): def __init__(self, points = [], keypoints = [], viz_manager=None):
self.prev_image = None self.prev_image = None
@@ -66,7 +79,7 @@ class AutoPilot(Pilot):
# Инициализация ORB детектора # Инициализация ORB детектора
self.orb_detector = cv2.ORB_create( self.orb_detector = cv2.ORB_create(
nfeatures=1800, nfeatures=1000,
scaleFactor=1.3, scaleFactor=1.3,
nlevels=4, nlevels=4,
edgeThreshold=19, edgeThreshold=19,
@@ -82,6 +95,8 @@ class AutoPilot(Pilot):
self.keypoints = keypoints self.keypoints = keypoints
self.target_idx = 0 self.target_idx = 0
self.timer = Timer()
def get_position(self) -> tuple[float, float]: def get_position(self) -> tuple[float, float]:
return self.x, self.y return self.x, self.y
@@ -195,7 +210,7 @@ class AutoPilot(Pilot):
# Смещение (уже отцентрировано) # Смещение (уже отцентрировано)
tx, ty = -H[0, 2], -H[1, 2] tx, ty = -H[0, 2], -H[1, 2]
print(" [Pilot] translate:", tx, ty) # print(" [Pilot] translate:", tx, ty)
# Вычисляем угол поворота # Вычисляем угол поворота
angle = -np.arctan2(a21, a11) angle = -np.arctan2(a21, a11)
@@ -227,12 +242,13 @@ class AutoPilot(Pilot):
) -> tuple[float, float, float] | None: ) -> tuple[float, float, float] | None:
""" Возвращает новые координаты и поворот на основе матрицы преобразования """ """ Возвращает новые координаты и поворот на основе матрицы преобразования """
tx, ty = -mat[0, 2], -mat[1, 2] tx, ty = -mat[0, 2], -mat[1, 2]
# Вычисляем угол поворота # Вычисляем угол поворота
rotation = -np.arctan2(mat[1, 0], mat[0, 0]) rotation = -np.arctan2(mat[1, 0], mat[0, 0])
print("[HOMOGRAPHY]", mat) # print("[HOMOGRAPHY]", mat)
print("[ROTATION]", rotation) # print("[ROTATION]", rotation)
# Координаты уже отцентрированы, поэтому используем их напрямую # Координаты уже отцентрированы, поэтому используем их напрямую
dx_meters = tx dx_meters = tx
@@ -248,11 +264,11 @@ class AutoPilot(Pilot):
# Обратите внимание: dy_meters инвертирован, так как в изображениях Y направлен вниз # Обратите внимание: dy_meters инвертирован, так как в изображениях Y направлен вниз
dx_global = dx_meters * cos_angle - dy_meters * sin_angle dx_global = dx_meters * cos_angle - dy_meters * sin_angle
dy_global = dx_meters * sin_angle + dy_meters * cos_angle dy_global = dx_meters * sin_angle + dy_meters * cos_angle
# Обновляем координаты БПЛА # Обновляем координаты БПЛА
x = x_source + dx_global x = x_source + dx_global
y = y_source + dy_global y = y_source + dy_global
# Нормализуем угол в диапазоне [-π, π] # Нормализуем угол в диапазоне [-π, π]
angle = math.atan2(math.sin(angle_global), math.cos(angle_global)) angle = math.atan2(math.sin(angle_global), math.cos(angle_global))
@@ -296,7 +312,10 @@ class AutoPilot(Pilot):
src_pts, dst_pts, matches, kp1, kp2 = self.detect_and_match_keypoints(landmark_image, current_image) src_pts, dst_pts, matches, kp1, kp2 = self.detect_and_match_keypoints(landmark_image, current_image)
if src_pts is not None and dst_pts is not None and self.vis_manager: if src_pts is not None and dst_pts is not None and self.vis_manager:
was_enabled = self.timer.enabled
if was_enabled: self.timer.stop()
self.vis_manager.update_chunk_matches(landmark_image, current_image, kp1, kp2, matches) self.vis_manager.update_chunk_matches(landmark_image, current_image, kp1, kp2, matches)
if was_enabled: self.timer.start()
if src_pts is not None and dst_pts is not None: if src_pts is not None and dst_pts is not None:
# Оцениваем матрицу трансформации # Оцениваем матрицу трансформации
@@ -310,23 +329,24 @@ class AutoPilot(Pilot):
rmse = landmark_transform.get('rmse', None) rmse = landmark_transform.get('rmse', None)
ok_rmse = (rmse is not None and rmse <= self.max_reproj_rmse) ok_rmse = (rmse is not None and rmse <= self.max_reproj_rmse)
if ok_scale and ok_inliers and ok_ratio and ok_rmse: if ok_scale and ok_inliers and ok_ratio and ok_rmse:
print("[HELP]") # print("[HELP]")
print("Matrix", landmark_transform['homography']) # print("Matrix", landmark_transform['homography'])
print("Position", self.x, self.y) # print("Position", self.x, self.y)
print("Position of point", self.points[self.target_idx]) # print("Position of point", self.points[self.target_idx])
print("[PILOT]", rmse, ratio, ok_rmse) # print("[PILOT]", rmse, ratio, ok_rmse)
# if False: # if False:
# Считаем абсолютную позу относительно координат ориентира # Считаем абсолютную позу относительно координат ориентира
landmark_world_x, landmark_world_y = self.points[self.target_idx] landmark_world_x, landmark_world_y = self.points[self.target_idx]
landmark_angle = 0 landmark_angle = 0
homography = landmark_transform['homography'] homography = landmark_transform['homography']
# homography = np.linalg.inv(homography) # homography = np.linalg.inv(homography)
print(f" [Pilot] Landmark correction applied (inliers={landmark_transform['inliers']}, ratio={ratio:.2f}, rmse={rmse:.2f})") # print(f" [Pilot] Landmark correction applied (inliers={landmark_transform['inliers']}, ratio={ratio:.2f}, rmse={rmse:.2f})")
return self.calc_position(homography, landmark_world_x, landmark_world_y, landmark_angle) return self.calc_position(homography, landmark_world_x, landmark_world_y, landmark_angle)
return None return None
def handle(self, image: Image) -> PilotCommand: def handle(self, image: Image) -> PilotCommand:
self.timer.start()
self.frame_count += 1 self.frame_count += 1
if self.prev_image is None: if self.prev_image is None:
@@ -335,7 +355,8 @@ class AutoPilot(Pilot):
height, width = self.prev_image.shape[:2] height, width = self.prev_image.shape[:2]
self.image_center = (width // 2, height // 2) self.image_center = (width // 2, height // 2)
return PilotCommand() self.timer.stop()
return PilotCommand(processing_time=self.timer.get_elapsed())
# Конвертируем текущее изображение # Конвертируем текущее изображение
current_image = self.image_to_numpy(image) current_image = self.image_to_numpy(image)
@@ -362,13 +383,15 @@ class AutoPilot(Pilot):
# Обновляем позицию и угол БПЛА (одометрия по кадрам) # Обновляем позицию и угол БПЛА (одометрия по кадрам)
self.update_drone_position(transformation_info) self.update_drone_position(transformation_info)
self.timer.stop()
# Выводим текущее состояние БПЛА # Выводим текущее состояние БПЛА
drone_state = self.get_drone_state() drone_state = self.get_drone_state()
print(f" [Pilot] Drone Position: ({drone_state['x']:.2f}, {drone_state['y']:.2f})") # 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] Angle: {drone_state['angle_degrees']:.1f}°")
print(f" [Pilot] Target Index: {self.target_idx}") # print(f" [Pilot] Target Index: {self.target_idx}")
print(f" [Pilot] Target Position: {self.points[self.target_idx]}") # print(f" [Pilot] Target Position: {self.points[self.target_idx]}")
print(f" [Pilot] Distance: {distance_to_target}") # print(f" [Pilot] Distance: {distance_to_target}")
# Обновляем визуализацию # Обновляем визуализацию
if self.vis_manager: if self.vis_manager:
@@ -394,6 +417,8 @@ class AutoPilot(Pilot):
homography_matrix, homography_matrix,
grid_step=85) grid_step=85)
self.timer.start()
# Пытаемся найти ориентир на картинке: # Пытаемся найти ориентир на картинке:
self.prev_image = current_image self.prev_image = current_image
npos = self.get_position_by_chunk() npos = self.get_position_by_chunk()
@@ -420,7 +445,7 @@ class AutoPilot(Pilot):
angle_trajectory = self.angle + math.pi / 2 angle_trajectory = self.angle + math.pi / 2
print("[ANGLE]", angle_trajectory, "->", target_angle) # print("[ANGLE]", angle_trajectory, "->", target_angle)
# Вычисляем разность углов (направление поворота) # Вычисляем разность углов (направление поворота)
angle_diff = target_angle - angle_trajectory angle_diff = target_angle - angle_trajectory
@@ -432,7 +457,13 @@ class AutoPilot(Pilot):
d_r = max(10, min(75., distance_to_target / 2)) d_r = max(10, min(75., distance_to_target / 2))
d_a_limit = d_r / 10 * 0.07 d_a_limit = d_r / 10 * 0.07
return PilotCommand(max(min(d_a_limit, angle_diff), -d_a_limit), d_r)
command = PilotCommand(
max(min(d_a_limit, angle_diff), -d_a_limit),
d_r, False, self.timer.get_elapsed()
)
self.timer.reset()
return command
def reset_position(self, x: float = 0.0, y: float = 0.0, angle: float = 0.0): def reset_position(self, x: float = 0.0, y: float = 0.0, angle: float = 0.0):
"""Сбрасывает позицию и угол БПЛА""" """Сбрасывает позицию и угол БПЛА"""

9
main.py
View File

@@ -87,7 +87,7 @@ def main():
vis_manager = VisualizationManager() vis_manager = VisualizationManager()
width, height = yandexMap.get_size() width, height = yandexMap.get_size()
print(width, height) # print(width, height)
points_coords = np.array(list(map(lambda p: [ points_coords = np.array(list(map(lambda p: [
(p[0] - points[0][0]) * width, (points[0][1] - p[1]) * height (p[0] - points[0][0]) * width, (points[0][1] - p[1]) * height
], points))) ], points)))
@@ -104,10 +104,14 @@ def main():
vis_manager.set_target_points(points_coords) vis_manager.set_target_points(points_coords)
proc_time = np.array([])
for i in range(10000000000): for i in range(10000000000):
print(f"Image #{i}") print(f"Image #{i}")
photo = simulator.get_photo() photo = simulator.get_photo()
command = pilot.handle(photo) command = pilot.handle(photo)
proc_time = np.append(proc_time, command.proccessing_time)
# Save Image # Save Image
photo.save(Path('images') / f"photo_{i}.png") photo.save(Path('images') / f"photo_{i}.png")
@@ -128,6 +132,9 @@ def main():
vis_manager.update_display() vis_manager.update_display()
vis_manager.pause(0.2) vis_manager.pause(0.2)
last_proc_times = proc_time[-10:]
# print("Average FPS:", 1 / last_proc_times.mean())
vis_manager.show_final() vis_manager.show_final()
if __name__ == "__main__": if __name__ == "__main__":

8
simulator.py
View File

@@ -86,16 +86,16 @@ class Simulator:
action.click_and_hold() action.click_and_hold()
self.angle += dangle self.angle += dangle
print(f" [Simulator] angle: {self.angle / math.pi * 180:.1f}°") # print(f" [Simulator] angle: {self.angle / math.pi * 180:.1f}°")
velocity = max(velocity, 10) velocity = max(velocity, 10)
dx = math.cos(math.pi / 2 + self.angle) * velocity dx = math.cos(math.pi / 2 + self.angle) * velocity
dy = math.sin(math.pi / 2 + self.angle) * velocity dy = math.sin(math.pi / 2 + self.angle) * velocity
print(" [Simulator] dx, dy:", [dx, dy]) # print(" [Simulator] dx, dy:", [dx, dy])
self.update_trajectory(dx, dy) self.update_trajectory(dx, dy)
action.move_by_offset(-dx, dy) action.move_by_offset(-dx, dy)
action.release() action.release()
action.perform() action.perform()
print(f" [Simulator] Position: {self.current_x}, {self.current_y}") # print(f" [Simulator] Position: {self.current_x}, {self.current_y}")
def get_photo(self) -> Image.Image: def get_photo(self) -> Image.Image:
png = self.yandexMap.driver.get_screenshot_as_png() png = self.yandexMap.driver.get_screenshot_as_png()
@@ -120,7 +120,7 @@ class Simulator:
signal = self.operatorPilot.act() signal = self.operatorPilot.act()
if signal is None: if signal is None:
self.mode = SimMode.AUTONOME self.mode = SimMode.AUTONOME
print("Режим возвращения домой!") # print("Режим возвращения домой!")
if self.mode == SimMode.AUTONOME: if self.mode == SimMode.AUTONOME:
signal = self.autonomePilot.act() signal = self.autonomePilot.act()