feat(radar): Implement Radar Metrology Suite (Iteration 18)

-  Engine: Modified CFAR and Processor to extract raw Range-Doppler and Range-Azimuth energy heatmaps.
-  Visuals: Integrated high-fidelity Matplotlib colormapping (Viridis/Magma) for Foxglove image streaming.
-  Data: Implemented 32-bit raw .npy persistence and JSONL telemetry for frame-level SNR analysis.
-  Tools: Added a dedicated verification utility for end-to-end signal flow validation.
-  Docs: Comprehensive documentation for the new 'Radar Lab' architecture in /intel/.

intel/radar/metrology_suite/README.md

@@ -0,0 +1,61 @@
+# Radar Metrology Suite: Implementation Hub 🛰️
+
+This directory serves as the source of truth for the **C-SHENRON Radar Metrology Suite**. The goal of this suite is to transform the physics-based radar simulation from a "Black Box" into a transparent "Radar Lab" environment.
+
+---
+
+## 🎯 1. The Core Objective
+
+To provide radar application engineers with 100% visibility into the signal processing chain. This is achieved by extracting and visualizing the internal energy states of the simulation before they are filtered into discrete points.
+
+### Key Deliverables:
+1.  **Visual Heatmaps:** Real-time Range-Doppler (RD) and Range-Azimuth (RA) streams in Foxglove.
+2.  **CFAR Transparency:** A visual mask of the adaptive threshold plane.
+3.  **Metrology Persistence:** Raw `.npy` storage of all FFT buffers for offline validation.
+4.  **Signal Telemetry:** JSON-based SNR and Noise Floor tracking.
+
+---
+
+## 🏗️ 2. Technical Architecture
+
+The suite hooks into the **ISOLATE** engine at the following points:
+
+### A. Signal Generation (`heatmap_gen_fast.py`)
+Current state: Successfully converts LiDAR points into complex ADC time-series.
+*   **Future Goal:** Implement Multi-Path interference logic.
+
+### B. Signal Processing (`radar_processor.py` & `cfar_detector.py`)
+Current state: Performs 2D FFT and CA-CFAR detection.
+*   **Modification Plan:** 
+    -   Retain the **3D FFT Cube** (Range x Doppler x Angle).
+    -   Extract the **Threshold Matrix** from `CA_CFAR.__call__`.
+    -   Compute global **Range-Azimuth** energy maps via mean-Doppler reduction.
+
+---
+
+## 🗺️ 3. Active Implementation Roadmap
+
+### Phase 1: Engine Heatmap Extraction (IN PROGRESS)
+- [ ] Modify `cfar_detector.py` to return the `rd_avg_noise_power` (Threshold Baseline).
+- [ ] Update `radar_processor.py` to capture RD and compute global RA heatmaps.
+- [ ] Update `model_wrapper.py` to expose heatmaps and signal telemetry.
+
+### Phase 2: Signal-to-Visual Pipeline (PENDING)
+- [ ] Implement 8-bit normalization and Viridis colormapping for radar image topics.
+- [ ] Update `test_shenron.py` to register new `/heatmaps/` image channels in MCAP.
+- [ ] Add JSON telemetry packaging for SNR/Noise metrics.
+
+### Phase 3: Raw Data Persistence (PENDING)
+- [ ] Create `metrology/rd`, `metrology/ra`, and `metrology/cfar` directory structure.
+- [ ] Implement `.npy` serialization for every frame during the simulation loop.
+
+---
+
+## 🧭 4. Pointers for Future Agents
+
+*   **Coordinate Frame:** Always remember: `Index 0 = Side (Y)`, `Index 1 = Forward (X)`.
+*   **Normalization:** When converting raw FFT magnitudes to images, use a log-dB scale to preserve dynamic range.
+*   **Performance:** Keep the `signal.convolve2d` calls optimized; we must maintain at least 1.0 FPS for UX.
+
+---
+*Created by Antigravity | Project: Fox CARLA ADAS | 2026-04-07*

scripts/ISOLATE/model_wrapper.py

@@ -50,6 +50,9 @@ class ShenronRadarModel:
             'RADAR_MOVING': False
         }
         
+        # Internal buffer for raw metrology (Heatmaps, SNR, etc.)
+        self.last_metrology = {}
+
     def _sync_configs(self):
         """Important: Sync global variables in sim_radar_utils to match current radar.obj"""
         import sim_radar_utils.utils_radar as ur
