rakadu1
/
Simulator
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

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.
1843_integration
RUSHIL AMBARISH KADU 1 month ago
parent
de215a27d7
commit
78205ad32a
3 changed files with 212 additions and 142 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 36
      run.bat
  2. 215
      scripts/data_to_mcap.py
  3. 103
      scripts/generate_shenron.py

36
run.bat
View File

@ -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 %*
)

215
scripts/data_to_mcap.py
View File

@ -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:

103
scripts/generate_shenron.py
View File

@ -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"

Write Preview
Loading…
Cancel
Save