Aruco Code and UVG/UAV Logic Fixes

src/utils/recorder.py

@@ -1,21 +1,37 @@
 #!/usr/bin/env python3
+"""Run recorder — captures simulation video and logs to a results folder.
+
+Each run gets a folder like results/run_1/ containing:
+  - log.txt              Process logs (stdout/stderr)
+  - flight_path.avi      Flight tracker video
+  - flight_path.png      Final flight path image
+  - camera.avi           Downward camera video
+  - camera_*.png         Camera snapshots at key moments
+  - gazebo.avi           Gazebo simulation window recording (via xwd)
+"""
 
 import os
 import sys
 import io
-import json
 import time
 import threading
 import cv2
 import numpy as np
+import subprocess
+import re
 from pathlib import Path
 from datetime import datetime
 
+try:
+    import PIL.Image
+    HAS_PIL = True
+except ImportError:
+    HAS_PIL = False
 
 class RunRecorder:
     def __init__(self, results_dir=None, fps=5):
         if results_dir is None:
-            project_dir = Path(__file__).resolve().parent.parent
+            project_dir = Path(__file__).resolve().parent.parent.parent
             results_dir = project_dir / "results"
 
         results_dir = Path(results_dir)
@@ -47,11 +63,15 @@ class RunRecorder:
 
         self._tracker_writer = None
         self._camera_writer = None
+        self._gazebo_writer = None
+        
         self._tracker_size = None
         self._camera_size = None
+        self._gazebo_size = None
 
         self._tracker_frames = 0
         self._camera_frames = 0
+        self._gazebo_frames = 0
 
         self._last_tracker_frame = None
         self._last_camera_frame = None
@@ -63,6 +83,10 @@ class RunRecorder:
         self._record_thread = None
         self._lock = threading.Lock()
 
