Log Image ids

uavf_2025/gnc/commander_node.py

@@ -18,7 +18,6 @@
 from gnc.util import validate_points, read_gpx_file, calculate_alignment_yaw
 from gnc.geofence import Geofence
 
-from datetime import datetime
 from shared import ensure_ros
 
 from gnc.dropzone_planner import DropzonePlanner
@@ -65,11 +64,14 @@ def __init__(self, args=None, preflight=False, run_avoidance=False):
 
         # logging configs
         self.src_path = str(Path(__file__).absolute().parent.parent.parent.parent)
+        logs_base = Path(f"{self.src_path}/uavf_2025/logs")
+        year_month_day = strftime("%Y-%m-%d")
+        index = len(list((logs_base / year_month_day).glob("*-*_*")))
+        time_string = strftime("%H_%M")
+        self.logs_path = logs_base / year_month_day / f"{index}-{time_string}"
+        self.logs_path.mkdir(parents=True, exist_ok=True)
         logging.basicConfig(
-            filename=self.src_path
-            + "/uavf_2025/uavf_2025/gnc/logs/commander_node_{:%Y-%m-%d-%m-%s}.log".format(
-                datetime.now()
-            ),
+            filename=str(self.logs_path / "commander_node.log"),
             format="%(asctime)s %(message)s",
             encoding="utf-8",
             level=logging.DEBUG,
@@ -93,10 +95,6 @@ def __init__(self, args=None, preflight=False, run_avoidance=False):
         # initialize drone pose provider and wait for it to be ready
         # MUST BE BEFORE DROPZONE PLANNER AND HOME POSE INIT
         self.log("Creating drone pose provider...")
-        logs_base = f"{self.src_path}/uavf_2025/logs"
-        time_string = strftime("%Y-%m-%d_%H-%M")
-        self.logs_path = Path(f"{logs_base}/nvme/{time_string}")
-        self.logs_path.mkdir(parents=True, exist_ok=True)
         self._logger = create_console_logger(self.logs_path, "gnc")
 
         self.pose_provider = DronePoseProvider(

uavf_2025/perception/camera/camera.py

@@ -98,7 +98,6 @@ def __init__(self, log_dir: str | Path, max_threads: int = 2):
         self.log_dir = Path(log_dir)
         self._prep_log_dir(self.log_dir)
 
-        self.datum_id = 0
         self.pool = ThreadPoolExecutor(max_workers=max_threads)
 
     @staticmethod
@@ -114,17 +113,16 @@ def log_async(self, image: Image, metadata: ImageMetadata):
 
     def _log_to_file(self, image: Image, metadata: ImageMetadata):
         try:  # since this runs in a pool executor it won't log exceptions unless we do this
-            image.save(self.log_dir / f"{self.datum_id}.jpg")
+            image.save(self.log_dir)
 
             json.dump(
                 {
                     "timestamp": metadata.timestamp,
                     "focal_len_px": metadata.focal_len_px,
                     "relative_pose": metadata.relative_pose.to_dict(),
                 },
-                open(self.log_dir / f"{self.datum_id}.json", "w"),
+                open(self.log_dir / f"{image._id}.json", "w"),
             )
-            self.datum_id += 1
         except Exception:
             traceback.print_exc()
 

uavf_2025/perception/odlc/target_tracking.py

@@ -1,6 +1,5 @@
 from __future__ import annotations
 from pathlib import Path
-from time import time
 import json
 
 import numpy as np
@@ -156,46 +155,5 @@ def update_with_detections(self, image, detections, pose):
                     # create new track
                     self._tracks.append(TargetTracker.Track(det, det_pos, det.det_id))
 
-        self._log_tracks()
-        self._log_detections(detections)
-
-    def _log_tracks(self) -> None:
-        if self._track_logs_path is None:
-            return
-        json.dump(
-            [
-                {
-                    "id": track.id,
-                    "position": track.position.tolist(),
-                    "probs": list(track.probs),
-                    "contributing_detections": track.contributing_detections,
-                }
-                for track in self._tracks
-            ],
-            open(self._track_logs_path / f"{time()}.json", "w"),
-        )
-        self._track_log_id += 1
-
-    def _log_detections(self, detections: list[TargetBBox]) -> None:
-        if self._detection_logs_path is None:
-            return
-        json.dump(
-            [
-                {
-                    "bbox": [
-                        int(detection.bbox.x),
-                        int(detection.bbox.y),
-                        int(detection.bbox.width),
-                        int(detection.bbox.height),
-                    ],
-                    "cls_probs": list(detection.cls_probs),
-                    "img_id": detection.img_id,
-                    "det_id": detection.det_id,
-                }
-                for detection in detections
-            ],
-            open(self._detection_logs_path / f"{time()}.json", "w"),
-        )
-
     def get_tracks(self) -> list[Track]:
         return self._tracks

uavf_2025/perception/perception.py

@@ -1,4 +1,5 @@
 import threading
+import json
 from enum import Enum
 from pathlib import Path
 from time import sleep, time
@@ -79,6 +80,9 @@ def __init__(
         self._do_record = do_record
         self._frames_since_autofocus = 0
         logs_path.mkdir(parents=True, exist_ok=True)
+        self.logs_path = logs_path
+        self.odlc_logs_path = logs_path / "odlc"
+        self.odlc_logs_path.mkdir(parents=True, exist_ok=True)
 
         self._logger = create_console_logger(logs_path, "perception")
         git_branch, git_commit = get_git_info()
@@ -99,10 +103,7 @@ def __init__(
         # )
         self._backup_camera = None
         # TODO: replace Pose.identity() here with actual offset of camera on the drone frame
-        self._tracker = TargetTracker(
-            track_logs_path=logs_path / "tracking",
-            detection_logs_path=logs_path / "detection",
-        )
+        self._tracker = TargetTracker()
         self._gimbal_flip_state: Perception.GimbalFlipState | None = None
         self._mode = Perception.Mode.IDLE
         self._drone_pose_provider = (
@@ -210,6 +211,10 @@ def loop_iter(self):
         else:
             img = self._camera.take_image()
 
+        if img is not None:
+            pass
+            # self._logger.debug(f"Image {img._id}")
+
         # TODO: un-comment this when we get calibration params for the backup cam.
         # img_backup = None
         if self._backup_camera is not None:
@@ -239,12 +244,46 @@ def loop_iter(self):
         if self._mode in (Perception.Mode.AREA_SCAN, Perception.Mode.TARGET_LOCK):
             if img is not None:
                 cam_pose = self._camera.get_world_pose(drone_pose)
-                self._tracker.update(img, cam_pose)
+                detections = self._tracker._detector.detect(img)
+                self._tracker.update_with_detections(img, detections, cam_pose)
                 tracks = self._tracker.get_tracks()
                 self._logger.info(f"{len(tracks)} tracks")
                 for t in self._tracker.get_tracks():
                     self._logger.debug(t)
 
+                track_info = [
+                    {
+                        "id": track.id,
+                        "position": track.position.tolist(),
+                        "probs": list(track.probs),
+                        "contributing_detections": track.contributing_detections,
+                    }
+                    for track in tracks
+                ]
+                detection_info = [
+                    {
+                        "bbox": [
+                            int(detection.bbox.x),
+                            int(detection.bbox.y),
+                            int(detection.bbox.width),
+                            int(detection.bbox.height),
+                        ],
+                        "cls_probs": list(detection.cls_probs),
+                        "img_id": detection.img_id,
+                        "det_id": detection.det_id,
+                    }
+                    for detection in detections
+                ]
+                with open(self.odlc_logs_path / f"{img._id}.json", "w+") as f:
+                    json.dump(
+                        {
+                            "tracks": track_info,
+                            "detections": detection_info,
+                            "cam_pose": cam_pose.to_json(),
+                        },
+                        f,
+                    )
+
         if self._mode == Perception.Mode.TARGET_LOCK:
             if self.current_target_id:
                 track = self.get_target_positions()[self.current_target_id]
@@ -300,6 +339,8 @@ def set_area_scan(self, roi_local_coords: np.ndarray, img=None):
 
     def set_target_lock(self, target_id: int):
         self._logger.info("Set Perception mode to target lock")
+        self._camera.point_center()
+        sleep(3)
         self._mode = Perception.Mode.TARGET_LOCK
         self.current_target_id = target_id
 

uavf_2025/perception/types.py

@@ -49,6 +49,21 @@ def with_pose(self, pose: Pose) -> CameraPose:
     def as_pose(self) -> Pose:
         return Pose(self.position, self.rotation)
 
+    def to_json(self) -> dict:
+        return {
+            "position": self.position.tolist(),
+            "rotation": self.rotation.as_quat().tolist(),
+            "focal_len_px": self.focal_len_px,
+        }
+
+    @staticmethod
+    def from_json(data: dict) -> CameraPose:
+        return CameraPose(
+            np.array(data["position"]),
+            Rotation.from_quat(data["rotation"]),
+            data["focal_len_px"],
+        )
+
 
 class Bbox2D(NamedTuple):
     """
@@ -123,6 +138,8 @@ class ImageDimensionsOrder(NamedTuple):
 
 _UnderlyingImageT = TypeVar("_UnderlyingImageT", np.ndarray, torch.Tensor)
 
+_id = 0
+
 
 class Image(Generic[_UnderlyingImageT]):
     """
@@ -163,6 +180,9 @@ def __init__(self, array: _UnderlyingImageT, dim_order: ImageDimensionsOrder = H
             )
 
         self._array: _UnderlyingImageT = array
+        global _id
+        self._id = _id
+        _id += 1
 
     def __getitem__(self, key):
         return self._array[key]
@@ -298,7 +318,7 @@ def save(self, fp: os.PathLike | str) -> None:
         if self._dim_order == CHW:
             np_array = np_array.transpose(1, 2, 0)
 
-        cv2.imwrite(str(fp), np_array)
+        cv2.imwrite(str(fp) + f"/{self._id}.jpg", np_array)
 
     def change_dim_order(self, target_dim_order: ImageDimensionsOrder) -> None:
         """

uavf_2025/shared/pdb_monitor.py

@@ -1,26 +1,31 @@
-from pathlib import Path
-from datetime import datetime
 import threading
 import time
 
 from shared.pdb_linker import PDBLinker
 from perception.lib.util import create_console_logger
 
+from typing import TYPE_CHECKING, Any
+
+if TYPE_CHECKING:  # Prevents circular import
+    from gnc.commander_node import (
+        CommanderNode,
+    )
+else:  # Shouldn't be necessary but we get a runtime error without doing this
+    CommanderNode = Any
+
+
 PRINT_FREQ = 5  # Frequency to print messages in seconds
 
 
 class PDBMonitor:
-    def __init__(self, commander_node):
+    def __init__(self, commander_node: CommanderNode):
         commander_node.log("pdbmonitor starting...")
         self.commander_node = commander_node
 
         # logging configs
-        src_path = Path(__file__).absolute().parent.parent.parent.parent
-        self.filepath = src_path / "uavf_2025/uavf_2025/shared/logs"
+        logs_path = self.commander_node.logs_path
 
-        self._logger = create_console_logger(
-            self.filepath, "pdb_logs_{:%Y-%m-%d-%m-%s}".format(datetime.now()), False
-        )
+        self._logger = create_console_logger(logs_path, "pdb_logs", False)
 
         self.linker = PDBLinker(self._logger)