diff --git a/ros2-poc/nodes/bridge_node.py b/ros2-poc/nodes/bridge_node.py index 60df842..e062775 100644 --- a/ros2-poc/nodes/bridge_node.py +++ b/ros2-poc/nodes/bridge_node.py @@ -136,6 +136,11 @@ def load_bridge_module(path: Path = BRIDGE_PY): SCANIMAGE_CMD_TOPIC = "/scanimage/cmd" SCANIMAGE_REPLY_TOPIC = "/scanimage/reply" SCANIMAGE_STATUS_TOPIC = "/scanimage/status" +# The SiPM controller (nodes/sipm_node.py, read-only): its 1 Hz status carries bias/enable/offsets/ +# temperature, the detector settings ScanImage's TIFF header does not have. +SIPM_CMD_TOPIC = "/sipm/cmd" +SIPM_REPLY_TOPIC = "/sipm/reply" +SIPM_STATUS_TOPIC = "/sipm/status" # ScanImage's ZeroMQ frame stream as topics (nodes/scanimage_frames_node.py, LAB-303 Block R). STREAMS: # recorded into the bag as received, like /bias/frames, and never derived into a run-log line (the # telemetry rule: a string-tagged stream is not a behavior sample). The preview images are bounded at @@ -155,6 +160,7 @@ def load_bridge_module(path: Path = BRIDGE_PY): CONNECTOR_TOPICS = { "bias": {"cmd": BIAS_CMD_TOPIC, "reply": BIAS_REPLY_TOPIC, "status": BIAS_STATUS_TOPIC}, "scanimage": {"cmd": SCANIMAGE_CMD_TOPIC, "reply": SCANIMAGE_REPLY_TOPIC, "status": SCANIMAGE_STATUS_TOPIC}, + "sipm": {"cmd": SIPM_CMD_TOPIC, "reply": SIPM_REPLY_TOPIC, "status": SIPM_STATUS_TOPIC}, } FRAME_TOPIC = "/arena/frame" RAW_QOS = QoSProfile( @@ -305,6 +311,7 @@ def topics(self) -> dict[str, str]: SESSION_TOPIC: STRING_TYPE, BROWSER_TOPIC: STRING_TYPE, EVENT_TOPIC: STRING_TYPE, KINEFLY_TOPIC: STRING_TYPE, BIAS_CMD_TOPIC: STRING_TYPE, BIAS_REPLY_TOPIC: STRING_TYPE, BIAS_STATUS_TOPIC: STRING_TYPE, BIAS_FRAMES_TOPIC: STRING_TYPE, SCANIMAGE_CMD_TOPIC: STRING_TYPE, SCANIMAGE_REPLY_TOPIC: STRING_TYPE, SCANIMAGE_STATUS_TOPIC: STRING_TYPE, + SIPM_CMD_TOPIC: STRING_TYPE, SIPM_REPLY_TOPIC: STRING_TYPE, SIPM_STATUS_TOPIC: STRING_TYPE, **STREAM_TOPICS, } if self._typed: @@ -859,7 +866,8 @@ def on_log_open() -> None: "record": {"bag": log.bag_name, "profile": log._profile, "typed": typed}, "topics": {"fictrac": FICTRAC_FRAME_TOPIC if typed else FICTRAC_TOPIC, "kinefly": KINEFLY_TOPIC, "bias": [BIAS_CMD_TOPIC, BIAS_REPLY_TOPIC, BIAS_STATUS_TOPIC, BIAS_FRAMES_TOPIC], - "scanimage": [SCANIMAGE_CMD_TOPIC, SCANIMAGE_REPLY_TOPIC, SCANIMAGE_STATUS_TOPIC], "frame": FRAME_TOPIC, + "scanimage": [SCANIMAGE_CMD_TOPIC, SCANIMAGE_REPLY_TOPIC, SCANIMAGE_STATUS_TOPIC], + "sipm": [SIPM_CMD_TOPIC, SIPM_REPLY_TOPIC, SIPM_STATUS_TOPIC], "frame": FRAME_TOPIC, "browser": BROWSER_TOPIC, "event": EVENT_TOPIC, "session": SESSION_TOPIC}, } ) diff --git a/ros2-poc/nodes/mcap_convert.py b/ros2-poc/nodes/mcap_convert.py index fc3e589..20e1645 100644 --- a/ros2-poc/nodes/mcap_convert.py +++ b/ros2-poc/nodes/mcap_convert.py @@ -26,6 +26,7 @@ /kinefly/raw the adapter envelope (DERIVED stream) → ["kf", ms, seq, t_src, L, R, head, abd] /bias/reply, /bias/status the BIAS node's JSON (DERIVED) → {"type":"connector","src":"bias",…} /scanimage/reply, /scanimage/status the ScanImage node's JSON (DERIVED) → {"type":"connector","src":"scanimage",…} + /sipm/reply, /sipm/status the SiPM node's JSON (DERIVED) → {"type":"connector","src":"sipm",…} /fictrac/raw | /fictrac/frame the adapter's FicTrac records → used only for level `full` Clock and order rules (comparison.md §4, B1/B2): @@ -76,11 +77,14 @@ TOPIC_BIAS_STATUS = "/bias/status" TOPIC_SCANIMAGE_REPLY = "/scanimage/reply" TOPIC_SCANIMAGE_STATUS = "/scanimage/status" +TOPIC_SIPM_REPLY = "/sipm/reply" +TOPIC_SIPM_STATUS = "/sipm/status" # Connector device topics → (connector id, event kind): their JSON becomes a DERIVED # {"type":"connector","src":,"kind":…} line (mirror of bridge_node.CONNECTOR_TOPICS). CONNECTOR_TOPIC_SRC = { TOPIC_BIAS_REPLY: ("bias", "reply"), TOPIC_BIAS_STATUS: ("bias", "status"), TOPIC_SCANIMAGE_REPLY: ("scanimage", "reply"), TOPIC_SCANIMAGE_STATUS: ("scanimage", "status"), + TOPIC_SIPM_REPLY: ("sipm", "reply"), TOPIC_SIPM_STATUS: ("sipm", "status"), } @@ -298,7 +302,7 @@ def fictrac_fields(rec: dict) -> tuple[int, list[float]] | None: def convert(records: list[dict], level: str | None = None, order: str = "seq", connectors=None) -> tuple[list, dict]: """→ (lines as JSON values, report). `level` None = the level the run was opened with.""" - stats = {"frames": 0, "browser_lines": 0, "rows": 0, "events": 0, "kf": 0, "bias": 0, "scanimage": 0, + stats = {"frames": 0, "browser_lines": 0, "rows": 0, "events": 0, "kf": 0, "bias": 0, "scanimage": 0, "sipm": 0, "connector_cmds": 0, "skipped": 0, "unordered_inserts": 0} opens = [r for r in records if r["topic"] == TOPIC_SESSION] session_open = None diff --git a/ros2-poc/nodes/rig.py b/ros2-poc/nodes/rig.py index e754925..0a465d7 100644 --- a/ros2-poc/nodes/rig.py +++ b/ros2-poc/nodes/rig.py @@ -8,6 +8,7 @@ pixi run rig -- --scanimage # + the ScanImage node (JSON server on this PC); --si-mock for a stand-in pixi run rig -- --foxglove # + foxglove_bridge (:8766) from the `viz` environment pixi run rig -- --scanimage-frames # + the ScanImage ZMQ frame-stream node (SUB tcp://127.0.0.1:65179; Bergamo, Block R) + pixi run rig -- --sipm --sipm-args "--dll-dir C:\\…\\Release" # + the read-only SiPM controller node (Bergamo, Block S) Order: the router first (nodes retry until it answers on 127.0.0.1:7447), then every adapter/node. Each child's stderr is prefixed with its name. Ctrl-C stops everything in @@ -141,6 +142,8 @@ def main(argv: list[str] | None = None) -> int: p.add_argument("--scanimage", action="store_true", help="also run the ScanImage node (talks to ScanImage's JSON server, default 127.0.0.1:5555)") p.add_argument("--scanimage-frames", action="store_true", help="also run the ScanImage frame-stream node (ZMQ SUB on ScanImage's ZMQAddress → /scanimage/frame_meta, frame_stats, preview/ch)") p.add_argument("--scanimage-frames-args", default="", help="extra args for scanimage_frames_node.py (e.g. --address tcp://127.0.0.1:65179 --preview-hz 5 --dump frames.jsonl)") + p.add_argument("--sipm", action="store_true", help="also run the read-only SiPM controller node (nodes/sipm_node.py; needs --sipm-args \"--dll-dir …\" or \"--fake\")") + p.add_argument("--sipm-args", default="", help="extra args for sipm_node.py (e.g. --dll-dir --params-file sipm_params.json)") p.add_argument("--si-mock", action="store_true", help="also run tests/si_json_mock.py (a stand-in for ScanImage's JSON server, port 5555)") p.add_argument("--sim", action="store_true", help="also run the FicTrac simulator (random walk, 50 Hz)") p.add_argument("--viz-args", default="", help="extra args for viz_node.py (--deg-per-frame, --frame-dir, --feature-az0, --frames)") @@ -212,6 +215,9 @@ def main(argv: list[str] | None = None) -> int: if args.scanimage_frames: # Connects (never binds), so no port pre-flight: ScanImage binds at each acquisition start. nodes.append(Child("si-frame", [py, str(NODES / "scanimage_frames_node.py"), *split_args(args.scanimage_frames_args)])) + if args.sipm: + # Owns the COM port while it runs: ThorSiPM_Control.exe must be closed (one program per port). + nodes.append(Child("sipm", [py, str(NODES / "sipm_node.py"), *split_args(args.sipm_args)])) bridge = Child("bridge", [py, str(NODES / "bridge_node.py"), *split_args(args.bridge_args)]) nodes.append(bridge) if foxglove: diff --git a/ros2-poc/nodes/sipm_cmd.py b/ros2-poc/nodes/sipm_cmd.py new file mode 100644 index 0000000..abdd6a0 --- /dev/null +++ b/ros2-poc/nodes/sipm_cmd.py @@ -0,0 +1,100 @@ +#!/usr/bin/env python3 +"""sipm_cmd.py — send one command to sipm_node.py and print its reply (all commands are read-only). + + pixi run sipm-cmd -- status + pixi run sipm-cmd -- get --name PARAM_PMT1_ENABLE + pixi run sipm-cmd -- get --id 1201 + pixi run sipm-cmd -- info --id 1201 + pixi run sipm-cmd -- scan --from 0 --to 2000 --out sipm_scan.json + pixi run sipm-cmd -- disconnect # free the COM port (e.g. to reopen ThorSiPM_Control.exe) + pixi run sipm-cmd -- reconnect + +Exit 0 on `ok`, 1 on an error reply, 2 on no reply. +""" + +from __future__ import annotations + +import argparse +import json +import sys +import threading +import time +import uuid +from pathlib import Path + +import rclpy +from rclpy.node import Node +from rclpy.qos import QoSDurabilityPolicy, QoSHistoryPolicy, QoSProfile, QoSReliabilityPolicy +from std_msgs.msg import String + +QOS = QoSProfile( + reliability=QoSReliabilityPolicy.RELIABLE, + history=QoSHistoryPolicy.KEEP_LAST, + depth=50, + durability=QoSDurabilityPolicy.VOLATILE, +) + + +def main(argv: list[str] | None = None) -> int: + p = argparse.ArgumentParser(description=__doc__, formatter_class=argparse.RawDescriptionHelpFormatter) + p.add_argument("command", help="ping | status | get | info | scan | disconnect | reconnect") + p.add_argument("--id", type=int, help="get/info: parameter ID") + p.add_argument("--name", help="get/info: a --params-file name or a PARAM_* name") + p.add_argument("--from", dest="first", type=int, help="scan: first ID (default 0)") + p.add_argument("--to", dest="last", type=int, help="scan: last ID (default 2000)") + p.add_argument("--timeout", type=float, default=30.0, help="seconds to wait for the reply (default: 30; a full scan takes a while)") + p.add_argument("--json", action="store_true", help="print the raw reply JSON") + p.add_argument("--out", help="also write the reply JSON to this file") + args, ros_args = p.parse_known_args(argv) + params = {k: v for k, v in (("id", args.id), ("name", args.name), ("from", args.first), ("to", args.last)) if v is not None} + cid = str(uuid.uuid4())[:8] + + rclpy.init(args=ros_args) + node = Node("sipm_cmd_" + cid) + got: dict = {} + done = threading.Event() + + def on_reply(msg: String) -> None: + try: + rep = json.loads(msg.data) + except json.JSONDecodeError: + return + if isinstance(rep, dict) and rep.get("id") == cid: + got.update(rep) + done.set() + + node.create_subscription(String, "/sipm/reply", on_reply, QOS) + pub = node.create_publisher(String, "/sipm/cmd", QOS) + time.sleep(0.5) # discovery + m = String() + m.data = json.dumps({"id": cid, "command": args.command, "params": params}) + pub.publish(m) + deadline = time.monotonic() + args.timeout + while not done.is_set() and time.monotonic() < deadline: + rclpy.spin_once(node, timeout_sec=0.1) + node.destroy_node() + try: + rclpy.try_shutdown() + except Exception: # rclpy's own SIGINT handler may already have shut the context down (races in Jazzy) + pass + if not got: + print(f"[sipm-cmd] no reply within {args.timeout:g}s (is `pixi run sipm-node` running?)", file=sys.stderr) + return 2 + if args.out: + Path(args.out).write_text(json.dumps(got, indent=1), encoding="utf-8") + if args.json: + print(json.dumps(got, indent=1)) + else: + v = got.get("value") + if args.command == "scan" and isinstance(v, dict): + for r in v.get("available", []): + print(f" {r['id']:5d} {r.get('name') or '?':40s} value={r.get('value')!r}") + print(f"{len(v.get('available', []))} available; not in the GUI enum: {v.get('not_in_enum')}") + print(f"{got.get('command')} → {got.get('outcome')} ({got.get('rtt_ms')} ms)" + + (f" value={json.dumps(v)[:300]}" if v not in (None, "") and args.command != "scan" else "") + + (f" error={got.get('error')}" if got.get("error") else "")) + return 0 if got.get("ok") else 1 + + +if __name__ == "__main__": + raise SystemExit(main()) diff --git a/ros2-poc/nodes/sipm_node.py b/ros2-poc/nodes/sipm_node.py new file mode 100644 index 0000000..d3e7020 --- /dev/null +++ b/ros2-poc/nodes/sipm_node.py @@ -0,0 +1,266 @@ +#!/usr/bin/env python3 +"""sipm_node.py — the Thorlabs SiPM controller as a ROS 2 node, READ-ONLY (Bergamo rig). + +Neither ScanImage nor the TIFF header knows the SiPM settings (bias, enable, offsets, bandwidth, +temperature). This node reads them through the vendor DLL (nodes/thorsipm.py, no SetParam +anywhere) and publishes them, so they land in the run's bag and run log next to ScanImage and the +arena. Same connector shape as bias_node.py / scanimage_node.py, all std_msgs/String JSON: + + /sipm/cmd IN {"id": , "command": , "params": {…}} + /sipm/reply OUT {"id", "command", "params", "ok", "outcome": "ok"|"error", "value", "error", + "rtt_ms", "t_ms", "rx_epoch_ms"} + /sipm/status OUT every --status-every s: {"reachable", "connected", "level": "ok"|"warn"|"error", + "warnings", "read_ms", "t_ms", "connected_detectors", "control_unit", + "detectors": {"1": {enable, gain_pos, gain_volts, output_offset, bandwidth_pos, + safety, saturations, detector_type, serial, …}, …}, "extra": {name: value}} + +Commands (none changes a detector setting): + ping / status the status snapshot now + get(id | name) one parameter by ID, or by a name from --params-file + info(id) GetParamInfo for one ID + scan(from, to) every available ID in the range with its value (Block S, step S2) + disconnect TeardownDevice: frees the COM port (e.g. to reopen the vendor GUI); + the node then stays disconnected until `reconnect` + reconnect open the controller again + +The node OWNS the controller while connected: ThorSiPM_Control.exe must be closed (one program +per COM port). If the port is busy the node keeps publishing `connected: false` with the reason +and retries every --reconnect-every s. Every DLL call runs on one worker thread. + + pixi run sipm-node -- --dll-dir "C:\\…\\ThorSiPM_Unreleased_9_25_25_1\\Release" --params-file sipm_params.json + pixi run sipm-node -- --fake # off-rig: the simulated controller + pixi run sipm-cmd -- scan --from 0 --to 2000 --json +""" + +from __future__ import annotations + +import argparse +import json +import queue +import sys +import threading +import time + +import rclpy +from rclpy.node import Node +from std_msgs.msg import String + +from _common import RELIABLE_QOS, raise_timer_resolution, spin, wall_ms +from thorsipm import KNOWN_PARAMS, SiPMError, ThorSiPM, load_params_file + +CMD_TOPIC = "/sipm/cmd" +REPLY_TOPIC = "/sipm/reply" +STATUS_TOPIC = "/sipm/status" +QOS = RELIABLE_QOS +COMMANDS = ("ping", "status", "get", "info", "scan", "disconnect", "reconnect") + + +def assess(snap: dict) -> tuple[str, list[str]]: + """Health from a snapshot: 'warn' on a tripped/saturated detector or a non-zero temperature + status (extra names containing 'tempstatus'); the raw values stay in the record.""" + warnings = [] + for d, rec in (snap.get("detectors") or {}).items(): + if rec.get("safety") not in (None, 0, 0.0): + warnings.append(f"detector {d}: safety={rec['safety']}") + if rec.get("saturations") not in (None, 0, 0.0): + warnings.append(f"detector {d}: saturations={rec['saturations']}") + for name, v in (snap.get("extra") or {}).items(): + if "tempstatus" in name.lower() and v not in (None, 0, 0.0): + warnings.append(f"{name}={v}") + return ("warn" if warnings else "ok"), warnings + + +class SiPMNode(Node): + def __init__(self, dev: ThorSiPM, extra: dict[str, int], status_every_s: float, reconnect_every_s: float) -> None: + super().__init__("sipm") + self.dev = dev + self.extra = extra + self.reconnect_every_s = reconnect_every_s + self.reply_pub = self.create_publisher(String, REPLY_TOPIC, QOS) + self.status_pub = self.create_publisher(String, STATUS_TOPIC, QOS) + self.create_subscription(String, CMD_TOPIC, self._on_cmd, QOS) + self._q: queue.Queue = queue.Queue() + self._status_pending = False + self._hold = False # True after `disconnect`: no automatic reconnect + self._last_try = 0.0 + self._last_error: str | None = None + self.commands = 0 + self._stopping = False # set by shutdown(): the node is destroyed, publish nothing more + self._worker = threading.Thread(target=self._work, name="sipm-dll", daemon=True) + self._worker.start() + self._q.put(("connect", None)) + if status_every_s > 0: + self.create_timer(status_every_s, self._enqueue_status) + self.get_logger().info(f"SiPM controller ← {CMD_TOPIC} → {REPLY_TOPIC}, {STATUS_TOPIC} every {status_every_s:g}s; " + f"{len(extra)} extra param(s): {', '.join(extra) or '-'}") + + # ── publishing ─────────────────────────────────────────────────────────────────────────── + def _publish(self, pub, obj: dict) -> None: + if self._stopping: + return + obj.setdefault("rx_epoch_ms", wall_ms()) + m = String() + m.data = json.dumps(obj, separators=(",", ":"), default=str) + pub.publish(m) + + def _on_cmd(self, msg: String) -> None: + try: + req = json.loads(msg.data) + except json.JSONDecodeError: + self._publish(self.reply_pub, {"id": None, "ok": False, "outcome": "error", "error": "command is not JSON"}) + return + if isinstance(req, dict): + self._q.put(("cmd", req)) + + def _enqueue_status(self) -> None: + if not self._status_pending: # coalesce: one poll waiting at most + self._status_pending = True + self._q.put(("status", None)) + + # ── the worker: every DLL call happens here ────────────────────────────────────────────── + def _work(self) -> None: + while True: + kind, payload = self._q.get() + try: + if kind == "connect": + self._connect() + elif kind == "status": + self._status_pending = False + self._do_status() + elif kind == "stop": + self.dev.close() + return + else: + self._do_cmd(payload) + except Exception as exc: # a DLL surprise must not kill the worker + self.get_logger().error(f"{kind} failed: {exc!r}") + if kind == "cmd": + self._publish(self.reply_pub, {"id": (payload or {}).get("id"), "command": (payload or {}).get("command"), + "ok": False, "outcome": "error", "error": f"node error: {exc!r}"}) + + def _connect(self) -> bool: + self._last_try = time.monotonic() + try: + n = self.dev.open() + except SiPMError as exc: + if str(exc) != self._last_error: + self.get_logger().warning(f"not connected: {exc}") + self._last_error = str(exc) + return False + self._last_error = None + self.get_logger().info(f"connected ({n} controller(s))") + return True + + def _snapshot(self) -> dict: + t_ms = wall_ms() + if not self.dev.is_open and not self._hold and time.monotonic() - self._last_try >= self.reconnect_every_s: + self._connect() + if not self.dev.is_open: + return {"reachable": False, "connected": False, "held": self._hold, "level": "error", + "error": "disconnected by command" if self._hold else (self._last_error or "not connected"), "t_ms": t_ms} + t0 = time.perf_counter() + try: + snap = self.dev.snapshot(self.extra) + except SiPMError as exc: + self.dev.close() + self._last_error = str(exc) + return {"reachable": False, "connected": False, "level": "error", "error": str(exc), "t_ms": t_ms} + level, warnings = assess(snap) + return {"reachable": True, "connected": True, "level": level, "warnings": warnings, + "read_ms": round((time.perf_counter() - t0) * 1000.0, 2), "t_ms": t_ms, **snap} + + def _do_status(self) -> None: + self._publish(self.status_pub, self._snapshot()) + + def _resolve_id(self, params: dict) -> int: + if "id" in params: + return int(params["id"]) + name = str(params.get("name", "")) + if name in self.extra: + return self.extra[name] + by_name = {v: k for k, v in KNOWN_PARAMS.items()} + if name in by_name: + return by_name[name] + raise ValueError(f"unknown parameter {name!r}: give `id`, a --params-file name, or a PARAM_* name") + + def _do_cmd(self, req: dict) -> None: + self.commands += 1 + cid, command, params = req.get("id"), str(req.get("command", "")), req.get("params") or {} + out: dict = {"id": cid, "command": command, "params": params, "t_ms": wall_ms()} + t0 = time.perf_counter() + try: + if command not in COMMANDS: + raise ValueError(f"unknown command {command!r} (one of {COMMANDS})") + if command == "disconnect": + self.dev.close() + self._hold = True + value = {"connected": False} + elif command == "reconnect": + self._hold = False + self.dev.close() + if not self._connect(): + raise SiPMError(self._last_error or "reconnect failed") + value = {"connected": True} + elif command in ("ping", "status"): + value = self._snapshot() + self._publish(self.status_pub, dict(value)) + else: + if not self.dev.is_open: + raise SiPMError("not connected" + (f": {self._last_error}" if self._last_error else "")) + if command == "get": + pid = self._resolve_id(params) + value = {"id": pid, "name": KNOWN_PARAMS.get(pid), "value": self.dev.get(pid)} + elif command == "info": + pid = self._resolve_id(params) + value = {"id": pid, "name": KNOWN_PARAMS.get(pid), "info": self.dev.info(pid)} + else: # scan + first, last = int(params.get("from", 0)), int(params.get("to", 2000)) + if last - first > 5000: + raise ValueError("scan range limited to 5000 IDs") + rows = self.dev.scan(first, last) + value = {"range": [first, last], "available": rows, "not_in_enum": [r["id"] for r in rows if not r["name"]]} + out.update({"ok": True, "outcome": "ok", "value": value}) + except (SiPMError, ValueError) as exc: + out.update({"ok": False, "outcome": "error", "error": str(exc)}) + out["rtt_ms"] = round((time.perf_counter() - t0) * 1000.0, 2) + self._publish(self.reply_pub, out) + if out["ok"]: + self.get_logger().info(f"{command} → ok ({out['rtt_ms']} ms)") + else: + self.get_logger().warning(f"{command} → error ({out['rtt_ms']} ms): {out.get('error')}") + + def shutdown(self, timeout_s: float = 3.0) -> None: + """Free the COM port on exit (TeardownDevice on the worker thread).""" + self._stopping = True + self._q.put(("stop", None)) + self._worker.join(timeout_s) + + +def main(argv: list[str] | None = None) -> int: + p = argparse.ArgumentParser(description=__doc__, formatter_class=argparse.RawDescriptionHelpFormatter) + p.add_argument("--dll-dir", help="folder with ThorSiPM.dll + ThorSiPMSettings.xml (the vendor GUI's Release folder)") + p.add_argument("--fake", action="store_true", help="use the simulated controller (off-rig tests)") + p.add_argument("--fake-busy", action="store_true", help="--fake with the COM port 'held by the GUI' (SelectDevice fails)") + p.add_argument("--params-file", help="JSON {name: id} of extra params to publish, e.g. the SiPM bias/temperature IDs from a scan") + p.add_argument("--status-every", type=float, default=1.0, help="status period in s; 0 = off (default: 1)") + p.add_argument("--reconnect-every", type=float, default=5.0, help="retry period while not connected, s (default: 5)") + args, ros_args = p.parse_known_args(argv) + try: + dev = ThorSiPM.from_args(args.dll_dir, fake=args.fake or args.fake_busy, fake_busy=args.fake_busy) + extra = load_params_file(args.params_file) + except (SiPMError, OSError, ValueError) as exc: + print(f"[sipm-node] {exc}", file=sys.stderr) + return 2 + raise_timer_resolution() + rclpy.init(args=ros_args) + node = SiPMNode(dev, extra, args.status_every, args.reconnect_every) + try: + spin(node) + finally: + node.shutdown() + print(f"[sipm-node] {node.commands} commands handled; controller released", file=sys.stderr) + return 0 + + +if __name__ == "__main__": + raise SystemExit(main()) diff --git a/ros2-poc/nodes/thorsipm.py b/ros2-poc/nodes/thorsipm.py new file mode 100644 index 0000000..9f2bfea --- /dev/null +++ b/ros2-poc/nodes/thorsipm.py @@ -0,0 +1,353 @@ +#!/usr/bin/env python3 +"""thorsipm.py — READ-ONLY driver for the Thorlabs SiPM controller (Bergamo rig), no ROS. + +The controller's software is `ThorSiPM_Control.exe` (an unreleased .NET build, 2025-09-25) on top +of a native `ThorSiPM.dll` that talks to the controller over a serial port (COM66 on Bergamo, set +in `ThorSiPMSettings.xml` next to the DLL). The DLL has ThorImage's standard device interface; +the signatures below are the GUI's own P/Invoke declarations, read from its .NET metadata +(scanimage-2026-bergamo, 2026-10-19; nothing decompiled): + + int FindDevices(ref int deviceCount) int SelectDevice(int device) + int GetParam(int paramID, ref double value) int GetParamString(int paramID, char* buf, int size) + int GetParamInfo(int paramID, ref int type, ref int available, ref int readOnly, + ref double min, ref double max, ref double default) + int TeardownDevice() (all return 1 = TRUE on success) + +This module wraps ONLY those. It has no SetParam / Start/Setup/Preflight call anywhere, so it +cannot change a detector setting. The parameter IDs in KNOWN_PARAMS are the GUI's `Params` enum; +the SiPM-specific ones the native DLL also names (PARAM_SIPM1_BIAS, …_TEMPERATURE, …_TEMPSTATUS, +…_OUTPUT_OFFSET_HIGH_GAIN/LOW_GAIN) have no ID in that enum, so `scan` finds them on the rig: +every ID whose GetParamInfo says "available", with its value — then match against the GUI. + +Only ONE program can hold the COM port: close ThorSiPM_Control.exe first (Block S, step S1). +The DLL opens `ThorSiPMSettings.xml` relative to the working directory, so `ThorSiPM.open()` +switches the process's working directory to the DLL folder (the node is a process of its own). + + python nodes/thorsipm.py scan --dll-dir "C:\\…\\ThorSiPM\\Release" --out sipm_scan.json + python nodes/thorsipm.py snapshot --dll-dir … [--params-file sipm_params.json] + python nodes/thorsipm.py watch --dll-dir … --period 1 --duration 60 + python nodes/thorsipm.py scan --fake # off-rig: the simulated controller below +""" + +from __future__ import annotations + +import argparse +import ctypes +import json +import os +import sys +import time +from pathlib import Path + +DLL_NAME = "ThorSiPM.dll" +MAX_DETECTORS = 4 + +# The GUI's `Params` enum (ThorSiPM_Control.dll metadata), the subset this controller can answer. +KNOWN_PARAMS: dict[int, str] = { + 700: "PARAM_PMT1_GAIN_POS", 701: "PARAM_PMT1_ENABLE", 702: "PARAM_PMT2_GAIN_POS", 703: "PARAM_PMT2_ENABLE", + 704: "PARAM_PMT3_GAIN_POS", 705: "PARAM_PMT3_ENABLE", 706: "PARAM_PMT4_GAIN_POS", 707: "PARAM_PMT4_ENABLE", + 713: "PARAM_PMT1_SAFETY", 714: "PARAM_PMT2_SAFETY", 715: "PARAM_PMT3_SAFETY", 716: "PARAM_PMT4_SAFETY", + 722: "PARAM_PMT1_BANDWIDTH_POS", 723: "PARAM_PMT2_BANDWIDTH_POS", 724: "PARAM_PMT3_BANDWIDTH_POS", 725: "PARAM_PMT4_BANDWIDTH_POS", + 743: "PARAM_CONTROL_UNIT_FIRMWAREUPDATE", 744: "PARAM_CONTROL_UNIT_FIRMWAREVERSION", 745: "PARAM_CONTROL_UNIT_SERIALNUMBER", + 750: "PARAM_PMT1_GAIN_POS_CURRENT_VOLTS", 751: "PARAM_PMT1_OUTPUT_OFFSET", 752: "PARAM_PMT1_OUTPUT_OFFSET_CURRENT", 753: "PARAM_PMT1_SERIALNUMBER", + 754: "PARAM_PMT2_GAIN_POS_CURRENT_VOLTS", 755: "PARAM_PMT2_OUTPUT_OFFSET", 756: "PARAM_PMT2_OUTPUT_OFFSET_CURRENT", 757: "PARAM_PMT2_SERIALNUMBER", + 758: "PARAM_PMT3_GAIN_POS_CURRENT_VOLTS", 759: "PARAM_PMT3_OUTPUT_OFFSET", 760: "PARAM_PMT3_OUTPUT_OFFSET_CURRENT", 761: "PARAM_PMT3_SERIALNUMBER", + 762: "PARAM_PMT4_GAIN_POS_CURRENT_VOLTS", 763: "PARAM_PMT4_OUTPUT_OFFSET", 764: "PARAM_PMT4_OUTPUT_OFFSET_CURRENT", 765: "PARAM_PMT4_SERIALNUMBER", + 782: "PARAM_CONNECTED_PMTS", + 783: "PARAM_PMT1_FIRMWAREUPDATE", 784: "PARAM_PMT2_FIRMWAREUPDATE", 785: "PARAM_PMT3_FIRMWAREUPDATE", 786: "PARAM_PMT4_FIRMWAREUPDATE", + 789: "PARAM_PMT1_TYPE", 790: "PARAM_PMT2_TYPE", 791: "PARAM_PMT3_TYPE", 792: "PARAM_PMT4_TYPE", + 920: "PARAM_PMT1_BANDWIDTH_POS_CURRENT", 921: "PARAM_PMT2_BANDWIDTH_POS_CURRENT", 922: "PARAM_PMT3_BANDWIDTH_POS_CURRENT", 923: "PARAM_PMT4_BANDWIDTH_POS_CURRENT", + 926: "PARAM_PMT1_SATURATIONS", 927: "PARAM_PMT2_SATURATIONS", 928: "PARAM_PMT3_SATURATIONS", 929: "PARAM_PMT4_SATURATIONS", + 932: "PARAM_PMT1_DETECTOR_TYPE", 933: "PARAM_PMT2_DETECTOR_TYPE", 934: "PARAM_PMT3_DETECTOR_TYPE", 935: "PARAM_PMT4_DETECTOR_TYPE", + 938: "PARAM_PMT1_OFFSET_STEP_SIZE", 939: "PARAM_PMT2_OFFSET_STEP_SIZE", 940: "PARAM_PMT3_OFFSET_STEP_SIZE", 941: "PARAM_PMT4_OFFSET_STEP_SIZE", + 944: "PARAM_PMT1_GAIN_STEP_SIZE", 945: "PARAM_PMT2_GAIN_STEP_SIZE", 946: "PARAM_PMT3_GAIN_STEP_SIZE", 947: "PARAM_PMT4_GAIN_STEP_SIZE", + 950: "PARAM_PMT1_AVAILABLE_BANDWIDTHS", 951: "PARAM_PMT2_AVAILABLE_BANDWIDTHS", 952: "PARAM_PMT3_AVAILABLE_BANDWIDTHS", 953: "PARAM_PMT4_AVAILABLE_BANDWIDTHS", + 956: "PARAM_PMT1_FIRMWAREVERSION", 957: "PARAM_PMT2_FIRMWAREVERSION", 958: "PARAM_PMT3_FIRMWAREVERSION", 959: "PARAM_PMT4_FIRMWAREVERSION", +} +# Never READ these either, even in a scan: their names say "firmware update", and a vendor +# GetParam with a side effect is not worth the risk. +NEVER_READ = frozenset({743, 783, 784, 785, 786}) +# Read with GetParamString instead of GetParam. +STRING_PARAMS = frozenset(i for i, n in KNOWN_PARAMS.items() if n.endswith(("SERIALNUMBER", "FIRMWAREVERSION"))) + + +def detector_params(n: int) -> dict[str, int]: + """The status fields for detector n (1-based) → param ID, from the GUI enum.""" + base = {"enable": 701, "gain_pos": 700, "gain_volts": 750, "output_offset": 751, "output_offset_current": 752, + "bandwidth_pos": 722, "bandwidth_pos_current": 920, "safety": 713, "saturations": 926, "detector_type": 932} + step = {"enable": 2, "gain_pos": 2, "gain_volts": 4, "output_offset": 4, "output_offset_current": 4, + "bandwidth_pos": 1, "bandwidth_pos_current": 1, "safety": 1, "saturations": 1, "detector_type": 1} + return {k: v + step[k] * (n - 1) for k, v in base.items()} + + +class SiPMError(RuntimeError): + pass + + +# ── backends: the real DLL (Windows) and a simulated controller (tests, off-rig) ───────────────── + +class DllBackend: + """ctypes binding of ThorSiPM.dll. Windows x64 only (the DLL is PE32+).""" + + def __init__(self, dll_dir: str | os.PathLike) -> None: + if os.name != "nt": + raise SiPMError("ThorSiPM.dll is a Windows DLL; use --fake off the rig") + self.dll_dir = Path(dll_dir).resolve() + if not (self.dll_dir / DLL_NAME).is_file(): + raise SiPMError(f"{DLL_NAME} not found in {self.dll_dir}") + os.add_dll_directory(str(self.dll_dir)) # SerialPortAccess.dll sits next to it + d = ctypes.CDLL(str(self.dll_dir / DLL_NAME)) + I, D, P = ctypes.c_int32, ctypes.c_double, ctypes.POINTER + sig = { + "FindDevices": [P(I)], "SelectDevice": [I], "TeardownDevice": [], + "GetParam": [I, P(D)], "GetParamString": [I, ctypes.c_char_p, I], + "GetParamInfo": [I, P(I), P(I), P(I), P(D), P(D), P(D)], + } + for name, argtypes in sig.items(): + f = getattr(d, name) + f.argtypes, f.restype = argtypes, I + self._d = d + + def find_devices(self) -> tuple[int, int]: + n = ctypes.c_int32(0) + return self._d.FindDevices(ctypes.byref(n)), n.value + + def select_device(self, i: int) -> int: + return self._d.SelectDevice(i) + + def teardown(self) -> int: + return self._d.TeardownDevice() + + def get_param(self, pid: int) -> tuple[int, float]: + v = ctypes.c_double(0.0) + return self._d.GetParam(pid, ctypes.byref(v)), v.value + + def get_param_string(self, pid: int, size: int = 256) -> tuple[int, str]: + buf = ctypes.create_string_buffer(size) + rc = self._d.GetParamString(pid, buf, size) + return rc, buf.value.decode("ascii", errors="replace").rstrip("\x00") + + def get_param_info(self, pid: int) -> tuple[int, dict]: + t, a, ro = ctypes.c_int32(), ctypes.c_int32(), ctypes.c_int32() + lo, hi, df = ctypes.c_double(), ctypes.c_double(), ctypes.c_double() + rc = self._d.GetParamInfo(pid, *(ctypes.byref(x) for x in (t, a, ro, lo, hi, df))) + return rc, {"type": t.value, "available": a.value, "read_only": ro.value, "min": lo.value, "max": hi.value, "default": df.value} + + +class FakeBackend: + """A simulated controller: two SiPMs (the Bergamo serial numbers) plus 'hidden' SiPM params + at IDs the GUI enum doesn't name, the way the real bias/temperature IDs are unknown.""" + + HIDDEN = {1201: 54.2, 1202: 54.6, 1211: 21.8, 1212: 22.3, 1221: 0.0, 1222: 0.0} # bias V ×2, temp °C ×2, temp status ×2 + + def __init__(self, busy: bool = False, n_detectors: int = 2) -> None: + self.busy = busy # True = the GUI holds the COM port: SelectDevice fails + self.selected = False + self.reads = 0 + self.vals: dict[int, float] = {782: n_detectors} + self.strs: dict[int, str] = {744: "1.2.3", 745: "CU000123"} + for n, sn in zip(range(1, n_detectors + 1), ("000027", "000026")): + p = detector_params(n) + self.vals.update({p["enable"]: 1, p["gain_pos"]: 0.80, p["gain_volts"]: 0.80, p["output_offset"]: 0.0, + p["output_offset_current"]: 0.0, p["bandwidth_pos"]: 2, p["bandwidth_pos_current"]: 2, + p["safety"]: 0, p["saturations"]: 0, p["detector_type"]: 3}) + self.strs[753 + 4 * (n - 1)] = sn + self.strs[956 + (n - 1)] = "5.0.1" + self.vals.update(self.HIDDEN) + + def find_devices(self) -> tuple[int, int]: + return 1, 1 + + def select_device(self, i: int) -> int: + if self.busy: + return 0 + self.selected = True + return 1 + + def teardown(self) -> int: + self.selected = False + return 1 + + def _ok(self, pid: int) -> bool: + return self.selected and (pid in self.vals or pid in self.strs or pid in NEVER_READ) + + def get_param(self, pid: int) -> tuple[int, float]: + self.reads += 1 + if not self.selected or pid not in self.vals: + return 0, 0.0 + v = self.vals[pid] + if pid in (1211, 1212): # a temperature that drifts a little + v += 0.01 * (self.reads % 7) + return 1, float(v) + + def get_param_string(self, pid: int, size: int = 256) -> tuple[int, str]: + return (1, self.strs[pid][: size - 1]) if self.selected and pid in self.strs else (0, "") + + def get_param_info(self, pid: int) -> tuple[int, dict]: + if not self._ok(pid): + return 0, {"type": 0, "available": 0, "read_only": 0, "min": 0.0, "max": 0.0, "default": 0.0} + t = 2 if pid in self.strs else 1 + return 1, {"type": t, "available": 1, "read_only": int(pid in self.strs or pid in self.HIDDEN), "min": 0.0, "max": 100.0, "default": 0.0} + + +# ── the driver ──────────────────────────────────────────────────────────────────────────────────── + +class ThorSiPM: + """Read-only access to one SiPM controller. `open()` → reads → `close()` (frees the COM port).""" + + def __init__(self, backend) -> None: + self.b = backend + self.is_open = False + self.n_devices = 0 + + @classmethod + def from_args(cls, dll_dir: str | None, fake: bool = False, fake_busy: bool = False) -> "ThorSiPM": + if fake: + return cls(FakeBackend(busy=fake_busy)) + if not dll_dir: + raise SiPMError("--dll-dir is required (the folder with ThorSiPM.dll and ThorSiPMSettings.xml)") + return cls(DllBackend(dll_dir)) + + def open(self, device: int = 0) -> int: + dll_dir = getattr(self.b, "dll_dir", None) + if dll_dir is not None: + os.chdir(dll_dir) # the DLL reads ThorSiPMSettings.xml from the working directory + rc, n = self.b.find_devices() + if rc != 1 or n < 1: + raise SiPMError(f"FindDevices found no controller (rc={rc}, count={n}): is it powered and on the COM port in ThorSiPMSettings.xml?") + if self.b.select_device(device) != 1: + raise SiPMError("SelectDevice failed: the COM port is probably held by ThorSiPM_Control.exe (close it), or the controller is off") + self.is_open, self.n_devices = True, n + return n + + def close(self) -> None: + if self.is_open: + self.b.teardown() + self.is_open = False + + def __enter__(self) -> "ThorSiPM": + self.open() + return self + + def __exit__(self, *exc) -> None: + self.close() + + def _need_open(self) -> None: + if not self.is_open: + raise SiPMError("not connected") + + def get(self, pid: int) -> float | str | None: + """One parameter's value, or None if the DLL refuses it. Strings via GetParamString.""" + self._need_open() + if pid in NEVER_READ: + raise SiPMError(f"param {pid} ({KNOWN_PARAMS.get(pid)}) is never read (firmware update)") + if pid in STRING_PARAMS: + rc, s = self.b.get_param_string(pid) + return s if rc == 1 else None + rc, v = self.b.get_param(pid) + return v if rc == 1 else None + + def info(self, pid: int) -> dict | None: + self._need_open() + rc, inf = self.b.get_param_info(pid) + return inf if rc == 1 else None + + def scan(self, first: int = 0, last: int = 2000) -> list[dict]: + """Every ID in [first, last] whose GetParamInfo answers and says available, with its value. + GetParam is called only for those, and never for NEVER_READ.""" + self._need_open() + out = [] + for pid in range(first, last + 1): + inf = self.info(pid) + if not inf or not inf.get("available"): + continue + row = {"id": pid, "name": KNOWN_PARAMS.get(pid), **inf} + if pid in NEVER_READ: + row["value"], row["note"] = None, "not read (firmware update)" + else: + try: + row["value"] = self.get(pid) + except SiPMError as exc: + row["value"], row["error"] = None, str(exc) + if pid not in STRING_PARAMS and row["value"] is None: + rc, s = self.b.get_param_string(pid) + if rc == 1 and s: + row["string"] = s + out.append(row) + return out + + def snapshot(self, extra: dict[str, int] | None = None, detectors: int | None = None) -> dict: + """The status record: control unit, per-detector fields (GUI enum), and `extra` named IDs + (e.g. {"sipm1_bias": }). Values the DLL refuses are None.""" + self._need_open() + n = detectors or int(self.get(782) or 0) or 2 + snap: dict = {"connected_detectors": n, "control_unit": {"serial": self.get(745), "firmware": self.get(744)}, "detectors": {}} + for d in range(1, min(n, MAX_DETECTORS) + 1): + rec = {k: self.get(pid) for k, pid in detector_params(d).items()} + rec["serial"] = self.get(753 + 4 * (d - 1)) + snap["detectors"][str(d)] = rec + if extra: + snap["extra"] = {name: self.get(pid) for name, pid in extra.items()} + return snap + + +def load_params_file(path: str | None) -> dict[str, int]: + """{"sipm1_bias": 1201, …} — the IDs identified from a scan (Block S, step S2).""" + if not path: + return {} + obj = json.loads(Path(path).read_text(encoding="utf-8")) + return {str(k): int(v) for k, v in obj.items() if not str(k).startswith("_")} + + +def main(argv: list[str] | None = None) -> int: + p = argparse.ArgumentParser(description=__doc__, formatter_class=argparse.RawDescriptionHelpFormatter) + p.add_argument("action", choices=("scan", "snapshot", "watch")) + p.add_argument("--dll-dir", help="folder with ThorSiPM.dll + ThorSiPMSettings.xml (the GUI's Release folder)") + p.add_argument("--fake", action="store_true", help="use the simulated controller (off-rig)") + p.add_argument("--from", dest="first", type=int, default=0, help="scan: first ID (default 0)") + p.add_argument("--to", dest="last", type=int, default=2000, help="scan: last ID (default 2000)") + p.add_argument("--params-file", help="JSON {name: id} of extra params to read (from the scan)") + p.add_argument("--period", type=float, default=1.0, help="watch: seconds between snapshots") + p.add_argument("--duration", type=float, default=60.0, help="watch: total seconds") + p.add_argument("--out", help="write the result as JSON (scan/snapshot) or JSON lines (watch)") + a = p.parse_args(argv) + out_path = Path(a.out).resolve() if a.out else None # before open() changes the working directory + extra = load_params_file(a.params_file and str(Path(a.params_file).resolve())) + dev = ThorSiPM.from_args(a.dll_dir, fake=a.fake) + t0 = time.perf_counter() + try: + n = dev.open() + print(f"[thorsipm] connected: {n} controller(s) found, device 0 selected ({(time.perf_counter() - t0) * 1000:.0f} ms)", file=sys.stderr) + if a.action == "scan": + rows = dev.scan(a.first, a.last) + res = {"t_epoch": time.time(), "range": [a.first, a.last], "available": rows} + unknown = [r for r in rows if not r["name"]] + print(f"[thorsipm] {len(rows)} available params, {len(unknown)} not in the GUI enum (SiPM bias/temperature candidates):", file=sys.stderr) + for r in rows: + print(f" {r['id']:5d} {r['name'] or '?':40s} value={r.get('value')!r} type={r['type']} ro={r['read_only']} range=[{r['min']:g}, {r['max']:g}]", file=sys.stderr) + text = json.dumps(res, indent=1) + (out_path.write_text(text, encoding="utf-8") if out_path else print(text)) + elif a.action == "snapshot": + text = json.dumps({"t_epoch": time.time(), **dev.snapshot(extra)}, indent=1) + (out_path.write_text(text, encoding="utf-8") if out_path else print(text)) + else: + fh = open(out_path, "a", encoding="utf-8") if out_path else None + end = time.monotonic() + a.duration + while time.monotonic() < end: + t1 = time.perf_counter() + rec = {"t_epoch": time.time(), **dev.snapshot(extra)} + rec["read_ms"] = round((time.perf_counter() - t1) * 1000, 1) + print(json.dumps(rec, separators=(",", ":")), file=fh or sys.stdout, flush=True) + time.sleep(max(0.0, a.period - (time.perf_counter() - t1))) + if fh: + fh.close() + except SiPMError as exc: + print(f"[thorsipm] {exc}", file=sys.stderr) + return 2 + finally: + dev.close() + return 0 + + +if __name__ == "__main__": + raise SystemExit(main()) diff --git a/ros2-poc/pixi.toml b/ros2-poc/pixi.toml index 3d090d5..37c8d1f 100644 --- a/ros2-poc/pixi.toml +++ b/ros2-poc/pixi.toml @@ -131,6 +131,11 @@ scanimage-node = "python nodes/scanimage_node.py" scanimage-cmd = "python nodes/scanimage_cmd.py" # ScanImage's ZeroMQ frame stream (LAB-303 Block R): SUB → /scanimage/frame_meta, frame_stats, preview/ch. scanimage-frames = "python nodes/scanimage_frames_node.py" +# The Thorlabs SiPM controller, READ-ONLY (Bergamo Block S): node, one-shot command, and the +# driver's own CLI (scan / snapshot / watch) for use without ROS. +sipm-node = "python nodes/sipm_node.py" +sipm-cmd = "python nodes/sipm_cmd.py" +sipm = "python nodes/thorsipm.py" # ── the bridge node: bridge.py's hub + log writer + dispatcher on ROS input ── bridge-node = "python nodes/bridge_node.py" # ── simulators (the root workspace's fictrac_sim.py, unmodified) ──────────── @@ -145,6 +150,7 @@ rig = "python nodes/rig.py" # `test` = the frame-stream harness (mock ZMQ publisher, ≈ 40 s) THEN the acceptance run; `-- args` reach the latter. test = { cmd = "python tests/test_ros2_poc.py", depends-on = ["test-frames"] } test-frames = "python tests/test_scanimage_frames.py" +test-sipm = "python tests/test_sipm.py" # latency / gaps / queue-depth report from a bag or the node's --dump JSONL (+ the matching TIFF) zmq-latency = "python tools/zmq_latency_report.py" studio-emulator = "python tests/studio_emulator.py" diff --git a/ros2-poc/tests/test_sipm.py b/ros2-poc/tests/test_sipm.py new file mode 100644 index 0000000..b2805ba --- /dev/null +++ b/ros2-poc/tests/test_sipm.py @@ -0,0 +1,295 @@ +#!/usr/bin/env python3 +"""test_sipm.py — the read-only SiPM controller driver and node, off-rig (Bergamo Block S). + + pixi run test-sipm # from ros2-poc/ (≈ 30 s) + pixi run test-sipm -- --keep + +Everything runs against thorsipm.FakeBackend (two SiPMs + 'hidden' bias/temperature params at IDs +the GUI enum doesn't name, the way the real ones are unknown until the rig scan): + + A driver, no ROS scan lists every available ID, flags the 6 not in the enum, never reads + the firmware-update params; snapshot fields + serials; a busy COM port + (GUI open) fails open() with a clear reason; no write path exists + B node, connected /sipm/status at ~1 Hz, connected, level ok, extra (params-file) values; + get by name / by id; info; scan finds the hidden IDs; disconnect holds + (no auto-reconnect), reconnect restores; clean exit + C node, port busy status says not connected with the reason and keeps publishing + D recording bridge_node registers /sipm/* in every bag and as a connector; the + converter derives /sipm/reply|status into connector events; the received + status messages round-trip through recorder.Recorder into an MCAP + +Exit 0 when every check passes. +""" + +from __future__ import annotations + +import argparse +import json +import re +import shutil +import signal +import subprocess +import sys +import tempfile +import threading +import time +import uuid +from pathlib import Path + +HERE = Path(__file__).resolve().parent +ROOT = HERE.parent +NODES = ROOT / "nodes" +PY = sys.executable +sys.path.insert(0, str(NODES)) +from _common import RELIABLE_QOS, SIGKILL, popen_group, signal_group, wait_port # noqa: E402 + +import rclpy # noqa: E402 +from rclpy.executors import SingleThreadedExecutor # noqa: E402 +from rclpy.node import Node # noqa: E402 +from rclpy.serialization import deserialize_message, serialize_message # noqa: E402 +from std_msgs.msg import String # noqa: E402 + +import thorsipm # noqa: E402 + +results: list[tuple[str, bool, str]] = [] + + +def check(name: str, ok: bool, info: str = "") -> bool: + results.append((name, bool(ok), info)) + print(f" {'PASS' if ok else 'FAIL'} {name}{'' if ok else ' — ' + info}", file=sys.stderr) + return bool(ok) + + +def wait_for(pred, timeout: float, period: float = 0.05) -> bool: + end = time.monotonic() + timeout + while time.monotonic() < end: + if pred(): + return True + time.sleep(period) + return bool(pred()) + + +class Proc: + def __init__(self, name: str, argv: list[str], logdir: Path) -> None: + self.name, self.argv = name, argv + self.log = open(logdir / f"{name}.log", "a", encoding="utf-8") + self.p = popen_group(argv, stdout=self.log, stderr=subprocess.STDOUT, text=True) + + def alive(self) -> bool: + return self.p.poll() is None + + def stop(self, grace: float = 5.0) -> int | None: + if self.alive(): + try: + signal_group(self.p, signal.SIGINT) + self.p.wait(grace) + except subprocess.TimeoutExpired: + signal_group(self.p, SIGKILL) + self.p.wait(3) + except (ProcessLookupError, OSError, ValueError): + pass + self.log.flush() + return self.p.returncode + + +class Client(Node): + """Subscribes /sipm/status + /sipm/reply; sends commands and waits for the matching reply.""" + + def __init__(self) -> None: + super().__init__("sipm_harness") + self.lock = threading.Lock() + self.status: list[tuple[int, dict, String]] = [] + self.replies: dict = {} + self.create_subscription(String, "/sipm/status", self._st, RELIABLE_QOS) + self.create_subscription(String, "/sipm/reply", self._rep, RELIABLE_QOS) + self.pub = self.create_publisher(String, "/sipm/cmd", RELIABLE_QOS) + + def _st(self, m: String) -> None: + with self.lock: + self.status.append((time.time_ns(), json.loads(m.data), m)) + + def _rep(self, m: String) -> None: + o = json.loads(m.data) + with self.lock: + self.replies[o.get("id")] = o + + def last_status(self) -> dict | None: + with self.lock: + return self.status[-1][1] if self.status else None + + def cmd(self, command: str, timeout: float = 10.0, **params) -> dict | None: + cid = str(uuid.uuid4())[:8] + m = String() + m.data = json.dumps({"id": cid, "command": command, "params": params}) + self.pub.publish(m) + wait_for(lambda: cid in self.replies, timeout) + with self.lock: + return self.replies.get(cid) + + +def part_a() -> None: + print("A driver, no ROS", file=sys.stderr) + fake = thorsipm.FakeBackend() + reads: list[int] = [] + orig = fake.get_param + fake.get_param = lambda pid: (reads.append(pid), orig(pid))[1] + dev = thorsipm.ThorSiPM(fake) + with dev: + rows = dev.scan(0, 2000) + snap = dev.snapshot({"sipm1_bias": 1201, "sipm1_temp_c": 1211}) + ids = {r["id"] for r in rows} + unknown = sorted(r["id"] for r in rows if not r["name"]) + check("A: scan returns every available ID (fake: 38)", len(rows) == 38, f"{len(rows)}: {sorted(ids)}") + check("A: scan flags the 6 IDs missing from the GUI enum (the bias/temperature candidates)", unknown == sorted(fake.HIDDEN), str(unknown)) + check("A: firmware-update params are listed but never read", 743 in ids and not (set(reads) & thorsipm.NEVER_READ), str(sorted(set(reads) & thorsipm.NEVER_READ))) + d1, d2 = snap["detectors"].get("1", {}), snap["detectors"].get("2", {}) + check("A: snapshot has both detectors with enable/gain/offset/bandwidth and serials 000027/000026", + d1.get("enable") == 1.0 and d1.get("gain_volts") == 0.8 and d1.get("serial") == "000027" and d2.get("serial") == "000026" + and "bandwidth_pos" in d1 and "output_offset" in d1, json.dumps(snap)[:300]) + check("A: extra (params-file) values read", snap.get("extra", {}).get("sipm1_bias") == 54.2, json.dumps(snap.get("extra"))) + check("A: close() tears down (frees the COM port)", not dev.is_open and not fake.selected, "") + busy = thorsipm.ThorSiPM(thorsipm.FakeBackend(busy=True)) + try: + busy.open() + check("A: busy COM port (GUI open) → open() fails", False, "opened") + except thorsipm.SiPMError as exc: + check("A: busy COM port (GUI open) → open() fails and names the GUI", "ThorSiPM_Control.exe" in str(exc), str(exc)) + src = (NODES / "thorsipm.py").read_text(encoding="utf-8") + (NODES / "sipm_node.py").read_text(encoding="utf-8") + calls = re.findall(r"\.(SetParam\w*|StartPosition|SetupPosition|PreflightPosition|PostflightPosition)\s*\(", src) + sig_names = re.search(r"sig = \{(.*?)\n \}", src, re.S) + check("A: no write path — no SetParam/Setup/Start call, and the ctypes binding declares none", + not calls and sig_names is not None and "SetParam" not in sig_names.group(1) and "Position" not in sig_names.group(1) + and not hasattr(thorsipm.ThorSiPM, "set"), f"calls={calls}") + + +def main(argv: list[str] | None = None) -> int: + ap = argparse.ArgumentParser(description=__doc__, formatter_class=argparse.RawDescriptionHelpFormatter) + ap.add_argument("--keep", action="store_true", help="keep the work dir") + ap.add_argument("--no-router", action="store_true", help="a Zenoh router is already running on 127.0.0.1:7447") + a = ap.parse_args(argv) + work = Path(tempfile.mkdtemp(prefix="sipm-test-")) + logdir = work / "proc-logs" + logdir.mkdir() + params_file = work / "sipm_params.json" + params_file.write_text(json.dumps({"_note": "IDs from the Block S scan", "sipm1_bias": 1201, "sipm2_bias": 1202, + "sipm1_temp_c": 1211, "sipm2_temp_c": 1212, "sipm1_tempstatus": 1221}), encoding="utf-8") + print(f"[test] work dir {work}", file=sys.stderr) + + part_a() + + procs: list[Proc] = [] + cl = ex = None + try: + if not a.no_router: + procs.append(Proc("router", ["ros2", "run", "rmw_zenoh_cpp", "rmw_zenohd"], logdir)) + check("zenoh router listening on 127.0.0.1:7447", wait_port("127.0.0.1", 7447, 20.0)) + rclpy.init() + cl = Client() + ex = SingleThreadedExecutor() + ex.add_node(cl) + threading.Thread(target=ex.spin, name="harness-spin", daemon=True).start() + + # ── B: node, connected ─────────────────────────────────────────────────────────────── + print("B node, connected", file=sys.stderr) + node = Proc("sipm-node", [PY, str(NODES / "sipm_node.py"), "--fake", "--params-file", str(params_file), "--status-every", "0.5"], logdir) + procs.append(node) + got = wait_for(lambda: len(cl.status) >= 4, 25.0) + st = cl.last_status() or {} + check("B: /sipm/status arrives", got, f"{len(cl.status)} messages") + with cl.lock: + ts = [t for t, _, _ in cl.status[-4:]] + gaps = [(b - c) / 1e9 for c, b in zip(ts, ts[1:])] + check("B: status period ≈ --status-every (0.5 s)", gaps and all(0.3 < g < 0.9 for g in gaps), str(gaps)) + check("B: status connected, level ok, two detectors, serials", st.get("connected") is True and st.get("level") == "ok" + and st.get("detectors", {}).get("1", {}).get("serial") == "000027" and st.get("connected_detectors") == 2, json.dumps(st)[:300]) + check("B: params-file values in status (bias, temperature)", st.get("extra", {}).get("sipm1_bias") == 54.2 + and 21.0 < (st.get("extra", {}).get("sipm2_temp_c") or 0) < 23.0, json.dumps(st.get("extra"))) + r = cl.cmd("get", name="PARAM_PMT2_ENABLE") + check("B: get by PARAM_* name", r and r.get("ok") and r["value"]["id"] == 703 and r["value"]["value"] == 1.0, json.dumps(r)) + r = cl.cmd("get", name="sipm2_bias") + check("B: get by params-file name", r and r.get("ok") and r["value"]["value"] == 54.6, json.dumps(r)) + r = cl.cmd("info", id=1201) + check("B: info", r and r.get("ok") and r["value"]["info"]["available"] == 1, json.dumps(r)) + r = cl.cmd("scan", timeout=30.0, **{"from": 690, "to": 1300}) + check("B: scan finds the hidden IDs", r and r.get("ok") and r["value"]["not_in_enum"] == sorted(thorsipm.FakeBackend.HIDDEN), json.dumps(r)[:300]) + r = cl.cmd("get", name="no_such_param") + check("B: an unknown name is an error reply, not a crash", r and r.get("ok") is False and "unknown parameter" in r.get("error", ""), json.dumps(r)) + r = cl.cmd("set", id=701, value=0) + check("B: there is no set command", r and r.get("ok") is False and "unknown command" in r.get("error", ""), json.dumps(r)) + r = cl.cmd("disconnect") + n0 = len(cl.status) + wait_for(lambda: len(cl.status) >= n0 + 3, 5.0) + st = cl.last_status() or {} + check("B: disconnect → status not connected, held (no auto-reconnect)", r and r.get("ok") and st.get("connected") is False and st.get("held") is True, json.dumps(st)) + r = cl.cmd("reconnect") + n0 = len(cl.status) + wait_for(lambda: len(cl.status) >= n0 + 2, 5.0) + check("B: reconnect → connected again", r and r.get("ok") and (cl.last_status() or {}).get("connected") is True, json.dumps(r)) + rc = node.stop() + check("B: clean exit on SIGINT", rc == 0, f"rc={rc}; log: {(logdir / 'sipm-node.log').read_text()[-400:]}") + + # ── C: node, port busy ─────────────────────────────────────────────────────────────── + print("C node, port busy", file=sys.stderr) + with cl.lock: + cl.status.clear() + busy = Proc("sipm-node-busy", [PY, str(NODES / "sipm_node.py"), "--fake-busy", "--status-every", "0.5", "--reconnect-every", "1"], logdir) + procs.append(busy) + wait_for(lambda: len(cl.status) >= 3, 25.0) + st = cl.last_status() or {} + check("C: busy port → status keeps coming, not connected, reason names the GUI", len(cl.status) >= 3 and st.get("connected") is False + and st.get("level") == "error" and "ThorSiPM_Control.exe" in (st.get("error") or ""), json.dumps(st)) + r = cl.cmd("get", id=701) + check("C: a read while not connected is an error reply", r and r.get("ok") is False and "not connected" in r.get("error", ""), json.dumps(r)) + busy.stop() + + # ── D: recording ───────────────────────────────────────────────────────────────────── + print("D recording", file=sys.stderr) + import bridge_node # noqa: E402 + import mcap_convert # noqa: E402 + from foxglove_json import read_messages # noqa: E402 + from recorder import Recorder # noqa: E402 + + lw = bridge_node.RecordingLogWriter(None, "behavior_v2", None, profile="crashsafe", typed=False, export="converted", connectors=[]) + tmap = lw.topics() + check("D: bridge_node records /sipm/cmd|reply|status in every bag", all(tmap.get(t) == "std_msgs/msg/String" for t in ("/sipm/cmd", "/sipm/reply", "/sipm/status")), str(sorted(tmap))) + check("D: bridge_node subscribes the SiPM node as a connector", bridge_node.CONNECTOR_TOPICS.get("sipm", {}).get("status") == "/sipm/status", "") + check("D: the converter derives /sipm/reply|status into connector events", + mcap_convert.CONNECTOR_TOPIC_SRC.get("/sipm/status") == ("sipm", "status") and mcap_convert.CONNECTOR_TOPIC_SRC.get("/sipm/reply") == ("sipm", "reply"), "") + bag = work / "sipm.bag" + rec = Recorder(bag, {"/sipm/status": "std_msgs/msg/String"}, profile="default") + rec.open() + with cl.lock: + sent = [(t, m) for t, _, m in cl.status] + for t_ns, m in sent: + rec.write_raw("/sipm/status", serialize_message(m), t_ns) + rec.close(reindex=False) + mcaps = sorted(bag.glob("*.mcap")) if bag.is_dir() else [bag] + msgs, torn = read_messages(mcaps[0]) if mcaps else ([], "no mcap") + back = [json.loads(deserialize_message(d, String).data) for topic, _, _, _, _, d in msgs if topic == "/sipm/status"] + check("D: status messages round-trip through the recorder into an MCAP", len(back) == len(sent) and back and back[-1].get("level") == "error" and not torn, + f"{len(back)}/{len(sent)} torn={torn}") + finally: + for pr in reversed(procs): + pr.stop() + pr.log.close() + try: + if ex is not None: + ex.shutdown(timeout_sec=1.0) + if cl is not None: + cl.destroy_node() + rclpy.try_shutdown() + except Exception: + pass + + n_ok = sum(1 for _, ok, _ in results if ok) + print(f"\n{n_ok} / {len(results)} checks passed", file=sys.stderr) + print("\n".join(["| check | result |", "|---|---|"] + [f"| {n} | {'PASS' if ok else 'FAIL — ' + i.replace('|', '/')[:160]} |" for n, ok, i in results])) + if not a.keep and n_ok == len(results): + shutil.rmtree(work, ignore_errors=True) + else: + print(f"[test] artifacts kept in {work}", file=sys.stderr) + return 0 if n_ok == len(results) else 1 + + +if __name__ == "__main__": + raise SystemExit(main())