|
|
@ -1,7 +1,7 @@ |
|
|
import os |
|
|
import os |
|
|
import sys |
|
|
import sys |
|
|
|
|
|
import time |
|
|
import numpy as np |
|
|
import numpy as np |
|
|
import tqdm |
|
|
|
|
|
import json |
|
|
import json |
|
|
from pathlib import Path |
|
|
from pathlib import Path |
|
|
|
|
|
|
|
|
@ -17,6 +17,30 @@ except ImportError as e: |
|
|
print(f"Error: Failed to import ShenronRadarModel. Ensure scripts/ISOLATE/model_wrapper.py exists. ({e})") |
|
|
print(f"Error: Failed to import ShenronRadarModel. Ensure scripts/ISOLATE/model_wrapper.py exists. ({e})") |
|
|
sys.exit(1) |
|
|
sys.exit(1) |
|
|
|
|
|
|
|
|
|
|
|
def _get_gpu_info(): |
|
|
|
|
|
"""Retrieve GPU hardware info for telemetry display.""" |
|
|
|
|
|
try: |
|
|
|
|
|
import torch |
|
|
|
|
|
if not torch.cuda.is_available(): |
|
|
|
|
|
return {"name": "CPU Fallback", "vram_gb": 0, "backend": "CPU"} |
|
|
|
|
|
|
|
|
|
|
|
# Try to get device name safely |
|
|
|
|
|
try: |
|
|
|
|
|
name = torch.cuda.get_device_name(0) |
|
|
|
|
|
except: |
|
|
|
|
|
name = "NVIDIA Device" |
|
|
|
|
|
|
|
|
|
|
|
# Try to get properties safely |
|
|
|
|
|
try: |
|
|
|
|
|
props = torch.cuda.get_device_properties(0) |
|
|
|
|
|
vram_total = props.total_memory / (1024**3) |
|
|
|
|
|
except: |
|
|
|
|
|
vram_total = 0 |
|
|
|
|
|
|
|
|
|
|
|
return {"name": name, "vram_gb": round(vram_total, 1), "backend": "CUDA"} |
|
|
|
|
|
except Exception as e: |
|
|
|
|
|
return {"name": f"Detection Error ({type(e).__name__})", "vram_gb": 0, "backend": "Unknown"} |
|
|
|
|
|
|
|
|
def process_session(session_path): |
|
|
def process_session(session_path): |
|
|
print(f"\n>>> Processing session: {session_path.name}") |
|
|
print(f"\n>>> Processing session: {session_path.name}") |
|
|
|
|
|
|
|
|
@ -34,9 +58,12 @@ def process_session(session_path): |
|
|
radar_types = ['awrl1432', 'radarbook'] |
|
|
radar_types = ['awrl1432', 'radarbook'] |
|
|
models = {} |
|
|
models = {} |
|
|
|
|
|
|
|
|
|
|
|
# ----------------------------------------------------------------------- |
|
|
|
|
|
# DIAGNOSTIC: Step 1 - Model Initialization |
|
|
|
|
|
# ----------------------------------------------------------------------- |
|
|
|
|
|
print(f" [DIAGNOSTIC] Step 1: Initializing models...", flush=True) |
|
|
for r_type in radar_types: |
|
|
for r_type in radar_types: |
|
|
try: |
|
|
try: |
|
|
print(f" Initializing ShenronRadarModel ({r_type})...") |
|
|
|
|
|
models[r_type] = ShenronRadarModel(radar_type=r_type) |
|
|
models[r_type] = ShenronRadarModel(radar_type=r_type) |
|
|
(session_path / r_type).mkdir(exist_ok=True) |
|
|
(session_path / r_type).mkdir(exist_ok=True) |
|
|
|
|
|
|
|
|
@ -52,26 +79,53 @@ def process_session(session_path): |
|
|
print(f" [WARNING] Failed to init {r_type}: {e}") |
|
|
print(f" [WARNING] Failed to init {r_type}: {e}") |
|
|
continue |
|
|
continue |
|
|
|
|
|
|
|
|
print(f" Generating Shenron Radar data for {len(lidar_files)} frames...") |
|
|
|
|
|
|
|
|
# ----------------------------------------------------------------------- |
|
|
|
|
|
# TELEMETRY: Emit structured init payload |
|
|
|
|
|
# ----------------------------------------------------------------------- |
|
|
|
|
|
print(f" [DIAGNOSTIC] Step 2: Collecting metadata...", flush=True) |
|
|
|
|
|
gpu_info = _get_gpu_info() |
|
|
|
|
|
|
|
|
|
|
|
radar_specs = {} |
|
|
|
|
|
for r_type, model in models.items(): |
|
|
|
|
|
radar_specs[r_type] = model.get_radar_specs() |
|
|
|
|
|
|
|
|
|
|
|
telemetry_init = { |
|
|
|
|
|
"gpu": gpu_info, |
|
|
|
|
|
"radars": radar_specs, |
|
|
|
|
|
"total_frames": len(lidar_files), |
|
|
|
|
|
"session": session_path.name, |
|
|
|
|
|
} |
|
|
|
|
|
print(f"[SHENRON_INIT]{json.dumps(telemetry_init)}", flush=True) |
|
|
|
|
|
|
|
|
for lidar_file in tqdm.tqdm(lidar_files, desc=" Simulating Radar", unit="frame"): |
|
|
|
|
|
# 1. Load Semantic LiDAR data once per frame |
|
|
|
|
|
# Expected raw: [x, y, z, cos, obj, tag] (6 cols) |
|
|
|
|
|
# Expected Shenron input: [x, y, z, intensity, cos, obj, tag] (7 cols) |
|
|
|
|
|
data = np.load(lidar_file) |
|
|
|
|
|
|
|
|
# ----------------------------------------------------------------------- |
|
|
|
|
|
# MAIN PROCESSING LOOP |
|
|
|
|
|
# ----------------------------------------------------------------------- |
|
|
|
|
|
print(f" [DIAGNOSTIC] Step 3: Starting main loop for {len(lidar_files)} frames...", flush=True) |
|
|
|
|
|
total_frames = len(lidar_files) |
|
|
|
|
|
frame_times = [] |
|
|
|
|
|
|
|
|
|
|
|
for frame_idx, lidar_file in enumerate(lidar_files): |
|
|
|
|
|
frame_start = time.time() |
|
|
|
|
|
|
|
|
|
|
|
# 1. Load Semantic LiDAR data |
|
|
|
|
|
try: |
|
|
|
|
|
data = np.load(lidar_file) |
|
|
|
|
|
except Exception as e: |
|
|
|
|
|
print(f"\n [ERROR] Failed to load {lidar_file.name}: {e}") |
|
|
|
|
|
continue |
|
|
|
|
|
|
|
|
if data.shape[1] == 6: |
|
|
if data.shape[1] == 6: |
|
|
# Pad with a dummy intensity column at index 3 |
|
|
|
|
|
# This aligns 'tag' to index 6 as expected by our lidar.py mapping |
|
|
|
|
|
padded_data = np.zeros((data.shape[0], 7), dtype=np.float32) |
|
|
padded_data = np.zeros((data.shape[0], 7), dtype=np.float32) |
|
|
padded_data[:, 0:3] = data[:, 0:3] # x, y, z |
|
|
|
|
|
padded_data[:, 4:7] = data[:, 3:6] # cos, obj, tag |
|
|
|
|
|
|
|
|
padded_data[:, 0:3] = data[:, 0:3] |
|
|
|
|
|
padded_data[:, 4:7] = data[:, 3:6] |
|
|
data = padded_data |
|
|
data = padded_data |
|
|
|
|
|
|
|
|
|
|
|
frame_metrics = {} |
|
|
|
|
|
|
|
|
for r_type, model in models.items(): |
|
|
for r_type, model in models.items(): |
|
|
try: |
|
|
try: |
|
|
# 2. Process through the physics-based model |
|
|
# 2. Process through the physics-based model |
|
|
# returns rich PCD: [M, 5] (x, y, z, velocity, magnitude) |
|
|
|
|
|
|
|
|
# print(f" [DEBUG] Processing {r_type} frame {frame_idx+1}...", end='\r', flush=True) |
|
|
rich_pcd = model.process(data) |
|
|
rich_pcd = model.process(data) |
|
|
|
|
|
|
|
|
# 3. Save to disk |
|
|
# 3. Save to disk |
|
|
@ -87,15 +141,61 @@ def process_session(session_path): |
|
|
np.save(met_base / "ra" / f"{frame_name}.npy", met['ra_heatmap']) |
|
|
np.save(met_base / "ra" / f"{frame_name}.npy", met['ra_heatmap']) |
|
|
np.save(met_base / "cfar" / f"{frame_name}.npy", met['threshold_matrix']) |
|
|
np.save(met_base / "cfar" / f"{frame_name}.npy", met['threshold_matrix']) |
|
|
|
|
|
|
|
|
# 5. Save Signal Metrics (Telemetry) |
|
|
|
|
|
metrics = model.get_signal_metrics() |
|
|
|
|
|
if metrics: |
|
|
|
|
|
metrics_file = met_base / "metrics.jsonl" |
|
|
|
|
|
with open(metrics_file, "a") as f: |
|
|
|
|
|
f.write(json.dumps({"frame": lidar_file.stem, **metrics}) + "\n") |
|
|
|
|
|
|
|
|
# 5. Save Signal Metrics |
|
|
|
|
|
try: |
|
|
|
|
|
metrics = model.get_signal_metrics() |
|
|
|
|
|
if metrics: |
|
|
|
|
|
# Clean metrics for JSON (handle NaN/Inf) |
|
|
|
|
|
clean_metrics = {} |
|
|
|
|
|
for k, v in metrics.items(): |
|
|
|
|
|
if isinstance(v, float) and (np.isnan(v) or np.isinf(v)): |
|
|
|
|
|
clean_metrics[k] = 0.0 |
|
|
|
|
|
else: |
|
|
|
|
|
clean_metrics[k] = v |
|
|
|
|
|
|
|
|
|
|
|
metrics_file = met_base / "metrics.jsonl" |
|
|
|
|
|
with open(metrics_file, "a") as f: |
|
|
|
|
|
f.write(json.dumps({"frame": lidar_file.stem, **clean_metrics}) + "\n") |
|
|
|
|
|
|
|
|
|
|
|
# CAPTURE UNIQUE POINT COUNT FOR TELEMETRY |
|
|
|
|
|
clean_metrics["pts"] = int(rich_pcd.shape[0]) if hasattr(rich_pcd, 'shape') else 0 |
|
|
|
|
|
frame_metrics[r_type] = clean_metrics |
|
|
|
|
|
except Exception as e: |
|
|
|
|
|
pass # Metrics failure shouldn't crash the loop |
|
|
|
|
|
|
|
|
except Exception as e: |
|
|
except Exception as e: |
|
|
print(f"\n [ERROR] Failed to process {lidar_file.name} for {r_type}: {e}") |
|
|
print(f"\n [ERROR] Failed to process {lidar_file.name} for {r_type}: {e}") |
|
|
|
|
|
|
|
|
|
|
|
# Timing |
|
|
|
|
|
frame_elapsed = time.time() - frame_start |
|
|
|
|
|
frame_times.append(frame_elapsed) |
|
|
|
|
|
|
|
|
|
|
|
avg_time = sum(frame_times[-10:]) / len(frame_times[-10:]) |
|
|
|
|
|
remaining = total_frames - (frame_idx + 1) |
|
|
|
|
|
eta_seconds = remaining * avg_time |
|
|
|
|
|
|
|
|
|
|
|
eta_str = f"{int(eta_seconds // 60)}m {int(eta_seconds % 60)}s" if eta_seconds > 60 else f"{int(eta_seconds)}s" |
|
|
|
|
|
progress_pct = int(((frame_idx + 1) / total_frames) * 100) |
|
|
|
|
|
|
|
|
|
|
|
telemetry_frame = { |
|
|
|
|
|
"frame": frame_idx + 1, |
|
|
|
|
|
"total": total_frames, |
|
|
|
|
|
"pct": progress_pct, |
|
|
|
|
|
"fps": round(1.0 / frame_elapsed, 2) if frame_elapsed > 0 else 0, |
|
|
|
|
|
"elapsed": round(frame_elapsed, 2), |
|
|
|
|
|
"eta": eta_str, |
|
|
|
|
|
"metrics": {} |
|
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
|
|
for r_type, m in frame_metrics.items(): |
|
|
|
|
|
telemetry_frame["metrics"][r_type] = { |
|
|
|
|
|
"snr": round(m.get("peak_snr_db", 0), 1), |
|
|
|
|
|
"pts": m.get("pts", 0), |
|
|
|
|
|
"peak": round(m.get("peak_magnitude", 0), 1), |
|
|
|
|
|
"bins": m.get("active_bins", 0), |
|
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
|
|
print(f"[SHENRON_STEP]{json.dumps(telemetry_frame)}", flush=True) |
|
|
|
|
|
|
|
|
def main(): |
|
|
def main(): |
|
|
data_root = project_root / "data" |
|
|
data_root = project_root / "data" |
|
|
@ -103,7 +203,6 @@ def main(): |
|
|
print(f"Error: {data_root} not found.") |
|
|
print(f"Error: {data_root} not found.") |
|
|
return |
|
|
return |
|
|
|
|
|
|
|
|
# Get all session folders |
|
|
|
|
|
sessions = sorted([d for d in data_root.iterdir() if d.is_dir()]) |
|
|
sessions = sorted([d for d in data_root.iterdir() if d.is_dir()]) |
|
|
|
|
|
|
|
|
if not sessions: |
|
|
if not sessions: |
|
|
@ -113,11 +212,8 @@ def main(): |
|
|
print(f"Found {len(sessions)} sessions.") |
|
|
print(f"Found {len(sessions)} sessions.") |
|
|
|
|
|
|
|
|
for session in sessions: |
|
|
for session in sessions: |
|
|
# Check if the session has frames.jsonl to confirm it's a valid data folder |
|
|
|
|
|
if (session / "frames.jsonl").exists(): |
|
|
if (session / "frames.jsonl").exists(): |
|
|
process_session(session) |
|
|
process_session(session) |
|
|
else: |
|
|
|
|
|
print(f"Skipping {session.name} (no frames.jsonl found).") |
|
|
|
|
|
|
|
|
|
|
|
print("\n" + "="*50) |
|
|
print("\n" + "="*50) |
|
|
print("SHENRON BATCH PROCESSING COMPLETE!") |
|
|
print("SHENRON BATCH PROCESSING COMPLETE!") |
|
|
|