@@ -108,8 +111,12 @@ class ShenronRadarModel:
             
             # 4. Target Detection and Rich Parameter Extraction
             # CFAR detection + Angle of Arrival (AoA) estimation
-            # returns: rangeAoA, pointcloud ([x, y, z, vel, mag])
-            _, rich_pcd = self.processor.convert_to_pcd(doppler_profile)
+            # returns: rangeAoA, pointcloud ([x, y, z, vel, mag]), metrology dict
+            _, rich_pcd, metrology = self.processor.convert_to_pcd(doppler_profile)
+            
+            # 5. Capture Advanced Metrology
+            # Calculate SNR and basic noise stats for the Frame Metrics
+            self.last_metrology = metrology
             
             return rich_pcd
             
@@ -119,6 +126,40 @@ class ShenronRadarModel:
             traceback.print_exc()
             return np.empty((0, 5))
 
+    def get_last_metrology(self):
+        """
+        Return the raw internal heatmaps and thresholds for the last processed frame.
+        
+        Returns:
+            dict: {
+                'rd_heatmap': np.ndarray,
+                'ra_heatmap': np.ndarray,
+                'threshold_matrix': np.ndarray
+            }
+        """
+        return self.last_metrology
+
+    def get_signal_metrics(self):
+        """
+        Calculates frame-level signal-to-noise ratio and noise floor metadata.
+        """
+        if not self.last_metrology:
+            return {}
+            
+        rd = self.last_metrology['rd_heatmap']
+        noise = self.last_metrology['threshold_matrix']
+        
+        peak_mag = np.max(rd)
+        avg_noise = np.mean(noise)
+        snr = 10 * np.log10(peak_mag / avg_noise) if avg_noise > 0 else 0
+        
+        return {
+            "peak_magnitude": float(peak_mag),
+            "avg_noise_floor": float(avg_noise),
+            "peak_snr_db": float(snr),
+            "active_bins": int(np.sum(rd > avg_noise))
+        }
+
 if __name__ == "__main__":
     # Internal test/demo
     model = ShenronRadarModel()

scripts/ISOLATE/sim_radar_utils/cfar_detector.py

@@ -48,23 +48,23 @@ class CA_CFAR():
         Description:
         ------------
             Performs the automatic detection on the input range-Doppler matrix.
-        Implementation notes:
-        ---------------------
-        Parameters:
-        -----------
-        :param rd_matrix: Range-Doppler map on which the automatic detection should be performed
-        :type rd_matrix: R x D complex numpy array
         Return values:
         --------------
         :return hit_matrix: Calculated hit matrix
