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

Shenron: Integrated High-Visibility FOV Frustums and Advanced Metrology Pipeline

main
RUSHIL AMBARISH KADU 1 month ago
parent
bf0ce30165
commit
7dcfef81d4
1 changed files with 163 additions and 6 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. 169
      scripts/test_shenron.py

169
scripts/test_shenron.py
View File

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

Write Preview
Loading…
Cancel
Save