+        # Gazebo window capture via xwd
+        self._gazebo_wid = None
+        self._find_gazebo_window()
+
         self.metadata = {
             "run": run_num,
             "start_time": datetime.now().isoformat(),
@@ -71,6 +95,37 @@ class RunRecorder:
 
         print(f"[REC] Recording to: {self.run_dir}")
 
+    def _find_gazebo_window(self):
+        """Attempts to find the Gazebo window ID using xwininfo."""
+        try:
+            # Find window ID
+            cmd = "xwininfo -root -tree | grep -i 'gazebo\|gz sim'"
+            output = subprocess.check_output(cmd, shell=True).decode('utf-8')
+            lines = output.strip().split('\n')
+            if not lines:
+                print("[REC] Gazebo window not found")
+                return
+
+            # Pick the first one
+            wid_line = lines[0]
+            wid_match = re.search(r'(0x[0-9a-fA-F]+)', wid_line)
+            if not wid_match:
+                return
+            self._gazebo_wid = wid_match.group(1)
+            print(f"[REC] Found Gazebo window ID: {self._gazebo_wid}")
+
+        except Exception as e:
+            print(f"[REC] Error finding Gazebo window: {e}")
+
+    def set_phase(self, phase: str):
+        # No-op since CSV logging is removed
+        pass
+
+    def log_position(self, uav_x=0, uav_y=0, uav_alt=0, uav_heading=0,
+                     ugv_x=0, ugv_y=0):
+        # No-op since CSV logging is removed
+        pass
+
     def start_logging(self):
         sys.stdout = self._tee_stdout
         sys.stderr = self._tee_stderr
@@ -83,6 +138,11 @@ class RunRecorder:
         self._tracker_ref = tracker
         self._camera_ref = camera
         self._recording = True
+        
+        # Re-check Gazebo window
+        if not self._gazebo_wid:
+             self._find_gazebo_window()
+
         self._record_thread = threading.Thread(
             target=self._record_loop, daemon=True
         )
@@ -111,6 +171,33 @@ class RunRecorder:
                 except Exception:
                     pass
 
+            # Gazebo Capture via xwd
+            if self._gazebo_wid and HAS_PIL:
+                try:
+                    # Capture window to stdout using xwd
+                    proc = subprocess.run(
+                        ['xwd', '-id', self._gazebo_wid, '-out', '/dev/stdout'],
+                        stdout=subprocess.PIPE,
+                        stderr=subprocess.PIPE
+                    )
+                    if proc.returncode == 0:
+                        # Parse XWD using PIL
+                        img_pil = PIL.Image.open(io.BytesIO(proc.stdout))
+                        # Convert to OpenCV (RGB -> BGR)
+                        img_np = np.array(img_pil)
+                        # PIL opens XWD as RGB usually, check mode
+                        if img_pil.mode == 'RGB':
+                            img_bgr = cv2.cvtColor(img_np, cv2.COLOR_RGB2BGR)
+                        elif img_pil.mode == 'RGBA':
+                            img_bgr = cv2.cvtColor(img_np, cv2.COLOR_RGBA2BGR)
+                        else:
+                            img_bgr = cv2.cvtColor(img_np, cv2.COLOR_RGB2BGR) # Fallback
+
+                        self._write_gazebo_frame(img_bgr)
+                except Exception:
+                    # xwd failed or window closed
+                    pass
+
             elapsed = time.time() - t0
             sleep_time = max(0, interval - elapsed)
             time.sleep(sleep_time)
@@ -135,6 +222,25 @@ class RunRecorder:
         self._camera_writer.write(frame)
         self._camera_frames += 1
 
+    def _write_gazebo_frame(self, frame):
+        h, w = frame.shape[:2]
+        # Ensure dimensions are even
+        if w % 2 != 0: w -= 1
+        if h % 2 != 0: h -= 1
+        frame = frame[:h, :w]
+        
+        if self._gazebo_writer is None:
+            self._gazebo_size = (w, h)
+            fourcc = cv2.VideoWriter_fourcc(*'XVID')
+            path = str(self.run_dir / "gazebo.avi")
+            self._gazebo_writer = cv2.VideoWriter(path, fourcc, self.fps, (w, h))
+        elif (w, h) != self._gazebo_size:
+             # Handle resize? Just skip for now or resize
+             frame = cv2.resize(frame, self._gazebo_size)
+            
+        self._gazebo_writer.write(frame)
+        self._gazebo_frames += 1
+
     def snapshot_camera(self, label="snapshot"):
         if self._camera_ref is None:
             return
@@ -148,50 +254,20 @@ class RunRecorder:
         self._camera_snapshots.append(filename)
         print(f"[REC] Snapshot: {filename}")
 
+    def snapshot_tracker(self, label="tracker"):
+        """Save a snapshot of the current flight tracker."""
+        if self._last_tracker_frame is not None:
+            filename = f"tracker_{label}.png"
+            path = self.run_dir / filename
+            cv2.imwrite(str(path), self._last_tracker_frame)
+            print(f"[REC] Tracker snapshot: {filename}")
+
     def save_summary(self, search_mode="", altitude=0, markers=None,
-                     landed=False, extra=None):
-        duration = time.time() - self.start_time
-        mins = int(duration // 60)
-        secs = int(duration % 60)
-
-        summary = {
-            **self.metadata,
-            "end_time": datetime.now().isoformat(),
-            "duration_seconds": round(duration, 1),
-            "duration_display": f"{mins}m {secs}s",
-            "search_mode": search_mode,
-            "altitude": altitude,
-            "landed": landed,
-            "markers_found": {},
-            "recordings": {
-                "log": "log.txt",
-                "flight_path_video": "flight_path.avi",
-                "flight_path_image": "flight_path.png",
-                "camera_video": "camera.avi",
-                "camera_snapshots": self._camera_snapshots,
-            },
-            "frame_counts": {
-                "tracker": self._tracker_frames,
-                "camera": self._camera_frames,
-            },
-        }
-
-        if markers:
-            for mid, info in markers.items():
-                pos = info.get('uav_position', {})
-                summary["markers_found"][str(mid)] = {
-                    "x": round(pos.get('x', 0), 2),
-                    "y": round(pos.get('y', 0), 2),
-                    "distance": round(info.get('distance', 0), 2),
-                }
-
-        if extra:
-            summary.update(extra)
-
-        path = self.run_dir / "summary.json"
-        with open(path, "w") as f:
-            json.dump(summary, f, indent=2)
-        print(f"[REC] Summary saved: {path}")
+                     landed=False, ugv_dispatched=False,
+                     ugv_target=None, extra=None):
+        # Summary JSON file saving moved/removed as per request
+        # Still useful to see basic stats in terminal?
+        pass
 
     def stop(self):
         self._recording = False
@@ -211,6 +287,8 @@ class RunRecorder:
             self._tracker_writer.release()
         if self._camera_writer:
             self._camera_writer.release()
+        if self._gazebo_writer:
+            self._gazebo_writer.release()
 
         self.stop_logging()
         self._log_file.close()
@@ -220,10 +298,14 @@ class RunRecorder:
         secs = int(duration % 60)
 
         self._original_stdout.write(
-            f"\n[REC] Run recorded: {self.run_dir}\n"
-            f"[REC] Duration: {mins}m {secs}s  |  "
-            f"Tracker: {self._tracker_frames} frames  |  "
-            f"Camera: {self._camera_frames} frames\n"
+            f"\n[REC] ═══════════════════════════════════════\n"
+            f"[REC]   Run #{self.run_num} recorded\n"
+            f"[REC]   Dir: {self.run_dir}\n"
+            f"[REC]   Duration: {mins}m {secs}s\n"
+            f"[REC]   Tracker: {self._tracker_frames} frames\n"
+            f"[REC]   Camera:  {self._camera_frames} frames\n"
+            f"[REC]   Gazebo:  {self._gazebo_frames} frames\n"
+            f"[REC] ═══════════════════════════════════════\n"
         )