You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
515 lines
27 KiB
515 lines
27 KiB
import os
|
|
import sys
|
|
import json
|
|
import base64
|
|
import io
|
|
import numpy as np
|
|
from PIL import Image
|
|
from mcap.writer import Writer
|
|
|
|
# Add ISOLATE path for sim_radar_utils imports
|
|
_isolate_path = os.path.join(os.path.dirname(os.path.abspath(__file__)), 'ISOLATE')
|
|
if _isolate_path not in sys.path:
|
|
sys.path.append(_isolate_path)
|
|
|
|
from sim_radar_utils.plots import render_heatmap, FastHeatmapEngine, postprocess_ra, scan_convert_ra
|
|
|
|
# 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_IMAGE_SCHEMA = {
|
|
"$schema": "https://json-schema.org/draft/2020-12/schema",
|
|
"$id": "foxglove.CompressedImage",
|
|
"title": "foxglove.CompressedImage",
|
|
"type": "object",
|
|
"properties": {
|
|
"timestamp": {
|
|
"type": "object",
|
|
"properties": {"sec": {"type": "integer"}, "nsec": {"type": "integer"}}
|
|
},
|
|
"frame_id": {"type": "string"},
|
|
"data": {"type": "string", "contentEncoding": "base64"},
|
|
"format": {"type": "string"}
|
|
}
|
|
}
|
|
|
|
FOXGLOVE_PCL_SCHEMA = {
|
|
"$schema": "https://json-schema.org/draft/2020-12/schema",
|
|
"$id": "foxglove.PointCloud",
|
|
"title": "foxglove.PointCloud",
|
|
"type": "object",
|
|
"properties": {
|
|
"timestamp": {
|
|
"type": "object",
|
|
"properties": {"sec": {"type": "integer"}, "nsec": {"type": "integer"}}
|
|
},
|
|
"frame_id": {"type": "string"},
|
|
"pose": FOXGLOVE_POSE_SCHEMA,
|
|
"point_stride": {"type": "integer"},
|
|
"fields": {
|
|
"type": "array",
|
|
"items": {
|
|
"type": "object",
|
|
"properties": {
|
|
"name": {"type": "string"},
|
|
"offset": {"type": "integer"},
|
|
"type": {"type": "integer"}
|
|
}
|
|
}
|
|
},
|
|
"data": {"type": "string", "contentEncoding": "base64"}
|
|
}
|
|
}
|
|
|
|
FOXGLOVE_METRICS_SCHEMA = {
|
|
"type": "object",
|
|
"properties": {
|
|
"timestamp": {"type": "object", "properties": {"sec": {"type": "integer"}, "nsec": {"type": "integer"}}},
|
|
"frame_id": {"type": "string"},
|
|
"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"}
|
|
}
|
|
}
|
|
|
|
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"}}}
|
|
}
|
|
}
|
|
}
|
|
}
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
# Hardware FOV specs for 3D frustum visualization
|
|
FRUSTUM_SPECS = {
|
|
"awrl1432": {"az_deg": 75.0, "el_deg": 20.0, "max_r": 150.0, "color": {"r": 1.0, "g": 0.5, "b": 0.0, "a": 1.0}},
|
|
"radarbook": {"az_deg": 60.0, "el_deg": 10.0, "max_r": 150.0, "color": {"r": 0.0, "g": 1.0, "b": 1.0, "a": 1.0}},
|
|
}
|
|
|
|
def load_frames(folder_path):
|
|
with open(os.path.join(folder_path, "frames.jsonl")) as f:
|
|
for line in f:
|
|
yield json.loads(line)
|
|
|
|
def convert_folder(folder_path):
|
|
folder_name = os.path.basename(folder_path)
|
|
output_path = os.path.join(folder_path, f"{folder_name}.mcap")
|
|
|
|
if os.path.exists(output_path):
|
|
print(f"\n>>> Skipping folder (MCAP already exists): {folder_name}", flush=True)
|
|
return
|
|
|
|
print(f"\n>>> Processing folder: {folder_name}", flush=True)
|
|
print(f"Target MCAP: {output_path}", flush=True)
|
|
|
|
with open(output_path, "wb") as f:
|
|
writer = Writer(f)
|
|
writer.start(profile="foxglove")
|
|
|
|
# Register Schemas
|
|
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="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
|
|
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/native", message_encoding="json", schema_id=lidar_schema_id)
|
|
radar_types = ['awrl1432', 'radarbook']
|
|
shenron_channels = {}
|
|
met_channels = {}
|
|
cached_axes = {}
|
|
metrics_lookups = {}
|
|
render_engines = {}
|
|
|
|
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)
|
|
|
|
met_channels[r_type] = {
|
|
"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),
|
|
"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),
|
|
"telemetry": writer.register_channel(topic=f"/radar/{r_type}/metrics", message_encoding="json", schema_id=metrics_schema_id),
|
|
"frustum": writer.register_channel(topic=f"/radar/{r_type}/fov_frustum", message_encoding="json", schema_id=scene_update_schema_id),
|
|
}
|
|
|
|
# Pre-load axes and radar specs
|
|
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 (flattened for Foxglove Plot panel)
|
|
metrics_lookups[r_type] = {}
|
|
met_dir = os.path.join(folder_path, r_type, "metrology")
|
|
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)
|
|
if "frame" in m_data:
|
|
metrics_lookups[r_type][m_data["frame"]] = {k: v for k, v in m_data.items() if k != "frame"}
|
|
print(f" - Loaded {len(metrics_lookups[r_type])} metrics records for {r_type}.")
|
|
|
|
# Load radar hardware specs for FastHeatmapEngine extent calculation
|
|
specs_path = os.path.join(met_dir, "radar_specs.json")
|
|
max_vel = 32.5 # fallback
|
|
if os.path.exists(specs_path):
|
|
with open(specs_path, "r") as sf:
|
|
hw = json.loads(sf.read())
|
|
max_vel = hw.get("max_velocity", 32.5)
|
|
|
|
max_r = cached_axes[r_type]['range_axis'][-1] if r_type in cached_axes else 150
|
|
display_limit = 120.0
|
|
|
|
# Initialize stateful Matplotlib renderers (ported from test_shenron.py)
|
|
render_engines[r_type] = {
|
|
'rd': FastHeatmapEngine(extent=[-max_vel, max_vel, 0, max_r], cmap='viridis', title=f'{r_type.upper()} Range-Doppler', xlabel='Doppler Velocity [m/s]', ylabel='Range [m]', ylim=[0, 120], interpolation='bicubic'),
|
|
'cfar': FastHeatmapEngine(extent=[-max_vel, max_vel, 0, max_r], cmap='plasma', title=f'{r_type.upper()} CFAR Noise Threshold', xlabel='Doppler Velocity [m/s]', ylabel='Range [m]', ylim=[0, 120], interpolation='bicubic'),
|
|
'ra_static': FastHeatmapEngine(extent=[-display_limit, display_limit, 0, display_limit], cmap='jet', vmin=-5, vmax=45, title=f'{r_type.upper()} Range-Azimuth (Absolute)', xlabel='Lateral distance [m]', ylabel='Longitudinal distance [m]', aspect='equal'),
|
|
'ra_dyn': FastHeatmapEngine(extent=[-display_limit, display_limit, 0, display_limit], cmap='jet', title=f'{r_type.upper()} Range-Azimuth (Dynamic)', xlabel='Lateral distance [m]', ylabel='Longitudinal distance [m]', aspect='equal'),
|
|
}
|
|
|
|
frame_count = 0
|
|
for frame in load_frames(folder_path):
|
|
ts_ns = int(frame["timestamp"] * 1e9)
|
|
ts_sec = ts_ns // 1_000_000_000
|
|
ts_nsec = ts_ns % 1_000_000_000
|
|
|
|
raw_pose = frame["ego_pose"]
|
|
x, y, z = raw_pose["x"], -raw_pose["y"], raw_pose["z"]
|
|
yaw_rad = -np.radians(raw_pose.get("yaw", 0))
|
|
|
|
ego_world_pose = {
|
|
"position": {"x": x, "y": y, "z": z},
|
|
"orientation": {"x": 0.0, "y": 0.0, "z": float(np.sin(yaw_rad/2)), "w": float(np.cos(yaw_rad/2))}
|
|
}
|
|
identity_pose = {"position": {"x": 0.0, "y": 0.0, "z": 0.0}, "orientation": {"x": 0.0, "y": 0.0, "z": 0.0, "w": 1.0}}
|
|
|
|
# CAMERA
|
|
camera_path = os.path.join(folder_path, "camera", frame["camera"])
|
|
if os.path.exists(camera_path):
|
|
with open(camera_path, "rb") as img_f:
|
|
img_bytes = img_f.read()
|
|
cam_msg = {
|
|
"timestamp": {"sec": ts_sec, "nsec": ts_nsec},
|
|
"frame_id": "ego_vehicle",
|
|
"format": "png" if frame["camera"].endswith(".png") else "jpeg",
|
|
"data": base64.b64encode(img_bytes).decode("ascii")
|
|
}
|
|
writer.add_message(camera_channel_id, log_time=ts_ns, data=json.dumps(cam_msg).encode(), publish_time=ts_ns)
|
|
|
|
# CAMERA (TPP)
|
|
if "camera_tpp" in frame:
|
|
camera_tpp_path = os.path.join(folder_path, "camera_tpp", frame["camera_tpp"])
|
|
if os.path.exists(camera_tpp_path):
|
|
with open(camera_tpp_path, "rb") as img_f:
|
|
img_bytes = img_f.read()
|
|
cam_tpp_msg = {
|
|
"timestamp": {"sec": ts_sec, "nsec": ts_nsec},
|
|
"frame_id": "ego_vehicle",
|
|
"format": "png" if frame["camera_tpp"].endswith(".png") else "jpeg",
|
|
"data": base64.b64encode(img_bytes).decode("ascii")
|
|
}
|
|
writer.add_message(camera_tpp_channel_id, log_time=ts_ns, data=json.dumps(cam_tpp_msg).encode(), publish_time=ts_ns)
|
|
|
|
# EGO POSE
|
|
writer.add_message(pose_channel_id, log_time=ts_ns, data=json.dumps(ego_world_pose).encode(), publish_time=ts_ns)
|
|
|
|
# LIDAR
|
|
lidar_path = os.path.join(folder_path, "lidar", frame["lidar"])
|
|
if os.path.exists(lidar_path):
|
|
points = np.load(lidar_path)
|
|
# Robustness handle 6 vs 7 cols
|
|
if points.shape[1] == 6:
|
|
# Pad to [x, y, z, velocity, cos, obj, tag]
|
|
padded = np.zeros((points.shape[0], 7), dtype=np.float32)
|
|
padded[:, 0:3] = points[:, 0:3]
|
|
padded[:, 4] = points[:, 3] # cos
|
|
padded[:, 5] = points[:, 4].view(np.uint32).astype(np.float32) # obj
|
|
padded[:, 6] = points[:, 5].view(np.uint32).astype(np.float32) # tag
|
|
ros_points = padded
|
|
else:
|
|
ros_points = points.copy().astype(np.float32)
|
|
# Correct bits for [x,y,z,vel,cos,obj,tag]
|
|
ros_points[:, 5] = ros_points[:, 5].view(np.uint32).astype(np.float32)
|
|
ros_points[:, 6] = ros_points[:, 6].view(np.uint32).astype(np.float32)
|
|
|
|
ros_points[:, 1] = -ros_points[:, 1] # RHS conversion
|
|
|
|
# MOUNT OFFSET: LiDAR is on the roof (Z=2.5)
|
|
lidar_pose = {"position": {"x": 0.0, "y": 0.0, "z": 2.5}, "orientation": {"x": 0.0, "y": 0.0, "z": 0.0, "w": 1.0}}
|
|
|
|
lidar_msg = {
|
|
"timestamp": {"sec": ts_sec, "nsec": ts_nsec},
|
|
"frame_id": "ego_vehicle", "pose": lidar_pose, "point_stride": 28,
|
|
"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":"cos_inc_angle","offset":16,"type":7},
|
|
{"name":"object_id","offset":20,"type":7},
|
|
{"name":"semantic_tag","offset":24,"type":7}
|
|
],
|
|
"data": base64.b64encode(ros_points.tobytes()).decode("ascii")
|
|
}
|
|
writer.add_message(lidar_channel_id, log_time=ts_ns, data=json.dumps(lidar_msg).encode(), publish_time=ts_ns)
|
|
|
|
# RADAR
|
|
radar_path = os.path.join(folder_path, "radar", frame["radar"])
|
|
if os.path.exists(radar_path):
|
|
r_data = np.load(radar_path)
|
|
if r_data.size > 0:
|
|
# r_data = [depth, azimuth, altitude, velocity]
|
|
# We negate azimuth to convert from CARLA (Right-handed for Y)
|
|
# note: CARLA is actually LHS (X-fwd, Y-right, Z-up)
|
|
# ROS is RHS (X-fwd, Y-left, Z-up) -> Negating Y converts it.
|
|
dist, az, alt, vel = r_data[:, 0], -r_data[:, 1], r_data[:, 2], r_data[:, 3]
|
|
|
|
xr, yr, zr = dist*np.cos(az)*np.cos(alt), dist*np.sin(az)*np.cos(alt), dist*np.sin(alt)
|
|
|
|
# Stack X, Y, Z, and Velocity (4 floats = 16 bytes stride)
|
|
radar_points = np.stack([xr, yr, zr, vel], axis=1).astype(np.float32)
|
|
|
|
# MOUNT OFFSET: Radar is on the bumper (X=2.0, Z=1.0)
|
|
radar_pose = {"position": {"x": 2.0, "y": 0.0, "z": 1.0}, "orientation": {"x": 0.0, "y": 0.0, "z": 0.0, "w": 1.0}}
|
|
|
|
radar_msg = {
|
|
"timestamp": {"sec": ts_sec, "nsec": ts_nsec},
|
|
"frame_id": "ego_vehicle",
|
|
"pose": radar_pose,
|
|
"point_stride": 16,
|
|
"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}
|
|
],
|
|
"data": base64.b64encode(radar_points.tobytes()).decode("ascii")
|
|
}
|
|
writer.add_message(radar_channel_id, log_time=ts_ns, data=json.dumps(radar_msg).encode(), publish_time=ts_ns)
|
|
|
|
# SHENRON RADARS
|
|
shenron_file = f"frame_{int(frame['frame_id']):06d}.npy"
|
|
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):
|
|
# RD (dB-converted with system gain offset)
|
|
rd_p = os.path.join(met_dir, "rd", f"{frame_name}.npy")
|
|
if os.path.exists(rd_p):
|
|
rd_data = np.load(rd_p)
|
|
rd_db = 10 * np.log10(np.clip(rd_data, 1e-9, None)) - 68.0
|
|
b64 = render_engines[r_type]['rd'].render(np.flipud(rd_db))
|
|
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)
|
|
|
|
# RA (Dual: Static Absolute + Dynamic Peak)
|
|
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, max_display_range=120.0)
|
|
|
|
# Static plot (fixed bounds for 1:1 magnitude tracking)
|
|
b64_static = render_engines[r_type]['ra_static'].render(bev_data)
|
|
if b64_static:
|
|
msg = {"timestamp": {"sec": ts_sec, "nsec": ts_nsec}, "frame_id": "ego_vehicle", "format": "png", "data": b64_static}
|
|
writer.add_message(met_channels[r_type]["ra"], log_time=ts_ns, data=json.dumps(msg).encode(), publish_time=ts_ns)
|
|
|
|
# Dynamic plot (auto-scaled to track peak signature)
|
|
b64_dynamic = render_engines[r_type]['ra_dyn'].render(bev_data)
|
|
if b64_dynamic:
|
|
msg_dyn = {"timestamp": {"sec": ts_sec, "nsec": ts_nsec}, "frame_id": "ego_vehicle", "format": "png", "data": b64_dynamic}
|
|
writer.add_message(met_channels[r_type]["ra_dynamic"], log_time=ts_ns, data=json.dumps(msg_dyn).encode(), publish_time=ts_ns)
|
|
|
|
elif os.path.exists(ra_p):
|
|
# Fallback: rectangular log plot (no axis info available)
|
|
ra_data = np.load(ra_p)
|
|
b64 = render_heatmap(np.log10(np.flipud(ra_data) + 1e-9), cmap='magma')
|
|
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)
|
|
|
|
# CFAR (dB-converted threshold mask)
|
|
cfar_p = os.path.join(met_dir, "cfar", f"{frame_name}.npy")
|
|
if os.path.exists(cfar_p):
|
|
cf_data = np.load(cfar_p)
|
|
cf_db = 10 * np.log10(np.clip(cf_data, 1e-9, None)) - 68.0
|
|
b64 = render_engines[r_type]['cfar'].render(np.flipud(cf_db))
|
|
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)
|
|
|
|
# TELEMETRY (Flattened for Foxglove Plot panel)
|
|
telemetry_row = metrics_lookups.get(r_type, {}).get(frame_name)
|
|
if telemetry_row:
|
|
telemetry_msg = {
|
|
"timestamp": {"sec": ts_sec, "nsec": ts_nsec},
|
|
"frame_id": "ego_vehicle",
|
|
**telemetry_row
|
|
}
|
|
writer.add_message(met_channels[r_type]["telemetry"], log_time=ts_ns, data=json.dumps(telemetry_msg).encode(), publish_time=ts_ns)
|
|
|
|
# 3D HARDWARE FOV FRUSTUM
|
|
axes = cached_axes.get(r_type)
|
|
if axes is not None and r_type in FRUSTUM_SPECS:
|
|
spec = FRUSTUM_SPECS[r_type]
|
|
az_rad = np.radians(spec["az_deg"])
|
|
el_rad = np.radians(spec["el_deg"])
|
|
fr = spec["max_r"]
|
|
c = [
|
|
[0.0, 0.0, 0.0],
|
|
[fr, -fr * np.tan(az_rad), fr * np.tan(el_rad)],
|
|
[fr, fr * np.tan(az_rad), fr * np.tan(el_rad)],
|
|
[fr, -fr * np.tan(az_rad), -fr * np.tan(el_rad)],
|
|
[fr, fr * np.tan(az_rad), -fr * np.tan(el_rad)],
|
|
]
|
|
rhs = [{"x": float(v[0]), "y": float(-v[1]), "z": float(v[2])} for v in c]
|
|
line_points = [
|
|
rhs[0], rhs[1], rhs[0], rhs[2], rhs[0], rhs[3], rhs[0], rhs[4],
|
|
rhs[1], rhs[2], rhs[2], rhs[4], rhs[4], rhs[3], rhs[3], rhs[1]
|
|
]
|
|
frustum_msg = {
|
|
"entities": [{
|
|
"id": f"radar_fov_{r_type}",
|
|
"frame_id": "ego_vehicle",
|
|
"timestamp": {"sec": ts_sec, "nsec": ts_nsec},
|
|
"lines": [{
|
|
"type": 1,
|
|
"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": spec["color"]
|
|
}]
|
|
}]
|
|
}
|
|
writer.add_message(met_channels[r_type]["frustum"], log_time=ts_ns, data=json.dumps(frustum_msg).encode(), publish_time=ts_ns)
|
|
|
|
|
|
frame_count += 1
|
|
if frame_count % 50 == 0:
|
|
print(f" Processed {frame_count} frames...", flush=True)
|
|
|
|
writer.finish()
|
|
print(f" Done! MCAP saved: {output_path} ({os.path.getsize(output_path)/1024/1024:.2f} MB)", flush=True)
|
|
|
|
def main():
|
|
PROJECT_ROOT = os.path.dirname(os.path.dirname(os.path.abspath(__file__)))
|
|
root_data = os.path.join(PROJECT_ROOT, "data")
|
|
if not os.path.exists(root_data):
|
|
print(f"Error: {root_data} directory not found.")
|
|
return
|
|
|
|
folders = [os.path.join(root_data, d) for d in os.listdir(root_data) if os.path.isdir(os.path.join(root_data, d))]
|
|
|
|
# Also check if 'root_data' itself contains 'frames.jsonl' (legacy single-folder mode)
|
|
if os.path.exists(os.path.join(root_data, "frames.jsonl")):
|
|
convert_folder(root_data)
|
|
|
|
for folder in folders:
|
|
if os.path.exists(os.path.join(folder, "frames.jsonl")):
|
|
# Check if MCAP already exists and avoid re-processing if you prefer,
|
|
# but here we'll process all matching folders.
|
|
convert_folder(folder)
|
|
|
|
if __name__ == "__main__":
|
|
main()
|