ref: comment output

2025-12-19 22:35:49 +03:00
parent f0ac60c8ef
commit d04d4a0cdc
3 changed files with 64 additions and 26 deletions
--- 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):
        """Сбрасывает позицию и угол БПЛА"""
--- 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__":
--- 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()