diff --git a/autopilot.py b/autopilot.py index 3422816..2494e8b 100644 --- a/autopilot.py +++ b/autopilot.py @@ -7,6 +7,8 @@ import cv2 import numpy as np from PIL import Image +from timer import Timer + from visualization import VisualizationManager random.seed(1) @@ -21,11 +23,18 @@ class PilotCommand: velocity: float dangle: float 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.stop = stop self.velocity = velocity + self.proccessing_time = processing_time class AutoPilot(Pilot): @@ -40,12 +49,16 @@ class AutoPilot(Pilot): target_idx: int # Текущая целевая метка # Всякие вспомогательные штуки + timer: Timer prev_image: np.ndarray | None orb_detector: cv2.ORB bf_matcher: cv2.BFMatcher frame_count: int image_center: tuple # Центр изображения (x, y) vis_manager: VisualizationManager # Менеджер визуализации (опционально) + + # Положение на основе ориентира + reserved_pos: tuple[float, float, float] | None def __init__(self, points = [], keypoints = [], viz_manager=None): self.prev_image = None @@ -66,7 +79,7 @@ class AutoPilot(Pilot): # Инициализация ORB детектора self.orb_detector = cv2.ORB_create( - nfeatures=1800, + nfeatures=1000, scaleFactor=1.3, nlevels=4, edgeThreshold=19, @@ -82,6 +95,8 @@ class AutoPilot(Pilot): self.keypoints = keypoints self.target_idx = 0 + self.timer = Timer() + def get_position(self) -> tuple[float, float]: return self.x, self.y @@ -195,7 +210,7 @@ class AutoPilot(Pilot): # Смещение (уже отцентрировано) tx, ty = -H[0, 2], -H[1, 2] - print(" [Pilot] translate:", tx, ty) + # print(" [Pilot] translate:", tx, ty) # Вычисляем угол поворота angle = -np.arctan2(a21, a11) @@ -227,12 +242,13 @@ class AutoPilot(Pilot): ) -> tuple[float, float, float] | None: """ Возвращает новые координаты и поворот на основе матрицы преобразования """ + tx, ty = -mat[0, 2], -mat[1, 2] # Вычисляем угол поворота rotation = -np.arctan2(mat[1, 0], mat[0, 0]) - print("[HOMOGRAPHY]", mat) - print("[ROTATION]", rotation) + # print("[HOMOGRAPHY]", mat) + # print("[ROTATION]", rotation) # Координаты уже отцентрированы, поэтому используем их напрямую dx_meters = tx @@ -248,11 +264,11 @@ 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 - + # Обновляем координаты БПЛА x = x_source + dx_global y = y_source + dy_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) 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) + if was_enabled: self.timer.start() 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) ok_rmse = (rmse is not None and rmse <= self.max_reproj_rmse) if ok_scale and ok_inliers and ok_ratio and ok_rmse: - print("[HELP]") - print("Matrix", landmark_transform['homography']) - print("Position", self.x, self.y) - print("Position of point", self.points[self.target_idx]) - print("[PILOT]", rmse, ratio, ok_rmse) + # print("[HELP]") + # print("Matrix", landmark_transform['homography']) + # print("Position", self.x, self.y) + # print("Position of point", self.points[self.target_idx]) + # print("[PILOT]", rmse, ratio, ok_rmse) # if False: # Считаем абсолютную позу относительно координат ориентира landmark_world_x, landmark_world_y = self.points[self.target_idx] landmark_angle = 0 homography = landmark_transform['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 None def handle(self, image: Image) -> PilotCommand: + self.timer.start() self.frame_count += 1 if self.prev_image is None: @@ -335,7 +355,8 @@ class AutoPilot(Pilot): height, width = self.prev_image.shape[: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) @@ -362,13 +383,15 @@ class AutoPilot(Pilot): # Обновляем позицию и угол БПЛА (одометрия по кадрам) self.update_drone_position(transformation_info) + self.timer.stop() + # Выводим текущее состояние БПЛА drone_state = self.get_drone_state() - 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}") + # 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: @@ -394,6 +417,8 @@ class AutoPilot(Pilot): homography_matrix, grid_step=85) + self.timer.start() + # Пытаемся найти ориентир на картинке: self.prev_image = current_image npos = self.get_position_by_chunk() @@ -420,7 +445,7 @@ class AutoPilot(Pilot): 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 @@ -432,7 +457,13 @@ class AutoPilot(Pilot): d_r = max(10, min(75., distance_to_target / 2)) 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): """Сбрасывает позицию и угол БПЛА""" diff --git a/main.py b/main.py index 763fb87..a2f8829 100644 --- a/main.py +++ b/main.py @@ -87,7 +87,7 @@ def main(): vis_manager = VisualizationManager() width, height = yandexMap.get_size() - print(width, height) + # print(width, height) points_coords = np.array(list(map(lambda p: [ (p[0] - points[0][0]) * width, (points[0][1] - p[1]) * height ], points))) @@ -104,10 +104,14 @@ def main(): vis_manager.set_target_points(points_coords) + proc_time = np.array([]) + for i in range(10000000000): print(f"Image #{i}") photo = simulator.get_photo() command = pilot.handle(photo) + + proc_time = np.append(proc_time, command.proccessing_time) # Save Image photo.save(Path('images') / f"photo_{i}.png") @@ -128,6 +132,9 @@ def main(): vis_manager.update_display() vis_manager.pause(0.2) + last_proc_times = proc_time[-10:] + # print("Average FPS:", 1 / last_proc_times.mean()) + vis_manager.show_final() if __name__ == "__main__": diff --git a/simulator.py b/simulator.py index 94d1b73..2c5653e 100644 --- a/simulator.py +++ b/simulator.py @@ -86,16 +86,16 @@ class Simulator: action.click_and_hold() 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) dx = math.cos(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) action.move_by_offset(-dx, dy) action.release() 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: png = self.yandexMap.driver.get_screenshot_as_png() @@ -120,7 +120,7 @@ class Simulator: signal = self.operatorPilot.act() if signal is None: self.mode = SimMode.AUTONOME - print("Режим возвращения домой!") + # print("Режим возвращения домой!") if self.mode == SimMode.AUTONOME: signal = self.autonomePilot.act()