Upgrade Ground Truth Schema (Critical for ADAS use-case)

Expanded object state to include physical (acceleration), geometric (bounding boxes),
and relative ADAS metrics (range, azimuth, closing velocity) for vehicles and pedestrians.

intel/context.md

@@ -135,17 +135,35 @@ Writes one frame's worth of data to disk each tick.
 {
   "frame_id": 82,
   "timestamp": 1234.56,
-  "scenario": "braking",
-  "brake_frame": 80,
-  "braked": true,
   "ego_pose": {"x": 12.3, "y": 4.5, "z": 0.0, "yaw": -91.2},
-  "camera": "frame_000082.png",
-  "radar":  "frame_000082.npy",
-  "lidar":  "frame_000082.npy",
-  "ground_truth": [{"id": 42, "x": ..., "speed": 8.3}]
+  "ground_truth": [
+    {
+      "id": 123,
+      "class": "vehicle",
+      "type": "vehicle.tesla.model3",
+      "transform": {"x": 10.5, "y": 2.1, "z": 0.5, "yaw": 90.0, ...},
+      "velocity": {"vx": 5.0, "vy": 0.0, "vz": 0.0, "speed": 5.0},
+      "acceleration": {"ax": 0.1, "ay": 0.0, "az": 0.0},
+      "bounding_box": {"l": 4.5, "w": 2.0, "h": 1.5},
+      "relative": {
+        "range": 15.2,
+        "azimuth": -2.5,
+        "closing_velocity": 1.2
+      }
+    }
+  ],
+  "scenario": "braking",
+  "brake_frame": 80
 }
 ```
 
+**ADAS Relative Metrics**:
+- `range`: Euclidean distance (m).
+- `azimuth`: Angle in ego-forward frame (degrees).
+- `closing_velocity`: Rate of approach (m/s). Positive means getting closer.
+
+**Scope**: Now tracks both `vehicle.*` and `walker.*` (pedestrians).
+
 **`extra_meta` pattern:** `save()` accepts `extra_meta: dict` which is merged into the record.
 Scenarios use `get_scenario_metadata()` to supply this — no recorder changes needed per scenario.
 

src/recorder.py

@@ -1,8 +1,11 @@
 import os
 import json
+import math
+import datetime
+from types import SimpleNamespace
+
 import numpy as np
 import cv2
-import datetime
 
 
 class Recorder:
@@ -68,46 +71,66 @@ class Recorder:
         # -------- EGO POSE --------
         transform = vehicle.get_transform()
 
-        # -------- GROUND TRUTH VEHICLES --------
+        # -------- GROUND TRUTH ACTORS --------
         world = vehicle.get_world()
-        actors = world.get_actors().filter("vehicle.*")
+        # Track both vehicles and pedestrians
+        actors = list(world.get_actors().filter("vehicle.*")) + \
+                 list(world.get_actors().filter("walker.*"))
 
-        gt_vehicles = []
+        gt_actors = []
+
+        ego_loc = transform.location
+        ego_vel = vehicle.get_velocity()
 
         for actor in actors:
             if actor.id == vehicle.id:
                 continue  # skip ego
 
+            # 1. Physical State
             t = actor.get_transform()
             v = actor.get_velocity()
-
-            speed = (v.x**2 + v.y**2 + v.z**2) ** 0.5
-
-            gt_vehicles.append({
-                "id": actor.id,
-                "x": t.location.x,
-                "y": t.location.y,
-                "z": t.location.z,
-                "vx": v.x,
-                "vy": v.y,
-                "vz": v.z,
-                "speed": speed
+            a = actor.get_acceleration()
+
+            # 2. Geometry
+            bb = actor.bounding_box
+            extent = bb.extent  # half-size
+
+            # 3. Relative Metrics (ADAS)
+            rel_metrics = self._calculate_relative_metrics(vehicle, actor)
+
+            gt_actors.append({
+                "id":    actor.id,
+                "class": self._get_actor_class(actor),
+                "type":  actor.type_id,
+                "transform": {
+                    "x": t.location.x, "y": t.location.y, "z": t.location.z,
+                    "yaw": t.rotation.yaw, "pitch": t.rotation.pitch, "roll": t.rotation.roll
+                },
+                "velocity": {
+                    "vx": v.x, "vy": v.y, "vz": v.z,
+                    "speed": math.sqrt(v.x**2 + v.y**2 + v.z**2)
+                },
+                "acceleration": {
+                    "ax": a.x, "ay": a.y, "az": a.z
+                },
+                "bounding_box": {
+                    "l": extent.x * 2, "w": extent.y * 2, "h": extent.z * 2
+                },
+                "relative": rel_metrics
             })
 
         # -------- RECORD --------
         record = {
-            "frame_id": self.frame_id,
+            "frame_id":  self.frame_id,
             "timestamp": cam.timestamp,
             "ego_pose": {
-                "x": transform.location.x,
-                "y": transform.location.y,
-                "z": transform.location.z,
+                "x": transform.location.x, "y": transform.location.y, "z": transform.location.z,
                 "yaw": transform.rotation.yaw
             },
             "camera": cam_file,
-            "radar": radar_file,
-            "lidar": lidar_file,
-            "ground_truth": gt_vehicles,
+            "radar":  radar_file,
+            "lidar":  lidar_file,
+            "ground_truth": gt_actors,
         }
 
         # Merge scenario metadata (e.g. {"scenario": "braking", "brake_frame": 80})
@@ -120,3 +143,72 @@ class Recorder:
     # ---------------- CLEANUP ----------------
     def close(self):
         self.meta_f.close()
+
+    # ---------------- HELPERS ----------------
+    def _get_actor_class(self, actor) -> str:
+        """Categorise actor into broad ADAS classes."""
+        type_id = actor.type_id
+        if "walker" in type_id:
+            return "pedestrian"
+        if "vehicle" in type_id:
+            if "bicycle" in type_id or "motorcycle" in type_id:
+                return "vru"  # Vulnerable Road User
+            return "vehicle"
+        return "unknown"
+
+    def _calculate_relative_metrics(self, ego, npc) -> dict:
+        """
+        Calculate relative range, azimuth, and closing velocity.
+        Uses right-handed coordinate projections where necessary.
+        """
+        e_t = ego.get_transform()
+        n_t = npc.get_transform()
+        e_v = ego.get_velocity()
+        n_v = npc.get_velocity()
+
+        # 1. Range (Euclidean distance)
+        rel_pos_v = n_t.location - e_t.location
+        rng = rel_pos_v.length()
+
+        if rng < 0.1:  # avoid div by zero
+            return {"range": rng, "azimuth": 0.0, "closing_velocity": 0.0}
+
+        # 2. Azimuth (Local angle in degrees)
+        # Project relative position into ego's local frame
+        # CARLA forward is X+, Right is Y+ (left-handed)
+        forward = e_t.get_forward_vector()
+        
+        # Simple dot-product for angle (cosine similarity)
+        # We use atan2 for full 360 bearing
+        # rel_pos_v in ego local coordinates
+        rel_pos_local = self._to_local_location(e_t, n_t.location)
+        azimuth = math.degrees(math.atan2(rel_pos_local.y, rel_pos_local.x))
+
+        # 3. Closing Velocity (V_c)
+        # Projection of relative velocity onto the unit line-of-sight vector
+        # V_c = -(V_npc - V_ego) . (P_npc - P_ego) / |P_npc - P_ego|
+        rel_vel = n_v - e_v
+        unit_los = rel_pos_v / rng
+        closing_vel = -(rel_vel.x * unit_los.x + rel_vel.y * unit_los.y + rel_vel.z * unit_los.z)
+
+        return {
+            "range":            round(rng, 3),
+            "azimuth":          round(azimuth, 3),
+            "closing_velocity": round(closing_vel, 3)
+        }
+
+    def _to_local_location(self, transform, location):
+        """Helper to convert world location to transform-local location."""
+        # Simple translation and rotation inverse
+        # CARLA locations also have raw_data but subtraction is easier
+        rel_loc = location - transform.location
+        
+        # Inverse rotation (Yaw only for simplicity of horizontal metrics)
+        yaw_rad = math.radians(transform.rotation.yaw)
+        c, s = math.cos(yaw_rad), math.sin(yaw_rad)
+        
+        # Rotate rel_loc vector by -yaw
+        lx =  rel_loc.x * c + rel_loc.y * s
+        ly = -rel_loc.x * s + rel_loc.y * c
+        
+        return SimpleNamespace(x=lx, y=ly, z=rel_loc.z)