#!/usr/bin/env python3 """Restore a Visio device's calibration from calibration.json. pip install "visio-schema>=0.9.1" # Python 3.10+, while online # power the device on and plug its USB cable into this computer # (firmware 1.1.6+), or join its Wi-Fi (GILABS-XXXXXX), then: python3 set_calibration.py calibration.json It tries the USB link first, then Wi-Fi; --host picks one address. Turn off VPN / proxy apps first: in tunnel mode they capture the device link. calibration.json is the device's own calibration-store format plus a "serial" key naming the one unit it belongs to. The script refuses any other unit, sends one SetCalibration per artifact, waits for each ack, then checks that the device reports the new values back. """ from __future__ import annotations import argparse import json import re import socket import subprocess import sys import time from dataclasses import dataclass from typing import Callable from visio_schema.foxglove import CameraCalibration_pb2, FrameTransform_pb2 from visio_schema.transport.framing import extract_frames, frame_bytes from visio_schema.v1.calibration import imu_pb2 from visio_schema.v1.control import command_pb2, command_result_pb2 from visio_schema.v1.service.device_info import device_info_pb2 from visio_schema.wire.control import ControlStream from visio_schema.wire.message import Message COMMAND_STREAM = int(ControlStream.CONTROL_STREAM_COMMAND) DEVICE_INFO_STREAM = int(ControlStream.CONTROL_STREAM_DEVICE_INFO) SetCalibration = command_pb2.SetCalibration BUS_PORT = 50001 WIFI_HOST = "192.168.4.1" # the device's own Wi-Fi gateway def usb_host_for(serial: str) -> str: """The unit's address on its USB network link (CDC-NCM): the firmware puts each unit on 10...2.""" return f"10.{int(serial[0:2], 16)}.{int(serial[2:4], 16)}.2" def tethered_hosts() -> list[str]: """Every device on a USB link to this computer. A device hands the computer 10.a.b.1 and sits at 10.a.b.2, so each local 10.a.b.1 marks one. Read from the OS's own address listing, whatever its language.""" for cmd in (["ip", "-4", "-o", "addr"], ["ifconfig"], ["ipconfig"]): try: out = subprocess.run(cmd, capture_output=True, text=True, errors="replace", timeout=5).stdout except (OSError, subprocess.SubprocessError): continue found = re.findall(r"\b10\.(\d{1,3})\.(\d{1,3})\.1\b", out) return [f"10.{a}.{b}.2" for a, b in found] return [] def candidate_hosts(serial: str) -> list[str]: """Where to look, best first: this unit's own USB address, any other tethered device (so a wrong unit gets named, not just missed), then the device Wi-Fi.""" hosts = [usb_host_for(serial), *tethered_hosts(), WIFI_HOST] return list(dict.fromkeys(hosts)) @dataclass class Artifact: """One calibration item: the command that sets it, and how to confirm it from the device's 1 Hz report on `report_topic`.""" label: str command: SetCalibration report_topic: str matches: Callable[[bytes], bool] class DeviceLink: """TCP bus connection: frames messages, learns topics from announces.""" def __init__(self, host: str, port: int): self.sock = socket.create_connection((host, port), timeout=3) self.sock.settimeout(0.5) self.sock.sendall(b"\x00") # flush any half frame on the device self.rx = bytearray() self.topics: dict[str, int] = {} self.info: device_info_pb2.DeviceInfo | None = None def close(self) -> None: self.sock.close() def send(self, stream_id: int, payload: bytes) -> None: msg = Message(stream_id=stream_id, payload=payload) self.sock.sendall(frame_bytes(msg)) def messages(self, seconds: float): """Yield every message received in the next `seconds`.""" deadline = time.monotonic() + seconds while time.monotonic() < deadline: try: data = self.sock.recv(65536) except socket.timeout: continue if not data: raise ConnectionError("device closed the connection") self.rx.extend(data) for m in extract_frames(self.rx): if m.stream_id == DEVICE_INFO_STREAM: self._learn(m.payload) yield m def _learn(self, payload: bytes) -> None: info = device_info_pb2.DeviceInfo() info.ParseFromString(payload) for ch in info.channels: self.topics[ch.topic] = ch.id if info.serial: self.info = info def stream_for(self, suffix: str) -> int | None: return next((sid for topic, sid in self.topics.items() if topic.endswith(suffix)), None) # ── calibration.json -> artifacts ──────────────────────────────────────── def close(a: float, b: float) -> bool: # The device stores and reports doubles, so values come back near-exact. # Relative: IMU noise densities are ~1e-4, below any absolute bound. return abs(a - b) <= 1e-9 + 1e-6 * abs(b) def all_close(got, want) -> bool: return len(got) == len(want) and all(map(close, got, want)) def intrinsics_artifact(i: int, intr: dict) -> Artifact: cc = CameraCalibration_pb2.CameraCalibration( frame_id=f"cam{i}", width=intr["width"], height=intr["height"], distortion_model=intr["model"], D=intr["d"], K=intr["k"], R=intr["r"], P=intr["p"]) def matches(payload: bytes) -> bool: got = CameraCalibration_pb2.CameraCalibration.FromString(payload) # A device running another capture mode reports K rescaled to # that mode; only the model and D survive the rescale unchanged. same_mode = (got.width, got.height) == (cc.width, cc.height) return (got.distortion_model == cc.distortion_model and all_close(got.D, cc.D) and (not same_mode or all_close(got.K, cc.K))) cmd = SetCalibration(sensor_kind=SetCalibration.CAMERA, sensor_index=i, intrinsics=cc) return Artifact(f"cam{i} intrinsics", cmd, f"/camera/{i}/intrinsics", matches) def pose_artifact(kind: str, i: int, pose: dict) -> Artifact: ft = FrameTransform_pb2.FrameTransform( parent_frame_id="cam0", child_frame_id=f"{kind}{i}") ft.translation.x, ft.translation.y, ft.translation.z = pose["t"] (ft.rotation.x, ft.rotation.y, ft.rotation.z, ft.rotation.w) = pose["q"] def matches(payload: bytes) -> bool: got = FrameTransform_pb2.FrameTransform.FromString(payload) t, r = got.translation, got.rotation return (all_close([t.x, t.y, t.z], pose["t"]) and all_close([r.x, r.y, r.z, r.w], pose["q"])) sensor_kind = (SetCalibration.CAMERA if kind == "cam" else SetCalibration.IMU) cmd = SetCalibration(sensor_kind=sensor_kind, sensor_index=i, extrinsics=ft) topic = f"/{'camera' if kind == 'cam' else 'imu'}/{i}/extrinsics" return Artifact(f"{kind}{i} extrinsics", cmd, topic, matches) IMU_INFO_FIELDS = ("accel_noise_density", "gyro_noise_density", "update_rate_hz", "time_offset_to_cam0_s") def imu_info_artifact(i: int, info: dict) -> Artifact: ic = imu_pb2.ImuCalibration(**{f: info[f] for f in IMU_INFO_FIELDS}) def matches(payload: bytes) -> bool: got = imu_pb2.ImuCalibration.FromString(payload) return all(close(getattr(got, f), info[f]) for f in IMU_INFO_FIELDS) cmd = SetCalibration(sensor_kind=SetCalibration.IMU, sensor_index=i, imu_info=ic) return Artifact(f"imu{i} info", cmd, f"/imu/{i}/info", matches) def artifacts_from(calibration: dict) -> list[Artifact]: out = [] for i, cam in enumerate(calibration["cameras"]): out.append(intrinsics_artifact(i, cam["intrinsics"])) if "extrinsics" in cam: # cam0 is the anchor: none out.append(pose_artifact("cam", i, cam["extrinsics"])) for i, imu in enumerate(calibration["imus"]): out.append(imu_info_artifact(i, imu["info"])) out.append(pose_artifact("imu", i, imu["extrinsics"])) return out # ── device steps ───────────────────────────────────────────────────────── def wait_for_announce(link: DeviceLink) -> int | None: """The stream command acks arrive on, once the device has announced itself; None if nothing announced within 10 s.""" for _ in link.messages(10.0): result_stream = link.stream_for("/command_result") if link.info and result_stream is not None: return result_stream return None def find_unit(hosts: list[str], port: int) -> tuple[DeviceLink, int]: """Connect to the first address where a device announces itself. A bare TCP connect is not proof: VPN / proxy apps in tunnel mode accept any connection and then reset it, so each address must announce.""" for host in hosts: try: link = DeviceLink(host, port) result_stream = wait_for_announce(link) except OSError as e: print(f" no device at {host} ({e})") continue if result_stream is not None: print(f"connected to {host}") return link, result_stream print(f" no device at {host} (no announce)") link.close() sys.exit("error: cannot reach the device - plug in its USB cable " "(firmware 1.1.6+) or join its Wi-Fi (GILABS-XXXXXX), and turn " "off any VPN / proxy app") def check_unit(link: DeviceLink, serial: str) -> None: """Refuse to touch any unit but the one this calibration belongs to.""" if link.info.serial != serial: sys.exit(f"error: connected device is {link.info.serial}, but this " f"calibration is for {serial}. Nothing was changed.") print(f"device {serial} ({link.info.device_name}, firmware " f"{link.info.firmware_version})") def push(link: DeviceLink, result_stream: int, item: Artifact, command_id: int) -> None: cmd = command_pb2.Command(command_id=command_id) cmd.set_calibration.CopyFrom(item.command) link.send(COMMAND_STREAM, cmd.SerializeToString()) for m in link.messages(10.0): if m.stream_id != result_stream: continue result = command_result_pb2.CommandResult.FromString(m.payload) if result.command_id != command_id: continue if not result.ok: sys.exit(f"error: device rejected {item.label}: " f"{result.error_code} {result.error_message}") print(f" set {item.label}") return sys.exit(f"error: no answer from the device for {item.label}") def verify(link: DeviceLink, items: list[Artifact]) -> bool: """Wait for the device's 1 Hz report of every item and compare.""" waiting = {item.report_topic: item for item in items} verdicts: dict[str, bool] = {} for m in link.messages(8.0): topic = next((t for t in waiting if link.stream_for(t) == m.stream_id), None) if topic is not None: verdicts[topic] = waiting.pop(topic).matches(m.payload) if not waiting: break for item in items: ok = verdicts.get(item.report_topic) state = {True: "ok", False: "MISMATCH", None: "NOT REPORTED"}[ok] print(f" check {item.label}: {state}") return len(verdicts) == len(items) and all(verdicts.values()) def main() -> int: ap = argparse.ArgumentParser(description=__doc__.split("\n")[0]) ap.add_argument("calibration", help="calibration.json for this unit") ap.add_argument("--host", help="device address (default: USB link, then Wi-Fi)") ap.add_argument("--port", type=int, default=BUS_PORT) ap.add_argument("--dry-run", action="store_true", help="check the file and list what would be set") args = ap.parse_args() with open(args.calibration, encoding="utf-8") as f: calibration = json.load(f) serial = calibration["serial"] items = artifacts_from(calibration) print(f"{args.calibration}: {len(items)} items for device {serial}") if args.dry_run: for item in items: print(f" would set {item.label}") return 0 hosts = [args.host] if args.host else candidate_hosts(serial) link, result_stream = find_unit(hosts, args.port) check_unit(link, serial) try: for command_id, item in enumerate(items, start=1): push(link, result_stream, item, command_id) verified = verify(link, items) except OSError as e: sys.exit(f"error: lost the connection to the device ({e})") if verified: print("DONE: calibration restored. Keep the device on for a few " "seconds before powering it off.") return 0 print("FAILED: the device does not report the calibration that was " "sent. Send this output to support.", file=sys.stderr) return 1 if __name__ == "__main__": sys.exit(main())