import io
import json
import math
import numpy as np
from foxglove_schemas_protobuf.CameraCalibration_pb2 import CameraCalibration
from foxglove_schemas_protobuf.CompressedImage_pb2 import CompressedImage
from foxglove_schemas_protobuf.FrameTransform_pb2 import FrameTransform
from foxglove_schemas_protobuf.Pose_pb2 import Pose
from foxglove_schemas_protobuf.PoseInFrame_pb2 import PoseInFrame
from foxglove_schemas_protobuf.Quaternion_pb2 import Quaternion
from foxglove_schemas_protobuf.Vector3_pb2 import Vector3
from google.protobuf.timestamp_pb2 import Timestamp
from mcap.writer import CompressionType, Writer
from mcap_protobuf.schema import register_schema
from PIL import Image
from nomadic_signal_pb2 import Signal, Vector
from nomadic_skeleton_pb2 import Point3, Skeleton
class NomadicMcapWriter:
"""Writes one recording: the nomadic_spec overlay plus protobuf messages."""
def __init__(self, path, overlay):
self._file = open(path, "wb")
self._writer = Writer(self._file, compression=CompressionType.ZSTD)
self._writer.start(library="my-converter")
# The overlay goes in a metadata record, so it is indexed in the summary section.
self._writer.add_metadata("nomadic_spec", {"spec": json.dumps(overlay)})
self._schemas = {}
self._channels = {}
def write(self, topic, t_ns, message):
if topic not in self._channels:
name = message.DESCRIPTOR.full_name
if name not in self._schemas:
self._schemas[name] = register_schema(self._writer, type(message))
self._channels[topic] = self._writer.register_channel(
topic=topic, message_encoding="protobuf", schema_id=self._schemas[name]
)
# One clock: log_time, publish_time and the message timestamp are the same instant.
self._writer.add_message(
channel_id=self._channels[topic], log_time=t_ns, publish_time=t_ns,
data=message.SerializeToString(),
)
def close(self):
self._writer.finish()
self._file.close()
def stamp(t_ns):
ts = Timestamp()
ts.FromNanoseconds(t_ns)
return ts
def jpeg_image(t_ns, frame_id, jpeg_bytes):
return CompressedImage(timestamp=stamp(t_ns), frame_id=frame_id, data=jpeg_bytes, format="jpeg")
def calibration(t_ns, frame_id, width, height, fx, fy, cx, cy):
# K is row-major. No D: the images are rectified.
return CameraCalibration(timestamp=stamp(t_ns), frame_id=frame_id, width=width, height=height,
K=[fx, 0, cx, 0, fy, cy, 0, 0, 1])
def transform(t_ns, child_frame_id, xyz, quat_xyzw):
return FrameTransform(
timestamp=stamp(t_ns), parent_frame_id="vehicle", child_frame_id=child_frame_id,
translation=Vector3(x=xyz[0], y=xyz[1], z=xyz[2]),
rotation=Quaternion(x=quat_xyzw[0], y=quat_xyzw[1], z=quat_xyzw[2], w=quat_xyzw[3]),
)
def pose(t_ns, frame_id, xyz, quat_xyzw):
return PoseInFrame(
timestamp=stamp(t_ns), frame_id=frame_id,
pose=Pose(position=Vector3(x=xyz[0], y=xyz[1], z=xyz[2]),
orientation=Quaternion(x=quat_xyzw[0], y=quat_xyzw[1],
z=quat_xyzw[2], w=quat_xyzw[3])),
)
def scalar(t_ns, value):
return Signal(timestamp=stamp(t_ns), number=float(value))
def flag(t_ns, value):
return Signal(timestamp=stamp(t_ns), flag=bool(value))
def text(t_ns, value):
return Signal(timestamp=stamp(t_ns), text=str(value))
def vector(t_ns, values):
return Signal(timestamp=stamp(t_ns), vector=Vector(values=[float(v) for v in values]))
def keypoints(t_ns, frame_id, positions, confidence=None):
return Skeleton(timestamp=stamp(t_ns), frame_id=frame_id,
positions=[Point3(x=p[0], y=p[1], z=p[2]) for p in positions],
confidence=list(confidence or []))
# ---------------------------------------------------------------------------------------
# An end-to-end example: 3 seconds of a Franka arm with an external and a wrist camera.
# Replace the synthetic data with reads from your own logs.
T0_NS = 1_760_000_000_000_000_000 # absolute Unix epoch nanoseconds
JOINTS = ["j1", "j2", "j3", "j4", "j5", "j6", "j7"]
overlay = {
"spec_version": "1.1",
"recording": {
"id": "franka-demo-0001",
"clock": "log_time",
"t0_ns": T0_NS,
"task": {"instruction": "Pick up the red cube and place it in the bowl."},
"platform": {
"kind": "manipulator",
"embodiment": "franka_panda",
"parts": [{
"name": "arm", "type": "arm",
"joints": {"signal": "arm_joints", "names": JOINTS},
"end_pose": {"channel": "/arm/ee_pose"},
"grip": {"signal": "gripper_opening"},
}],
},
},
"primary_view": "/cam/external",
"views": [
{"channel": "/cam/external", "role": "external_1", "label": "External"},
{"channel": "/cam/wrist", "role": "wrist_right", "label": "Wrist"},
],
"signals": [
{"channel": "/sig/arm_joints", "name": "arm_joints", "type": "vector",
"dim": 7, "components": JOINTS, "unit": "rad", "rate_hz": 100},
{"channel": "/sig/gripper_opening", "name": "gripper_opening", "type": "float",
"unit": "1", "rate_hz": 100},
{"channel": "/sig/control_mode", "name": "control_mode", "type": "enum",
"values": ["teleop", "autonomous"], "note": "who is controlling the arm"},
],
"source": {"dataset": "my-lab-teleop", "converter": "my-converter 0.1"},
}
def synthetic_jpeg(i, width=640, height=480):
rgb = np.zeros((height, width, 3), dtype=np.uint8)
rgb[..., 0] = (i * 8) % 255
buf = io.BytesIO()
Image.fromarray(rgb).save(buf, format="JPEG")
return buf.getvalue()
def main(path="franka-demo-0001.mcap"):
messages = [] # (t_ns, topic, message), written in time order below
for camera in ("external", "wrist"):
messages.append((T0_NS, f"/cam/{camera}/calibration",
calibration(T0_NS, f"cam_{camera}", 640, 480, 600, 600, 320, 240)))
for i in range(30): # 10 fps cameras
t = T0_NS + i * 100_000_000
for camera in ("external", "wrist"):
messages.append((t, f"/cam/{camera}", jpeg_image(t, f"cam_{camera}", synthetic_jpeg(i))))
for i in range(300): # 100 Hz proprioception
t = T0_NS + i * 10_000_000
q = [0.1 * math.sin(i / 50 + k) for k in range(7)]
messages.append((t, "/sig/arm_joints", vector(t, q)))
messages.append((t, "/sig/gripper_opening", scalar(t, 0.5 + 0.5 * math.cos(i / 40))))
messages.append((t, "/arm/ee_pose", pose(t, "robot_base", (0.5, 0.0, 0.3 + 0.001 * i),
(1.0, 0.0, 0.0, 0.0))))
messages.append((T0_NS, "/sig/control_mode", text(T0_NS, "teleop")))
writer = NomadicMcapWriter(path, overlay)
for t, topic, message in sorted(messages, key=lambda m: m[0]):
writer.write(topic, t, message)
writer.close()
if __name__ == "__main__":
main()