From 78205ad32adbf980e4160945889c3a10dcac6f2a Mon Sep 17 00:00:00 2001 From: rakadu1 Date: Thu, 9 Apr 2026 16:58:11 +0530 Subject: [PATCH] feat: Aligned simulation pipeline with high-fidelity testbench diagnostics - Upgraded run.bat with robust argument logic and unbuffered logging for Dashboard SSE. - Integrated multi-radar support (awrl1432, radarbook) in generate_shenron.py. - Unified MCAP topic naming convention (/radar/native, /radar/{type}/metrics, /radar/{type}/heatmaps). - Added per-radar telemetry generation and packaging using foxglove.Telemetry schema. - Modernized heatmap colormap API and added numerical safety clips. --- run.bat | 36 +++--- scripts/data_to_mcap.py | 215 ++++++++++++++++++++++-------------- scripts/generate_shenron.py | 103 +++++++++-------- 3 files changed, 212 insertions(+), 142 deletions(-) diff --git a/run.bat b/run.bat index 8041f31..ca02b6f 100644 --- a/run.bat +++ b/run.bat @@ -1,25 +1,21 @@ @echo off -:: Activate the CARLA conda environment -call C:\ProgramData\miniconda3\Scripts\activate.bat carla312 - -:: --------------------------------------------------------------- +:: =============================================================== +:: FOX CARLA ADAS SIMULATION — ONE-CLICK RUNNER +:: =============================================================== :: Usage: :: run.bat braking :: run.bat cutin :: run.bat obstacle --frames 120 :: run.bat --list-scenarios -:: run.bat -l -:: --------------------------------------------------------------- +:: =============================================================== + +:: Activate the CARLA conda environment +call C:\ProgramData\miniconda3\Scripts\activate.bat carla312 if "%~1"=="" ( echo [ERROR] No argument specified. echo. - echo Usage: - echo run.bat braking - echo run.bat cutin - echo run.bat obstacle --frames 120 - echo run.bat --list-scenarios - echo. + echo Usage example: run.bat braking pause exit /b 1 ) @@ -27,11 +23,21 @@ if "%~1"=="" ( :: Force unbuffered stdout/stderr so the Dashboard GUI receives output immediately set PYTHONUNBUFFERED=1 -:: Pass --list-scenarios / -l directly, otherwise treat first arg as scenario name -if "%~1"=="--list-scenarios" ( +:: Logic: +:: 1. If user passes --list-scenarios or -l, call directly. +:: 2. If user already specifies --scenario or -s, pass all args directly. +:: 3. Otherwise, prepend --scenario to the first argument. + +set FIRST_ARG=%~1 + +if "%FIRST_ARG%"=="--list-scenarios" ( python src/main.py --list-scenarios -) else if "%~1"=="-l" ( +) else if "%FIRST_ARG%"=="-l" ( python src/main.py --list-scenarios +) else if "%FIRST_ARG%"=="-s" ( + python src/main.py %* +) else if "%FIRST_ARG%"=="--scenario" ( + python src/main.py %* ) else ( python src/main.py --scenario %* ) diff --git a/scripts/data_to_mcap.py b/scripts/data_to_mcap.py index 88aaf5d..307a9a9 100644 --- a/scripts/data_to_mcap.py +++ b/scripts/data_to_mcap.py @@ -4,7 +4,7 @@ import base64 import io import numpy as np from PIL import Image -import matplotlib.cm as cm +import matplotlib from mcap.writer import Writer # Official Foxglove JSON Schemas @@ -69,6 +69,21 @@ 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"}} + } +} + def render_heatmap(data, cmap='viridis', vmin=None, vmax=None): """Convert 2D array to colormapped B64 PNG with guide-compliant normalization.""" # Step 6: Normalization [0, 1] @@ -85,8 +100,7 @@ def render_heatmap(data, cmap='viridis', vmin=None, vmax=None): # Step 7: Apply Radar-style Colormap (Blue-style) # Using matplotlib.cm API for consistency with this script's imports - mapper = cm.get_cmap(cmap) - rgba = mapper(norm) # (H, W, 4) + rgba = matplotlib.colormaps[cmap](norm) # (H, W, 4) rgb = (rgba[:, :, :3] * 255).astype(np.uint8) img = Image.fromarray(rgb) @@ -102,8 +116,7 @@ def postprocess_ra(ra_heatmap, range_axis, smooth_sigma=1.0): # 1. Clutter removal (subtract per-range-bin mean to suppress static ground) clutter = np.mean(ra_heatmap, axis=1, keepdims=True) ra = ra_heatmap - (0.8 * clutter) # Subtract context-aware mean - # 2. Physics-based dynamic range compression (Linear -> Log) - # Conversion to dB scale with System Gain Calibration (calculated from Iter 28) + ra = np.clip(ra, 1e-9, None) SYSTEM_GAIN_OFFSET = 68.0 ra_db = 10 * np.log10(ra) - SYSTEM_GAIN_OFFSET @@ -177,31 +190,52 @@ def convert_folder(folder_path): 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()) + metrics_schema_id = writer.register_schema(name="foxglove.Telemetry", encoding="jsonschema", data=json.dumps(FOXGLOVE_METRICS_SCHEMA).encode()) # Register Channels camera_channel_id = writer.register_channel(topic="/camera", message_encoding="json", schema_id=camera_schema_id) camera_tpp_channel_id = writer.register_channel(topic="/camera_tpp", message_encoding="json", schema_id=camera_schema_id) lidar_channel_id = writer.register_channel(topic="/lidar", message_encoding="json", schema_id=lidar_schema_id) pose_channel_id = writer.register_channel(topic="/ego_pose", message_encoding="json", schema_id=pose_schema_id) - radar_channel_id = writer.register_channel(topic="/radar", message_encoding="json", schema_id=lidar_schema_id) - shenron_channel_id = writer.register_channel(topic="/radar/shenron", message_encoding="json", schema_id=lidar_schema_id) - - # Register Metrology Channels - met_ra_id = writer.register_channel(topic="/radar/shenron/heatmaps/range_azimuth", message_encoding="json", schema_id=camera_schema_id) - met_rd_id = writer.register_channel(topic="/radar/shenron/heatmaps/range_doppler", message_encoding="json", schema_id=camera_schema_id) - met_cfar_id = writer.register_channel(topic="/radar/shenron/heatmaps/cfar_mask", message_encoding="json", schema_id=camera_schema_id) - - # Pre-load axes for scan conversion if they exist - met_dir = os.path.join(folder_path, "metrology") - range_ax = None - angle_ax = None - if os.path.exists(met_dir): - r_ax_p = os.path.join(met_dir, "range_axis.npy") - a_ax_p = os.path.join(met_dir, "angle_axis.npy") - if os.path.exists(r_ax_p) and os.path.exists(a_ax_p): - range_ax = np.load(r_ax_p) - angle_ax = np.load(a_ax_p) - print(" - Loaded physical axes for high-fidelity visualization.") + radar_channel_id = writer.register_channel(topic="/radar/native", message_encoding="json", schema_id=lidar_schema_id) + radar_types = ['awrl1432', 'radarbook'] + shenron_channels = {} + metrics_channels = {} + met_channels = {} + cached_axes = {} + metrics_lookups = {} + + 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) + metrics_channels[r_type] = writer.register_channel(topic=f"/radar/{r_type}/metrics", message_encoding="json", schema_id=metrics_schema_id) + + met_channels[r_type] = { + "ra": writer.register_channel(topic=f"/radar/{r_type}/heatmaps/range_azimuth", message_encoding="json", schema_id=camera_schema_id), + "rd": writer.register_channel(topic=f"/radar/{r_type}/heatmaps/range_doppler", 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) + } + + # Pre-load axes for scan conversion if they exist + met_dir = os.path.join(folder_path, r_type, "metrology") + if os.path.exists(met_dir): + r_ax_p = os.path.join(met_dir, "range_axis.npy") + a_ax_p = os.path.join(met_dir, "angle_axis.npy") + if os.path.exists(r_ax_p) and os.path.exists(a_ax_p): + cached_axes[r_type] = { + 'range_axis': np.load(r_ax_p), + 'angle_axis': np.load(a_ax_p) + } + print(f" - Loaded physical axes for {r_type} visualization.") + + # Load Metrics Lookup if available + metrics_lookups[r_type] = {} + metrics_path = os.path.join(met_dir, "metrics.jsonl") + if os.path.exists(metrics_path): + with open(metrics_path, "r") as mf: + for line in mf: + m_data = json.loads(line) + metrics_lookups[r_type][m_data["frame"]] = m_data + print(f" - Loaded {len(metrics_lookups[r_type])} metrics records for {r_type}.") frame_count = 0 for frame in load_frames(folder_path): @@ -323,66 +357,81 @@ def convert_folder(folder_path): } writer.add_message(radar_channel_id, log_time=ts_ns, data=json.dumps(radar_msg).encode(), publish_time=ts_ns) - # SHENRON RADAR + # SHENRON RADARS shenron_file = f"frame_{int(frame['frame_id']):06d}.npy" - shenron_path = os.path.join(folder_path, "shenron_radar", shenron_file) - if os.path.exists(shenron_path): - s_data = np.load(shenron_path) - if s_data.size > 0: - # s_data = [x, y, z, velocity, magnitude] - # ISOLATE coords: X is fwd, Y is right. - # ROS: X is fwd, Y is left. - ros_shenron = s_data.copy().astype(np.float32) - ros_shenron[:, 1] = -ros_shenron[:, 1] # Negate Y for ROS - - # MOUNT OFFSET: Shenron Radar is on the bumper (X=2.0, Z=1.0) - shenron_pose = {"position": {"x": 2.0, "y": 0.0, "z": 1.0}, "orientation": {"x": 0.0, "y": 0.0, "z": 0.0, "w": 1.0}} - - shenron_msg = { - "timestamp": {"sec": ts_sec, "nsec": ts_nsec}, - "frame_id": "ego_vehicle", - "pose": shenron_pose, - "point_stride": 20, # 5 floats * 4 bytes - "fields": [ - {"name":"x","offset":0,"type":7}, - {"name":"y","offset":4,"type":7}, - {"name":"z","offset":8,"type":7}, - {"name":"velocity","offset":12,"type":7}, - {"name":"magnitude","offset":16,"type":7} - ], - "data": base64.b64encode(ros_shenron.tobytes()).decode("ascii") - } - writer.add_message(shenron_channel_id, log_time=ts_ns, data=json.dumps(shenron_msg).encode(), publish_time=ts_ns) - - # METROLOGY HEATMAPS - if os.path.exists(met_dir): - frame_name = f"frame_{int(frame['frame_id']):06d}" - - # RA (Polar Sector BEV) - ra_p = os.path.join(met_dir, "ra", f"{frame_name}.npy") - if os.path.exists(ra_p) and range_ax is not None: - ra_data = np.load(ra_p) - # Use smooth_sigma=0.0 for sharp focus (Iter 27 baseline) - ra_processed = postprocess_ra(ra_data, range_ax, smooth_sigma=0.0) - bev_data = scan_convert_ra(ra_processed, range_ax, angle_ax, img_size=512) - b64 = render_heatmap(bev_data, cmap='jet', vmin=-5, vmax=45) - msg = {"timestamp": {"sec": ts_sec, "nsec": ts_nsec}, "frame_id": "ego_vehicle", "format": "png", "data": b64} - writer.add_message(met_ra_id, log_time=ts_ns, data=json.dumps(msg).encode(), publish_time=ts_ns) - - # RD (Log-scaled) - rd_p = os.path.join(met_dir, "rd", f"{frame_name}.npy") - if os.path.exists(rd_p): - rd_data = np.log10(np.load(rd_p) + 1e-9) - b64 = render_heatmap(np.flipud(rd_data), cmap='viridis') - msg = {"timestamp": {"sec": ts_sec, "nsec": ts_nsec}, "frame_id": "ego_vehicle", "format": "png", "data": b64} - writer.add_message(met_rd_id, log_time=ts_ns, data=json.dumps(msg).encode(), publish_time=ts_ns) - - # CFAR (Mask) - cfar_p = os.path.join(met_dir, "cfar", f"{frame_name}.npy") - if os.path.exists(cfar_p): - b64 = render_heatmap(np.flipud(np.load(cfar_p)), cmap='plasma') - msg = {"timestamp": {"sec": ts_sec, "nsec": ts_nsec}, "frame_id": "ego_vehicle", "format": "png", "data": b64} - writer.add_message(met_cfar_id, log_time=ts_ns, data=json.dumps(msg).encode(), publish_time=ts_ns) + frame_name = f"frame_{int(frame['frame_id']):06d}" + + for r_type in radar_types: + shenron_path = os.path.join(folder_path, r_type, shenron_file) + if os.path.exists(shenron_path): + s_data = np.load(shenron_path) + if s_data.size > 0: + # s_data = [x, y, z, velocity, magnitude] + # ISOLATE coords: X is fwd, Y is right. + # ROS: X is fwd, Y is left. + ros_shenron = s_data.copy().astype(np.float32) + ros_shenron[:, 1] = -ros_shenron[:, 1] # Negate Y for ROS + + # MOUNT OFFSET: Shenron Radar is on the bumper (X=2.0, Z=1.0) + shenron_pose = {"position": {"x": 2.0, "y": 0.0, "z": 1.0}, "orientation": {"x": 0.0, "y": 0.0, "z": 0.0, "w": 1.0}} + + shenron_msg = { + "timestamp": {"sec": ts_sec, "nsec": ts_nsec}, + "frame_id": "ego_vehicle", + "pose": shenron_pose, + "point_stride": 20, # 5 floats * 4 bytes + "fields": [ + {"name":"x","offset":0,"type":7}, + {"name":"y","offset":4,"type":7}, + {"name":"z","offset":8,"type":7}, + {"name":"velocity","offset":12,"type":7}, + {"name":"magnitude","offset":16,"type":7} + ], + "data": base64.b64encode(ros_shenron.tobytes()).decode("ascii") + } + writer.add_message(shenron_channels[r_type], log_time=ts_ns, data=json.dumps(shenron_msg).encode(), publish_time=ts_ns) + + met_dir = os.path.join(folder_path, r_type, "metrology") + if os.path.exists(met_dir): + # RA (Polar Sector BEV) + ra_p = os.path.join(met_dir, "ra", f"{frame_name}.npy") + if os.path.exists(ra_p) and r_type in cached_axes: + ra_data = np.load(ra_p) + axes = cached_axes[r_type] + ra_processed = postprocess_ra(ra_data, axes['range_axis'], smooth_sigma=0.0) + bev_data = scan_convert_ra(ra_processed, axes['range_axis'], axes['angle_axis'], img_size=512) + b64 = render_heatmap(bev_data, cmap='jet', vmin=-5, vmax=45) + if b64: + msg = {"timestamp": {"sec": ts_sec, "nsec": ts_nsec}, "frame_id": "ego_vehicle", "format": "png", "data": b64} + writer.add_message(met_channels[r_type]["ra"], log_time=ts_ns, data=json.dumps(msg).encode(), publish_time=ts_ns) + + # RD (Log-scaled) + rd_p = os.path.join(met_dir, "rd", f"{frame_name}.npy") + if os.path.exists(rd_p): + rd_data = np.log10(np.load(rd_p) + 1e-9) + b64 = render_heatmap(np.flipud(rd_data), cmap='viridis') + if b64: + msg = {"timestamp": {"sec": ts_sec, "nsec": ts_nsec}, "frame_id": "ego_vehicle", "format": "png", "data": b64} + writer.add_message(met_channels[r_type]["rd"], log_time=ts_ns, data=json.dumps(msg).encode(), publish_time=ts_ns) + + # CFAR (Mask) + cfar_p = os.path.join(met_dir, "cfar", f"{frame_name}.npy") + if os.path.exists(cfar_p): + b64 = render_heatmap(np.flipud(np.load(cfar_p)), cmap='plasma') + if b64: + msg = {"timestamp": {"sec": ts_sec, "nsec": ts_nsec}, "frame_id": "ego_vehicle", "format": "png", "data": b64} + writer.add_message(met_channels[r_type]["cfar"], log_time=ts_ns, data=json.dumps(msg).encode(), publish_time=ts_ns) + + # METRICS (Telemetry) + if frame_name in metrics_lookups.get(r_type, {}): + m_payload = metrics_lookups[r_type][frame_name].copy() + m_payload.pop("frame", None) + telemetry_msg = { + "timestamp": {"sec": ts_sec, "nsec": ts_nsec}, + "frame_id": "ego_vehicle", + "metrics": m_payload + } + writer.add_message(metrics_channels[r_type], log_time=ts_ns, data=json.dumps(telemetry_msg).encode(), publish_time=ts_ns) frame_count += 1 if frame_count % 50 == 0: diff --git a/scripts/generate_shenron.py b/scripts/generate_shenron.py index 90fa61d..bdb7193 100644 --- a/scripts/generate_shenron.py +++ b/scripts/generate_shenron.py @@ -2,6 +2,7 @@ import os import sys import numpy as np import tqdm +import json from pathlib import Path # Add project root and ISOLATE paths @@ -30,57 +31,71 @@ def process_session(session_path): print(f" [SKIP] No .npy files in 'lidar' folder.") return - output_dir = session_path / "shenron_radar" - output_dir.mkdir(exist_ok=True) + radar_types = ['awrl1432', 'radarbook'] + models = {} - # Initialize the model once per session - print(f" Initializing ShenronRadarModel...") - model = ShenronRadarModel(radar_type='radarbook') + for r_type in radar_types: + try: + print(f" Initializing ShenronRadarModel ({r_type})...") + models[r_type] = ShenronRadarModel(radar_type=r_type) + (session_path / r_type).mkdir(exist_ok=True) + + # Create Metrology folders + met_base = session_path / r_type / "metrology" + for sub in ["rd", "ra", "cfar"]: + (met_base / sub).mkdir(parents=True, exist_ok=True) + + # Save physical axes once per session + np.save(met_base / "range_axis.npy", models[r_type].processor.rangeAxis) + np.save(met_base / "angle_axis.npy", models[r_type].processor.angleAxis) + except Exception as e: + print(f" [WARNING] Failed to init {r_type}: {e}") + continue print(f" Generating Shenron Radar data for {len(lidar_files)} frames...") - # Create Metrology folders - met_base = session_path / "metrology" - for sub in ["rd", "ra", "cfar"]: - (met_base / sub).mkdir(parents=True, exist_ok=True) - - # Save physical axes once per session (same for all frames) - np.save(met_base / "range_axis.npy", model.processor.rangeAxis) - np.save(met_base / "angle_axis.npy", model.processor.angleAxis) - for lidar_file in tqdm.tqdm(lidar_files, desc=" Simulating Radar", unit="frame"): - try: - # 1. Load Semantic LiDAR data - # 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) - - 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[:, 0:3] = data[:, 0:3] # x, y, z - padded_data[:, 4:7] = data[:, 3:6] # cos, obj, tag - data = padded_data - - # 2. Process through the physics-based model - # returns rich PCD: [M, 5] (x, y, z, velocity, magnitude) - rich_pcd = model.process(data) - - # 3. Save to disk - output_file = output_dir / lidar_file.name - np.save(output_file, rich_pcd) + # 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) + + 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[:, 0:3] = data[:, 0:3] # x, y, z + padded_data[:, 4:7] = data[:, 3:6] # cos, obj, tag + data = padded_data - # 4. Save Metrology Heatmaps - met = model.get_last_metrology() - if met: - frame_name = lidar_file.stem - np.save(met_base / "rd" / f"{frame_name}.npy", met['rd_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']) + for r_type, model in models.items(): + try: + # 2. Process through the physics-based model + # returns rich PCD: [M, 5] (x, y, z, velocity, magnitude) + rich_pcd = model.process(data) + + # 3. Save to disk + output_file = session_path / r_type / lidar_file.name + np.save(output_file, rich_pcd) + + # 4. Save Metrology Heatmaps + met_base = session_path / r_type / "metrology" + met = model.get_last_metrology() + if met: + frame_name = lidar_file.stem + np.save(met_base / "rd" / f"{frame_name}.npy", met['rd_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']) + + # 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") - except Exception as e: - print(f"\n [ERROR] Failed to process {lidar_file.name}: {e}") + except Exception as e: + print(f"\n [ERROR] Failed to process {lidar_file.name} for {r_type}: {e}") def main(): data_root = project_root / "data"