+        :return detection_gate: Calculated active detection gate (noise floor * threshold gain)
         """
-        # Convert range-Doppler map values to power
-        rd_matrix = np.abs(rd_matrix) ** 2
+        # Convert range-Doppler map values to power if complex
+        if np.iscomplexobj(rd_matrix):
+            rd_matrix = np.abs(rd_matrix) ** 2
 
         # Perform detection
         rd_windowed_sum = signal.convolve2d(rd_matrix, self.mask, mode='same')
         rd_avg_noise_power = rd_windowed_sum / self.num_valid_cells_in_window
-        rd_snr = rd_matrix / rd_avg_noise_power
-        hit_matrix = rd_snr > self.threshold
         
-        return hit_matrix
+        # Effective detection gate (threshold plane)
+        detection_gate = rd_avg_noise_power * self.threshold
+        
+        # Binary detection based on adaptive threshold
+        hit_matrix = rd_matrix > detection_gate
+
+        return hit_matrix, detection_gate

scripts/ISOLATE/sim_radar_utils/radar_processor.py

@@ -58,11 +58,20 @@ class RadarProcessor:
     def convert_to_pcd(self, dopplerProfile):
         avgDopplerProfile = np.squeeze(np.mean(dopplerProfile, 2))
         
-        # detect useful peaks using CFAR
-        detections = self.cfar(np.square(np.abs(avgDopplerProfile)))
+        # Capture RD Heatmap (Raw Power)
+        rd_heatmap = np.square(np.abs(avgDopplerProfile))
+
+        # detect useful peaks using CFAR (NOW RETURNING THRESHOLD BASELINE)
+        hit_matrix, threshold_matrix = self.cfar(rd_heatmap)
+
+        # Global Range-Azimuth Heatmap Calculation
+        # Compute mean across Doppler bins and then perform Angle FFT
+        ra_heatmap_raw = np.mean(dopplerProfile, axis=1) # Shape: (Range, Antenna)
+        ra_heatmap_complex = fftshift(fft(ra_heatmap_raw, fftCfg['NFFTAnt'], 1), 1)
+        ra_heatmap = np.square(np.abs(ra_heatmap_complex))
 
         # identify range bin and velocity bin for each detected point
-        rowSel, colSel = np.nonzero(detections)
+        rowSel, colSel = np.nonzero(hit_matrix)
 
         # pointSel = np.zeros(shape=(len(rowSel), len(radarCfg['AntIdx'])), dtype=complex)
         pointSel = np.zeros(shape=(len(rowSel), radarCfg['NrChn']), dtype=complex)
@@ -91,4 +100,11 @@ class RadarProcessor:
 
         pointcloud = np.stack([x, y, z, velVals, magVals], axis=1)
 
-        return rangeAoA, pointcloud
+        # Build Metrology Dictionary
+        metrology = {
+            "rd_heatmap": rd_heatmap,
+            "ra_heatmap": ra_heatmap,
+            "threshold_matrix": threshold_matrix
+        }
+
+        return rangeAoA, pointcloud, metrology

scripts/analysis/verify_metrology_logic.py

@@ -0,0 +1,88 @@
+import numpy as np
+import os
+import sys
+from pathlib import Path
+
+# Add project root and ISOLATE paths
+project_root = Path(__file__).parent.parent.parent
+sys.path.append(str(project_root))
+sys.path.append(str(project_root / 'scripts' / 'ISOLATE'))
+
+try:
+    from scripts.ISOLATE.model_wrapper import ShenronRadarModel
+    print("✅ Model Import: SUCCESS")
+except Exception as e:
+    print(f"❌ Model Import: FAILED ({e})")
+    sys.exit(1)
+
+def verify_signal_flow(frame_idx=190):
+    print(f"\n--- 🛰️ Radar Lab Verification (Frame {frame_idx}) ---")
+    
+    # 1. Initialize Engine
+    try:
+        model = ShenronRadarModel(radar_type='awrl1432')
+        print(f"✅ Engine Init: awrl1432 (Gain: {model.radar_obj.gain}dB)")
+    except Exception as e:
+        print(f"❌ Engine Init: FAILED ({e})")
+        return
+
+    # 2. Load Real LiDAR Data
+    lidar_path = project_root / 'Shenron_debug' / 'logs' / 'lidar' / f"frame_{frame_idx:06d}.npy"
+    if not lidar_path.exists():
+        print(f"❌ Lidar Data: NOT FOUND at {lidar_path}")
+        return
+    
+    data = np.load(lidar_path)
+    # Pad to 7-col [x,y,z,int,cos,obj,tag] if old 6-col format
+    if data.shape[1] == 6:
+        padded = np.zeros((data.shape[0], 7), dtype=np.float32)
+        padded[:, 0:3] = data[:, 0:3]
+        padded[:, 4:7] = data[:, 3:6]
+        data = padded
+    print(f"✅ Lidar Data Loaded: {data.shape[0]} points")
+
+    # 3. Process Frame
+    try:
+        pcd = model.process(data)
+        print(f"✅ Signal Process: SUCCESS ({len(pcd)} detections)")
+    except Exception as e:
+        print(f"❌ Signal Process: FAILED ({e})")
+        return
+
+    # 4. Verify Metrology Extraction
+    try:
+        met = model.get_last_metrology()
+        keys = met.keys()
+        expected = ["rd_heatmap", "ra_heatmap", "threshold_matrix"]
+        
+        status = True
+        for k in expected:
+            if k not in keys:
+                print(f"❌ Metrology Key Missing: {k}")
+                status = False
+            else:
+                val = met[k]
+                if np.max(val) <= 0:
+                    print(f"❌ Metrology Data Null: {k} (Max: {np.max(val)})")
+                    status = False
+        
+        if status:
+            print("✅ Metrology Extraction: ALL KEYS VALID")
+            print(f"   -> RD Heatmap: {met['rd_heatmap'].shape} (Peak Power: {np.max(met['rd_heatmap']):.2e})")
+            print(f"   -> RA Heatmap: {met['ra_heatmap'].shape} (Peak Power: {np.max(met['ra_heatmap']):.2e})")
+            print(f"   -> CFAR Threshold: {met['threshold_matrix'].shape} (Mean: {np.mean(met['threshold_matrix']):.2e})")
+            
+    except Exception as e:
+        print(f"❌ Metrology Extraction: FAILED ({e})")
+
+    # 5. Verify Signal Metrics
+    try:
+        metrics = model.get_signal_metrics()
+        print(f"✅ Signal Metrics: {metrics}")
+        if metrics['peak_snr_db'] <= 0:
+            print("⚠️ WARNING: Very low SNR detected (< 0dB). Check gain.")
+    except Exception as e:
+        print(f"❌ Signal Metrics: FAILED ({e})")
+
+if __name__ == "__main__":
+    verify_signal_flow(190)

scripts/test_shenron.py

@@ -6,6 +6,11 @@ from pathlib import Path
 import json
 import base64
 import argparse
+import matplotlib
+import matplotlib.pyplot as plt
+import matplotlib.cm as cm
+import io
+from PIL import Image
 from mcap.writer import Writer
 
 # Add project root and ISOLATE paths
@@ -59,6 +64,43 @@ FOXGLOVE_PCL_SCHEMA = {
         "data": {"type": "string", "contentEncoding": "base64"}
     }
 }
+FOXGLOVE_METRICS_SCHEMA = {
+    "$schema": "https://json-schema.org/draft/2020-12/schema",
+    "$id": "foxglove.Telemetry",
+    "title": "foxglove.Telemetry",
+    "type": "object",
+    "properties": {
+        "timestamp": {"type": "object", "properties": {"sec": {"type": "integer"}, "nsec": {"type": "integer"}}},
+        "frame_id": {"type": "string"},
+        "metrics": {"type": "object", "additionalProperties": {"type": "number"}}
+    }
+}
+
+def render_heatmap(data, vmin=None, vmax=None, cmap='viridis'):
+    """Converts a 2D numpy array to a colormapped PNG base64 string."""
+    if data is None or data.size == 0:
+        return None
+    
+    # Simple log scaling if needed? For now we assume input is power or magnitude
+    # Normalize to 0-1
+    if vmin is None: vmin = np.min(data)
+    if vmax is None: vmax = np.max(data)
+    
+    if vmax > vmin:
+        norm_data = (data - vmin) / (vmax - vmin)
+    else:
+        norm_data = np.zeros_like(data)
+    
+    # Apply colormap (Updated to use modern matplotlib.colormaps API)
+    color_mapped = matplotlib.colormaps[cmap](norm_data) # [H, W, 4]
+    
+    # Convert to 8-bit RGB
+    rgb = (color_mapped[:, :, :3] * 255).astype(np.uint8)
+    
+    img = Image.fromarray(rgb)
+    buffered = io.BytesIO()
+    img.save(buffered, format="PNG")
+    return base64.b64encode(buffered.getvalue()).decode("ascii")
 
 def load_frames(folder_path):
     with open(os.path.join(folder_path, "frames.jsonl")) as f:
@@ -95,6 +137,12 @@ def run_testbench(iter_name):
             print(f"  -> Initializing {r_type} engine...")
             models[r_type] = ShenronRadarModel(radar_type=r_type)
             (iter_dir / r_type).mkdir(exist_ok=True)
+            
+            # Create Metrology folders
+            met_base = iter_dir / r_type / "metrology"
+            for sub in ["rd", "ra", "cfar"]:
+                (met_base / sub).mkdir(parents=True, exist_ok=True)
+                
         except Exception as e:
             print(f"  -> [WARNING] Failed to init {r_type}: {e}")
             
@@ -113,6 +161,20 @@ def run_testbench(iter_name):
                 rich_pcd = model.process(data)
                 out_path = iter_dir / r_type / lidar_file.name
                 np.save(out_path, rich_pcd)
+                
+                # --- PHASES 1 & 3: Save Raw Metrology (.npy) ---
+                met = model.get_last_metrology()
+                if met:
+                    frame_name = lidar_file.stem # e.g., frame_000200
+                    np.save(iter_dir / r_type / "metrology" / "rd" / f"{frame_name}.npy", met['rd_heatmap'])
+                    np.save(iter_dir / r_type / "metrology" / "ra" / f"{frame_name}.npy", met['ra_heatmap'])
+                    np.save(iter_dir / r_type / "metrology" / "cfar" / f"{frame_name}.npy", met['threshold_matrix'])
+                    
+                    # Log Metrics
+                    metrics = model.get_signal_metrics()
+                    with open(iter_dir / r_type / "metrology" / "metrics.jsonl", "a") as mf:
+                        mf.write(json.dumps({"frame": frame_name, **metrics}) + "\n")
+                        
             except Exception as e:
                 print(f"[ERROR] Frame {lidar_file.name} failed for {r_type}: {e}")
 
@@ -139,9 +201,18 @@ def run_testbench(iter_name):
         }
         
         shenron_channels = {}
+        metrology_channels = {}
         for r_type in radar_types:
             shenron_channels[r_type] = writer.register_channel(topic=f"/radar/{r_type}", message_encoding="json", schema_id=lidar_schema_id)
             
+            # Register Metrology Channels
+            metrology_channels[r_type] = {
+                "rd": writer.register_channel(topic=f"/radar/{r_type}/heatmaps/range_doppler", message_encoding="json", schema_id=camera_schema_id),
+                "ra": writer.register_channel(topic=f"/radar/{r_type}/heatmaps/range_azimuth", message_encoding="json", schema_id=camera_schema_id),
+                "cfar": writer.register_channel(topic=f"/radar/{r_type}/heatmaps/cfar_mask", message_encoding="json", schema_id=camera_schema_id),
+                "telemetry": writer.register_channel(topic=f"/radar/{r_type}/metrics", message_encoding="json", schema_id=writer.register_schema(name="foxglove.Telemetry", encoding="jsonschema", data=json.dumps(FOXGLOVE_METRICS_SCHEMA).encode()))
+            }
+
         try:
             frames_gen = load_frames(logs_dir)
             frames = list(frames_gen)
@@ -258,6 +329,33 @@ def run_testbench(iter_name):
                         }
                         writer.add_message(shenron_channels[r_type], log_time=ts_ns, data=json.dumps(shenron_msg).encode(), publish_time=ts_ns)
 
+                    # --- PHASE 2: Stream Metrology Visuals ---
+                    met_folder = iter_dir / r_type / "metrology"
+                    rd_p = met_folder / "rd" / shenron_fname
+                    ra_p = met_folder / "ra" / shenron_fname
+                    cf_p = met_folder / "cfar" / shenron_fname
+                    
+                    if rd_p.exists():
+                        rd_data = np.load(rd_p)
+                        b64 = render_heatmap(np.log10(rd_data + 1e-9), cmap='viridis')
+                        if b64:
+                            msg = {"timestamp": {"sec": ts_sec, "nsec": ts_nsec}, "frame_id": "ego_vehicle", "format": "png", "data": b64}
+                            writer.add_message(metrology_channels[r_type]["rd"], log_time=ts_ns, data=json.dumps(msg).encode(), publish_time=ts_ns)
+
+                    if ra_p.exists():
+                        ra_data = np.load(ra_p)
+                        b64 = render_heatmap(np.log10(ra_data + 1e-9), cmap='magma') # Magma for top-down contrast
+                        if b64:
+                            msg = {"timestamp": {"sec": ts_sec, "nsec": ts_nsec}, "frame_id": "ego_vehicle", "format": "png", "data": b64}
+                            writer.add_message(metrology_channels[r_type]["ra"], log_time=ts_ns, data=json.dumps(msg).encode(), publish_time=ts_ns)
+                    
+                    if cf_p.exists():
+                        cf_data = np.load(cf_p)
+                        b64 = render_heatmap(np.log10(cf_data + 1e-9), cmap='plasma') # Plasma for threshold mask
+                        if b64:
+                            msg = {"timestamp": {"sec": ts_sec, "nsec": ts_nsec}, "frame_id": "ego_vehicle", "format": "png", "data": b64}
+                            writer.add_message(metrology_channels[r_type]["cfar"], log_time=ts_ns, data=json.dumps(msg).encode(), publish_time=ts_ns)
+
         writer.finish()
     print(f"\n[SUCCESS] Iteration packaged to: {output_mcap}")
     print(f"File size: {os.path.getsize(output_mcap)/1024/1024:.2f} MB")