From 7dcfef81d48a19346e58c5b1c8b2d69cc8a10722 Mon Sep 17 00:00:00 2001 From: rakadu1 Date: Fri, 17 Apr 2026 16:34:30 +0530 Subject: [PATCH] Shenron: Integrated High-Visibility FOV Frustums and Advanced Metrology Pipeline --- scripts/test_shenron.py | 169 ++++++++++++++++++++++++++++++++++++++-- 1 file changed, 163 insertions(+), 6 deletions(-) diff --git a/scripts/test_shenron.py b/scripts/test_shenron.py index f623219..8f5d7db 100644 --- a/scripts/test_shenron.py +++ b/scripts/test_shenron.py @@ -13,6 +13,54 @@ import io from PIL import Image from mcap.writer import Writer +# Official Foxglove JSON Schemas +FOXGLOVE_POSE_SCHEMA = { + "$schema": "https://json-schema.org/draft/2020-12/schema", + "$id": "foxglove.Pose", + "title": "foxglove.Pose", + "type": "object", + "properties": { + "position": {"type": "object", "properties": {"x": {"type": "number"}, "y": {"type": "number"}, "z": {"type": "number"}}}, + "orientation": {"type": "object", "properties": {"x": {"type": "number"}, "y": {"type": "number"}, "z": {"type": "number"}, "w": {"type": "number"}}} + } +} + +FOXGLOVE_SCENE_UPDATE_SCHEMA = { + "$schema": "https://json-schema.org/draft/2020-12/schema", + "$id": "foxglove.SceneUpdate", + "title": "foxglove.SceneUpdate", + "type": "object", + "properties": { + "entities": { + "type": "array", + "items": { + "type": "object", + "properties": { + "id": {"type": "string"}, + "frame_id": {"type": "string"}, + "timestamp": {"type": "object", "properties": {"sec": {"type": "integer"}, "nsec": {"type": "integer"}}}, + "lines": { + "type": "array", + "items": { + "type": "object", + "properties": { + "type": {"type": "integer"}, + "pose": FOXGLOVE_POSE_SCHEMA, + "points": { + "type": "array", + "items": {"type": "object", "properties": {"x": {"type": "number"}, "y": {"type": "number"}, "z": {"type": "number"}}} + }, + "thickness": {"type": "number"}, + "color": {"type": "object", "properties": {"r": {"type": "number"}, "g": {"type": "number"}, "b": {"type": "number"}, "a": {"type": "number"}}} + } + } + } + } + } + } + } +} + # Add project root and ISOLATE paths project_root = Path(__file__).parent.parent sys.path.append(str(project_root)) @@ -65,14 +113,24 @@ FOXGLOVE_PCL_SCHEMA = { } } 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"}} + "peak_magnitude": {"type": "number"}, + "avg_noise_floor": {"type": "number"}, + "peak_snr_db": {"type": "number"}, + "active_bins": {"type": "number"}, + "cfar_target_count": {"type": "number"}, + "farthest_target_m": {"type": "number"}, + "closest_target_m": {"type": "number"}, + "mean_absolute_doppler": {"type": "number"}, + "doppler_variance": {"type": "number"}, + "dynamic_range_db": {"type": "number"}, + "ego_vicinity_power": {"type": "number"}, + "clutter_ratio": {"type": "number"}, + "signal_to_clutter_ratio_db": {"type": "number"}, + "azimuth_variance": {"type": "number"} } } @@ -330,6 +388,8 @@ def run_testbench(iter_name): pose_schema_id = writer.register_schema(name="foxglove.Pose", encoding="jsonschema", data=json.dumps(FOXGLOVE_POSE_SCHEMA).encode()) camera_schema_id = writer.register_schema(name="foxglove.CompressedImage", encoding="jsonschema", data=json.dumps(FOXGLOVE_IMAGE_SCHEMA).encode()) lidar_schema_id = writer.register_schema(name="foxglove.PointCloud", encoding="jsonschema", data=json.dumps(FOXGLOVE_PCL_SCHEMA).encode()) + telemetry_schema_id = writer.register_schema(name="TelemetryMetrics", encoding="jsonschema", data=json.dumps(FOXGLOVE_METRICS_SCHEMA).encode()) + scene_update_schema_id = writer.register_schema(name="foxglove.SceneUpdate", encoding="jsonschema", data=json.dumps(FOXGLOVE_SCENE_UPDATE_SCHEMA).encode()) # Register Channels channels = { @@ -351,7 +411,8 @@ def run_testbench(iter_name): "ra": writer.register_channel(topic=f"/radar/{r_type}/heatmaps/range_azimuth", message_encoding="json", schema_id=camera_schema_id), "ra_dynamic": writer.register_channel(topic=f"/radar/{r_type}/heatmaps/range_azimuth_dynamic", 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())) + "telemetry": writer.register_channel(topic=f"/radar/{r_type}/metrics", message_encoding="json", schema_id=telemetry_schema_id), + "frustum": writer.register_channel(topic=f"/radar/{r_type}/fov_frustum", message_encoding="json", schema_id=scene_update_schema_id) } try: @@ -363,10 +424,21 @@ def run_testbench(iter_name): print(f"[ERROR] Could not load frames.jsonl from {logs_dir}: {e}") return - # Pre-load physical axes & Heatmap Engines + # Pre-load telemetry maps to avoid doing O(N^2) disk reads + telemetry_cache = {} cached_axes = {} render_engines = {} + for r_type in radar_types: + metrics_path = iter_dir / r_type / "metrology" / "metrics.jsonl" + telemetry_cache[r_type] = {} + if metrics_path.exists(): + with open(metrics_path, "r") as mf: + for line in mf: + ld = json.loads(line) + if "frame" in ld: + telemetry_cache[r_type][ld["frame"]] = {k: v for k, v in ld.items() if k != "frame"} + range_ax_p = iter_dir / r_type / "metrology" / "range_axis.npy" angle_ax_p = iter_dir / r_type / "metrology" / "angle_axis.npy" if range_ax_p.exists() and angle_ax_p.exists(): @@ -562,6 +634,91 @@ def run_testbench(iter_name): 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) + + # --- PHASE 3: Telemetry Stream --- + telemetry_row = telemetry_cache[r_type].get(shenron_fname.replace('.npy', '')) + if telemetry_row: + # Flattening message natively so Foxglove Plot panel can instantly locate numbers dynamically map /radar/.../metrics.peak_magnitude + telemetry_msg = { + "timestamp": {"sec": ts_sec, "nsec": ts_nsec}, + "frame_id": "ego_vehicle", + **telemetry_row + } + writer.add_message(metrology_channels[r_type]["telemetry"], log_time=ts_ns, data=json.dumps(telemetry_msg).encode(), publish_time=ts_ns) + + # --- PHASE 4: 3D Hardware FOV Frustum (Audited Geometry & Visibility) --- + axes = cached_axes.get(r_type) + if axes is not None: + # 1. Base Physical Constraints (Use Hardware Specs for the "Box") + # Some range axes go to FFT-limit, we cap the visual frustum to a reasonable effective range + max_r = 150.0 + + # Hardware Azimuth/Elevation typicals + az_limit_deg = 75.0 + el_limit_deg = 15.0 + + if r_type == "awrl1432": + az_limit_deg = 75.0 + el_limit_deg = 20.0 + elif r_type == "radarbook": + az_limit_deg = 60.0 + el_limit_deg = 10.0 + + # Convert to Radians + az_rad = np.radians(az_limit_deg) + el_rad = np.radians(el_limit_deg) + + # 2. Vertex Calculation (Planar Far-Face Project at X = max_r) + # This ensures the frustum extends the full distance forward on the boresight. + # Origin is (0,0,0) in the Entity frame + # Corners (TL, TR, BL, BR) + # Carla LHS: X=Forward, Y=Right, Z=Up + c = [ + [0.0, 0.0, 0.0], # V0: Origin + [max_r, -max_r * np.tan(az_rad), max_r * np.tan(el_rad)], # V1: TL (NegAz, PosEl) + [max_r, max_r * np.tan(az_rad), max_r * np.tan(el_rad)], # V2: TR (PosAz, PosEl) + [max_r, -max_r * np.tan(az_rad), -max_r * np.tan(el_rad)], # V3: BL (NegAz, NegEl) + [max_r, max_r * np.tan(az_rad), -max_r * np.tan(el_rad)], # V4: BR (PosAz, NegEl) + ] + + # RHS Conversion: [X, -Y, Z] + rhs = [{"x": float(v[0]), "y": float(-v[1]), "z": float(v[2])} for v in c] + + # 3. One-Time Verification Log + if frame == frames[0]: + tqdm.tqdm.write(f"\n [AUDIT] {r_type.upper()} Frustum Calculation:") + tqdm.tqdm.write(f" - Spec: Az ±{az_limit_deg}°, El ±{el_limit_deg}°, Range {max_r}m") + tqdm.tqdm.write(f" - V1 (RHS TL): X={rhs[1]['x']:.1f}, Y={rhs[1]['y']:.1f}, Z={rhs[1]['z']:.1f}") + tqdm.tqdm.write(f" - range_axis[0, -1]: {axes['range_axis'][0]:.1f}, {axes['range_axis'][-1]:.1f}") + tqdm.tqdm.write(f" - angle_axis (rad) [0, -1]: {axes['angle_axis'][0]:.3f}, {axes['angle_axis'][-1]:.3f}") + + # 4. Connect primitives (LINE_LIST pairs) + line_points = [ + rhs[0], rhs[1], rhs[0], rhs[2], rhs[0], rhs[3], rhs[0], rhs[4], # Beams + rhs[1], rhs[2], rhs[2], rhs[4], rhs[4], rhs[3], rhs[3], rhs[1] # Face + ] + + color_map = { + "awrl1432": {"r": 1.0, "g": 0.5, "b": 0.0, "a": 1.0}, # Solid Orange + "radarbook": {"r": 0.0, "g": 1.0, "b": 1.0, "a": 1.0} # Solid Cyan + } + f_color = color_map.get(r_type, {"r": 1.0, "g": 1.0, "b": 1.0, "a": 1.0}) + + frustum_msg = { + "entities": [{ + "id": f"radar_fov_{r_type}", + "frame_id": "ego_vehicle", + "timestamp": {"sec": ts_sec, "nsec": ts_nsec}, + "lines": [{ + "type": 1, # LINE_LIST + "pose": {"position": {"x": 2.0, "y": 0.0, "z": 1.0}, "orientation": {"x": 0.0, "y": 0.0, "z": 0.0, "w": 1.0}}, + "points": line_points, + "thickness": 0.5, + "color": f_color + }] + }] + } + writer.add_message(metrology_channels[r_type]["frustum"], log_time=ts_ns, data=json.dumps(frustum_msg).encode(), publish_time=ts_ns) writer.finish() print(f"\n[SUCCESS] Iteration packaged to: {output_mcap}")