diff --git a/ros2-poc/README.md b/ros2-poc/README.md index adfbf5a6..2043067c 100644 --- a/ros2-poc/README.md +++ b/ros2-poc/README.md @@ -150,11 +150,23 @@ OK while data flows, STALE after `--stale-s`, WARN never seen, ERROR for BIAS un failed command, with rate / last sequence / latency / last error + time), and a `visualization_msgs/MarkerArray` on `/viz/markers`: the ball rotated by FicTrac's lab-frame orientation with markings so the rotation is visible, the fly fixed on top facing +X, the arena -as a translucent cylinder, the pattern feature as a bar on the wall at -`wrap180(frame_dir·idx·deg_per_frame + az0)` (the same mapping `fictrac_sim.py --model fly` -uses), and a text readout. The stock `foxglove_bridge` serves it on **ws://localhost:8766** +as a translucent cylinder, the pattern feature as a bar on the wall at `wrap180(display_deg)` +(below), and a text readout. The stock `foxglove_bridge` serves it on **ws://localhost:8766** (8765 is the Studio bridge). +**The display's rotation, not just its index.** The bridge reports the frame index mod the +pattern's frame count, so with a periodic pattern (a 20-frame grating) `azimuth_deg = +wrap180(frame_dir·idx·deg_per_frame + az0)` stays inside one 36° period while the fly turns +through thousands of degrees. `/viz/arena/display_deg` sums the index steps, each taken the short +way round the modulus. That is the display's unwrapped rotation, which the closed-loop plot sets +against the fly's turn. The modulus comes from `--frames N` when given; otherwise it is learned +within each epoch as the largest index seen + 1, which is enough because a wrap passes through +the largest index. For a pattern that spans the arena (n·deg_per_frame = 360), `wrap180(display_deg)` +equals the feature azimuth, so the 3D bar is unchanged there, and for a grating it now turns +with the pattern. `/viz/fictrac/fwd_dps` and `side_dps` are the rates of FicTrac's integrated +forward/side motion (cols 20/21) in °/s, the units of `speed_dps`, so the kinematics plot reads +on one axis. `pixi run test-viz` checks this math. + ```bash pixi run rig -- --sim --sim-args "--model fly --kp 2 --seed 1" --foxglove # Arena + FicTrac pixi run rig -- --sim --bias --bias-sim --foxglove # Arena + BIAS (sim) @@ -163,14 +175,15 @@ pixi run rig -- --sim --bias --foxglove # Layout → Import from file… → ros2-poc/foxglove/arena-fictrac.json or arena-bias.json ``` -`arena-fictrac.json`: 3D scene, closed-loop plot (fly turn vs feature azimuth vs bias angle), -kinematics plot, frame-index plot, FicTrac light + rate gauge, diagnostics summary/detail, last -error, log. `arena-bias.json`: 3D scene, BIAS state timeline (unreachable / idle / preview / -recording), frame count vs arena index, latest BIAS reply (shows the `artifact` path), reachable / -capturing / writing-video lights, fps gauge, diagnostics, last error, log. Both work live and on a -replayed MCAP once the log moves to MCAP. Verified here only as topics and advertised channels -(no GUI in the container); the layouts are hand-written Foxglove JSON, so a panel whose schema -drifted shows up empty and is one click to re-point. +`arena-fictrac.json`: 3D scene, closed-loop plot (fly turn vs display rotation), bias angle, +kinematics plot (speed, forward, side in °/s), frame-index plot, FicTrac light + rate gauge, +diagnostics summary/detail, last error, log. `arena-bias.json`: 3D scene, BIAS state timeline +(unreachable / idle / preview / recording), camera frame count and the displayed index on separate +plots, latest BIAS reply (shows the `artifact` path), BIAS / camera / video lights, fps gauge, +diagnostics, last error, log. The ROS log panel is live only: the bags do not record `/rosout`. +Both layouts were checked in Foxglove (web 3.4, 2026-10-16) on ww18 hardware runs: each bag was +played back at 1× through this node and its outputs recorded. The files and the checker +(`tools/check_layouts.py`) are on branch `data/ros2-poc-ww18-2026-10-15`. ## Log format: MCAP sizing and the two-channel plan (2026-10-15) diff --git a/ros2-poc/foxglove/arena-bias.json b/ros2-poc/foxglove/arena-bias.json index bd5d0f36..a0ad90f0 100644 --- a/ros2-poc/foxglove/arena-bias.json +++ b/ros2-poc/foxglove/arena-bias.json @@ -37,36 +37,40 @@ "imageMode": {} }, "Indicator!reachable": { + "foxglovePanelTitle": "BIAS", "path": "/viz/bias/reachable.data", "style": "bulb", "fallbackColor": "#8b949e", - "fallbackLabel": "BIAS: no status yet", + "fallbackLabel": "no status", "rules": [ - { "operator": "=", "rawValue": "true", "color": "#00e676", "label": "BIAS reachable" }, - { "operator": "=", "rawValue": "false", "color": "#ff5252", "label": "BIAS unreachable" } + { "operator": "=", "rawValue": "true", "color": "#00e676", "label": "online" }, + { "operator": "=", "rawValue": "false", "color": "#ff5252", "label": "offline" } ] }, "Indicator!capturing": { + "foxglovePanelTitle": "Camera", "path": "/viz/bias/capturing.data", "style": "bulb", "fallbackColor": "#8b949e", - "fallbackLabel": "capture: unknown", + "fallbackLabel": "unknown", "rules": [ { "operator": "=", "rawValue": "true", "color": "#00e676", "label": "capturing" }, - { "operator": "=", "rawValue": "false", "color": "#8b949e", "label": "not capturing" } + { "operator": "=", "rawValue": "false", "color": "#8b949e", "label": "idle" } ] }, "Indicator!logging": { + "foxglovePanelTitle": "Video", "path": "/viz/bias/logging.data", "style": "bulb", "fallbackColor": "#8b949e", - "fallbackLabel": "video: unknown", + "fallbackLabel": "unknown", "rules": [ - { "operator": "=", "rawValue": "true", "color": "#ffd740", "label": "writing video" }, - { "operator": "=", "rawValue": "false", "color": "#8b949e", "label": "no video" } + { "operator": "=", "rawValue": "true", "color": "#ffd740", "label": "writing" }, + { "operator": "=", "rawValue": "false", "color": "#8b949e", "label": "off" } ] }, "Gauge!fps": { + "foxglovePanelTitle": "Camera fps", "path": "/viz/bias/fps.data", "minValue": 0, "maxValue": 200, @@ -82,10 +86,24 @@ "isSynced": true }, "Plot!frames": { - "title": "BIAS frame count vs displayed index", + "title": "BIAS camera frames", "paths": [ - { "value": "/viz/bias/frame_count.data", "enabled": true, "timestampMethod": "receiveTime", "label": "camera frames" }, - { "value": "/viz/arena/index.data", "enabled": true, "timestampMethod": "receiveTime", "label": "arena frame index" } + { "value": "/viz/bias/frame_count.data", "enabled": true, "timestampMethod": "receiveTime", "label": "camera frames" } + ], + "showXAxisLabels": true, + "showYAxisLabels": true, + "showLegend": true, + "legendDisplay": "floating", + "showPlotValuesInLegend": true, + "isSynced": true, + "xAxisVal": "timestamp", + "sidebarDimension": 240, + "followingViewWidth": 60 + }, + "Plot!arenaindex": { + "title": "Displayed frame index", + "paths": [ + { "value": "/viz/arena/index.data", "enabled": true, "timestampMethod": "receiveTime", "label": "frame index" } ], "showXAxisLabels": true, "showYAxisLabels": true, @@ -125,6 +143,7 @@ "expansion": "all" }, "RosOut!log": { + "foxglovePanelTitle": "ROS log (live only: bags do not record /rosout)", "searchTerms": [], "minLogLevel": 2, "nameFilter": {} @@ -147,20 +166,25 @@ "second": { "direction": "row", "splitPercentage": 55, - "first": "Plot!frames", + "first": { + "direction": "column", + "splitPercentage": 55, + "first": "Plot!frames", + "second": "Plot!arenaindex" + }, "second": "RawMessages!reply" } } }, "second": { "direction": "column", - "splitPercentage": 28, + "splitPercentage": 34, "first": { "direction": "row", - "splitPercentage": 66, + "splitPercentage": 60, "first": { - "direction": "row", - "splitPercentage": 50, + "direction": "column", + "splitPercentage": 33, "first": "Indicator!reachable", "second": { "direction": "column", diff --git a/ros2-poc/foxglove/arena-fictrac.json b/ros2-poc/foxglove/arena-fictrac.json index 71c24f41..283d409d 100644 --- a/ros2-poc/foxglove/arena-fictrac.json +++ b/ros2-poc/foxglove/arena-fictrac.json @@ -37,11 +37,25 @@ "imageMode": {} }, "Plot!closedloop": { - "title": "Closed loop: fly vs display", + "title": "Closed loop: fly turn vs display rotation (deg, unwrapped)", "paths": [ - { "value": "/viz/fictrac/turn_deg.data", "enabled": true, "timestampMethod": "receiveTime", "label": "fly turn (deg, unwrapped)" }, - { "value": "/viz/arena/azimuth_deg.data", "enabled": true, "timestampMethod": "receiveTime", "label": "feature azimuth (deg, right +)" }, - { "value": "/viz/arena/bias_deg.data", "enabled": true, "timestampMethod": "receiveTime", "label": "bias angle (deg)" } + { "value": "/viz/fictrac/turn_deg.data", "enabled": true, "timestampMethod": "receiveTime", "label": "fly turn" }, + { "value": "/viz/arena/display_deg.data", "enabled": true, "timestampMethod": "receiveTime", "label": "display rotation" } + ], + "showXAxisLabels": true, + "showYAxisLabels": true, + "showLegend": true, + "legendDisplay": "floating", + "showPlotValuesInLegend": true, + "isSynced": true, + "xAxisVal": "timestamp", + "sidebarDimension": 240, + "followingViewWidth": 30 + }, + "Plot!bias": { + "title": "Bias angle (deg)", + "paths": [ + { "value": "/viz/arena/bias_deg.data", "enabled": true, "timestampMethod": "receiveTime", "label": "bias" } ], "showXAxisLabels": true, "showYAxisLabels": true, @@ -54,11 +68,11 @@ "followingViewWidth": 30 }, "Plot!kinematics": { - "title": "FicTrac kinematics", + "title": "FicTrac kinematics (deg/s of ball rotation)", "paths": [ - { "value": "/viz/fictrac/speed_dps.data", "enabled": true, "timestampMethod": "receiveTime", "label": "speed (deg/s)" }, - { "value": "/viz/fictrac/fwd_rad.data", "enabled": true, "timestampMethod": "receiveTime", "label": "forward (rad, integrated)" }, - { "value": "/viz/fictrac/side_rad.data", "enabled": true, "timestampMethod": "receiveTime", "label": "side (rad, integrated)" } + { "value": "/viz/fictrac/speed_dps.data", "enabled": true, "timestampMethod": "receiveTime", "label": "speed" }, + { "value": "/viz/fictrac/fwd_dps.data", "enabled": true, "timestampMethod": "receiveTime", "label": "forward" }, + { "value": "/viz/fictrac/side_dps.data", "enabled": true, "timestampMethod": "receiveTime", "label": "side" } ], "showXAxisLabels": true, "showYAxisLabels": true, @@ -94,6 +108,7 @@ "collapsedSections": [] }, "Gauge!ftrate": { + "foxglovePanelTitle": "FicTrac rate (Hz)", "path": "/viz/fictrac/rate_hz.data", "minValue": 0, "maxValue": 120, @@ -103,13 +118,14 @@ "reverse": false }, "Indicator!fictrac": { + "foxglovePanelTitle": "FicTrac", "path": "/viz/fictrac/rate_hz.data", "style": "bulb", "fallbackColor": "#8b949e", - "fallbackLabel": "FicTrac: no data", + "fallbackLabel": "no data", "rules": [ - { "operator": ">", "rawValue": "5", "color": "#00e676", "label": "FicTrac flowing" }, - { "operator": "<=", "rawValue": "5", "color": "#ff5252", "label": "FicTrac stalled" } + { "operator": ">", "rawValue": "5", "color": "#00e676", "label": "flowing" }, + { "operator": "<=", "rawValue": "5", "color": "#ff5252", "label": "stalled" } ] }, "RawMessages!lasterror": { @@ -121,6 +137,7 @@ "expansion": "all" }, "RosOut!log": { + "foxglovePanelTitle": "ROS log (live only: bags do not record /rosout)", "searchTerms": [], "minLogLevel": 2, "nameFilter": {} @@ -139,7 +156,12 @@ "second": { "direction": "column", "splitPercentage": 55, - "first": "Plot!closedloop", + "first": { + "direction": "row", + "splitPercentage": 68, + "first": "Plot!closedloop", + "second": "Plot!bias" + }, "second": { "direction": "row", "splitPercentage": 60, diff --git a/ros2-poc/nodes/viz_node.py b/ros2-poc/nodes/viz_node.py index 6088f47a..b1a10ea4 100644 --- a/ros2-poc/nodes/viz_node.py +++ b/ros2-poc/nodes/viz_node.py @@ -8,10 +8,15 @@ /viz/fictrac/heading_deg FicTrac col 17 heading, degrees 0–360 /viz/fictrac/turn_deg unwrapped turn since the node started (Σ wrap180(Δheading)) /viz/fictrac/speed_dps col 19 (rad/frame) × measured frame rate, degrees per second - /viz/fictrac/fwd_rad, /viz/fictrac/side_rad cols 20 / 21 + /viz/fictrac/fwd_rad, /viz/fictrac/side_rad cols 20 / 21 (integrated since FicTrac started) + /viz/fictrac/fwd_dps, /viz/fictrac/side_dps rates of cols 20 / 21, degrees per second — + the same units as speed_dps, so the three share one plot /viz/fictrac/rate_hz measured FicTrac frame rate (1 Hz update) /viz/arena/index displayed frame index (from the bridge's /arena/frame) /viz/arena/azimuth_deg feature azimuth in the fly's view = wrap180(frame_dir·idx·deg_per_frame + az0) + /viz/arena/display_deg the display's UNWRAPPED rotation: Σ wrapped index steps × frame_dir × + deg_per_frame (DisplayAngle). A periodic pattern (a 20-frame grating) + keeps azimuth_deg inside one period; this follows the fly's turn /viz/arena/bias_deg live bias angle when a waveform runs (else 0) /viz/kinefly/left_deg, right_deg, lmr_deg, wbf_hz /viz/bias/frame_count, /viz/bias/fps @@ -25,12 +30,14 @@ visualization_msgs/MarkerArray /viz/markers (--markers-hz, default 20): the ball (sphere + markings rotated by FicTrac's lab-frame orientation, cols 12–14), the fly (arrow, fixed, facing +X), the arena (translucent cylinder) and the pattern - feature (bar on the wall at the current azimuth), a text readout. + feature (bar on the wall at wrap180(display_deg), which is the azimuth for a + pattern spanning the arena and the pattern's rotation for a grating), a + text readout. Pure presentation: nothing here is logged or fed back. Foxglove connects through the stock foxglove_bridge (`pixi run foxglove`, ws://localhost:8766); layouts in foxglove/*.json. - pixi run viz -- --deg-per-frame 1.8 --frame-dir 1 --feature-az0 0 + pixi run viz -- --deg-per-frame 1.8 --frame-dir 1 --feature-az0 0 [--frames 20] """ from __future__ import annotations @@ -59,6 +66,57 @@ def wrap180(deg: float) -> float: return ((deg + 180.0) % 360.0) - 180.0 +class DisplayAngle: + """The display's unwrapped rotation, from the WRAPPED frame index the bridge reports. + + The bridge sends idx mod n_frames, so a periodic pattern (a 20-frame grating at 1.8°/frame) + only ever reports 0…19 and its azimuth spans one 36° period while the fly turns through + thousands of degrees. Summing the index steps, each taken the short way round the modulus, + recovers the rotation: Σ step × frame_dir × deg_per_frame. It starts at the first frame's + azimuth (frame_dir·idx·deg_per_frame + az0), so for a pattern that spans the arena + (n·deg_per_frame = 360) wrap180(deg) equals the feature azimuth. + + The modulus is `frames` when given; otherwise it is learned within each epoch (the bridge's + `epoch` counts heading tares, i.e. trial starts, where the pattern may change) as the + largest index seen + 1. That is enough because a wrap always passes through the largest + index (19→0 or 0→19), which is then already counted. A jump at an epoch start (the tare + moving the display to its start frame) is kept as the step it is. + """ + + def __init__(self, deg_per_frame: float, frame_dir: int = 1, az0: float = 0.0, frames: int = 0) -> None: + self.dpf, self.dir, self.az0 = deg_per_frame, frame_dir, az0 + self.fixed = frames if frames and frames > 1 else 0 + self.n = self.fixed + self.deg: float | None = None + self._prev: int | None = None + self._epoch = None + + def step(self, idx: int, epoch=None) -> float: + if not self.fixed: + if epoch != self._epoch: + self.n = 0 + self.n = max(self.n, idx + 1) + self._epoch = epoch + if self._prev is None: + self.deg = self.dir * idx * self.dpf + self.az0 + else: + d = idx - self._prev + if self.n > 1: + d = (d + self.n // 2) % self.n - self.n // 2 + self.deg += self.dir * d * self.dpf + self._prev = idx + return self.deg + + +def per_frame_rate_dps(prev: tuple | None, fc: int, value: float, frame_rate_hz: float): + """deg/s of an integrated FicTrac quantity (cols 20/21, rad) between consecutive samples: + Δvalue / Δframe-counter × frame rate, so a dropped frame does not double the rate. None on + the first sample or when the counter does not advance (a FicTrac restart).""" + if prev is None or fc <= prev[0] or frame_rate_hz <= 0: + return None + return math.degrees(value - prev[1]) / (fc - prev[0]) * frame_rate_hz + + def rotvec_to_quat(rx: float, ry: float, rz: float): """Axis-angle rotation vector (rad) → (x, y, z, w).""" a = math.sqrt(rx * rx + ry * ry + rz * rz) @@ -138,9 +196,12 @@ def __init__(self, a: argparse.Namespace) -> None: "speed": f64("/viz/fictrac/speed_dps"), "fwd": f64("/viz/fictrac/fwd_rad"), "side": f64("/viz/fictrac/side_rad"), + "fwd_dps": f64("/viz/fictrac/fwd_dps"), + "side_dps": f64("/viz/fictrac/side_dps"), "ft_rate": f64("/viz/fictrac/rate_hz"), "index": f64("/viz/arena/index"), "az": f64("/viz/arena/azimuth_deg"), + "display": f64("/viz/arena/display_deg"), "bias": f64("/viz/arena/bias_deg"), "left": f64("/viz/kinefly/left_deg"), "right": f64("/viz/kinefly/right_deg"), @@ -163,15 +224,20 @@ def __init__(self, a: argparse.Namespace) -> None: self.create_subscription(String, "/kinefly/raw", self._kinefly, RELIABLE_QOS) self.create_subscription(String, "/bias/status", self._bias_status, RELIABLE_QOS) self.create_subscription(String, "/bias/reply", self._bias_reply, RELIABLE_QOS) - self.create_subscription(String, "/arena/frame", self._arena, 10) + # RELIABLE_QOS (depth 200) like the others: with depth 10 this subscription dropped 15 of + # 9,969 frames when a ww18 bag was replayed at 1x (2026-10-16). + self.create_subscription(String, "/arena/frame", self._arena, RELIABLE_QOS) # State for the markers self.quat = (0.0, 0.0, 0.0, 1.0) self.heading_deg = 0.0 self.turn_deg = 0.0 self._prev_hd = None + self._prev_motion = None # (frame counter, col 20, col 21) of the previous FicTrac frame self.idx = 0 self.az_deg = 0.0 + self.display = DisplayAngle(a.deg_per_frame, a.frame_dir, a.feature_az0, a.frames) + self.display_deg = None self.bias_deg = 0.0 self.bias_active = False self.bias_state = "unreachable" @@ -233,9 +299,18 @@ def _fictrac(self, msg: String) -> None: self.quat = rotvec_to_quat(f[11], f[12], f[13]) self._pub("heading", hd) self._pub("turn", self.turn_deg) - self._pub("speed", math.degrees(f[18]) * (s.rate_hz or self.a.assume_rate)) + rate = s.rate_hz or self.a.assume_rate + self._pub("speed", math.degrees(f[18]) * rate) self._pub("fwd", f[19]) self._pub("side", f[20]) + fc = int(f[0]) + prev = self._prev_motion + fwd = per_frame_rate_dps(prev and (prev[0], prev[1]), fc, f[19], rate) + side = per_frame_rate_dps(prev and (prev[0], prev[2]), fc, f[20], rate) + if fwd is not None and side is not None: + self._pub("fwd_dps", fwd) + self._pub("side_dps", side) + self._prev_motion = (fc, f[19], f[20]) def _kinefly(self, msg: String) -> None: env = self._env(msg) @@ -307,6 +382,8 @@ def _arena(self, msg: String) -> None: self._pub("index", idx) self.az_deg = wrap180(self.a.frame_dir * idx * self.a.deg_per_frame + self.a.feature_az0) self._pub("az", self.az_deg) + self.display_deg = self.display.step(idx, fr.get("epoch")) + self._pub("display", self.display_deg) b = fr.get("bias") self.bias_active = isinstance(b, (int, float)) self.bias_deg = float(b) if self.bias_active else 0.0 @@ -384,8 +461,10 @@ def _markers(self) -> None: cyl.scale.z = ARENA_H cyl.color = color(0.2, 0.6, 1.0, 0.12) arr.markers.append(cyl) - # 4: the pattern feature on the wall at the current azimuth (right-positive → −Y) - az = math.radians(self.az_deg) + # 4: the pattern feature on the wall (right-positive → −Y), at the display's rotation: the + # feature azimuth for a pattern that spans the arena, and for a periodic pattern (a + # grating) a point of the pattern turning with it rather than its phase in one period + az = math.radians(wrap180(self.display_deg) if self.display_deg is not None else self.az_deg) feat = self._marker(4, Marker.CUBE) feat.pose.position.x = (ARENA_R - 0.08) * math.cos(az) feat.pose.position.y = -(ARENA_R - 0.08) * math.sin(az) @@ -401,8 +480,9 @@ def _markers(self) -> None: txt.pose.position.z = BALL_R + ARENA_H + 0.6 txt.scale.z = 0.3 txt.color = color(0.9, 0.93, 0.95, 1.0) - txt.text = (f"idx {self.idx} az {self.az_deg:+.1f}° heading {self.heading_deg:.1f}°" - + (f" bias {self.bias_deg:+.1f}°" if self.bias_active else "")) + txt.text = (f"idx {self.idx} display {self.display_deg or 0.0:+.0f}° turn {self.turn_deg:+.0f}°" + f" heading {self.heading_deg:.0f}°" + + (f" bias {self.bias_deg:+.0f}°" if self.bias_active else "")) arr.markers.append(txt) self.p_markers.publish(arr) @@ -412,6 +492,8 @@ def main(argv: list[str] | None = None) -> int: p.add_argument("--deg-per-frame", type=float, default=1.8, help="pattern pitch, display degrees per frame index (default: 1.8)") p.add_argument("--frame-dir", type=int, default=1, choices=(1, -1), help="+1: an index increase moves the feature RIGHT (default)") p.add_argument("--feature-az0", type=float, default=0.0, help="feature azimuth at index 0, degrees, right-positive (default: 0)") + p.add_argument("--frames", type=int, default=0, + help="pattern frame count (the index modulus) for display_deg; 0 = learn it per epoch from the indices (default)") p.add_argument("--assume-rate", type=float, default=50.0, help="FicTrac frame rate used for speed before the first measured second (default: 50)") p.add_argument("--markers-hz", type=float, default=20.0, help="3D marker update rate (default: 20)") p.add_argument("--stale-s", type=float, default=2.0, help="seconds of silence before a source reads STALE (default: 2)") diff --git a/ros2-poc/pixi.toml b/ros2-poc/pixi.toml index 9cdba36f..b5dd6c47 100644 --- a/ros2-poc/pixi.toml +++ b/ros2-poc/pixi.toml @@ -76,4 +76,5 @@ size-study = "python sizing/compare.py" rig = "python nodes/rig.py" # ── acceptance test (LAB-303 PoC criteria): whole graph + Studio emulator + model fly ── test = "python tests/test_ros2_poc.py" +test-viz = "python tests/test_viz_node.py" studio-emulator = "python tests/studio_emulator.py" diff --git a/ros2-poc/tests/test_viz_node.py b/ros2-poc/tests/test_viz_node.py new file mode 100644 index 00000000..da2b097e --- /dev/null +++ b/ros2-poc/tests/test_viz_node.py @@ -0,0 +1,92 @@ +#!/usr/bin/env python3 +"""test_viz_node.py — the pure math in nodes/viz_node.py (no ROS graph needed). + + pixi run python tests/test_viz_node.py + +DisplayAngle (the unwrapped display rotation from the bridge's wrapped index) and +per_frame_rate_dps (rates of FicTrac's integrated cols 20/21). Importing viz_node needs rclpy +and the std/visualization/diagnostic message packages, which every environment here has. +""" +from __future__ import annotations + +import math +import sys +from pathlib import Path + +sys.path.insert(0, str(Path(__file__).resolve().parents[1] / "nodes")) +from viz_node import DisplayAngle, per_frame_rate_dps, wrap180 # noqa: E402 + +failures = 0 + + +def check(name: str, ok: bool, detail: str = "") -> None: + global failures + failures += not ok + print(f"{'PASS' if ok else 'FAIL'} {name}" + (f" ({detail})" if detail and not ok else "")) + + +def close(a: float, b: float, tol: float = 1e-9) -> bool: + return abs(a - b) <= tol + + +# 1. A 20-frame grating turned forward through 3 periods: the wrapped index cycles 0..19 three +# times, the unwrapped angle grows by 1.8° a step (60 steps = 108°), never jumping at 19→0. +d = DisplayAngle(1.8, frames=20) +idx = [i % 20 for i in range(61)] +out = [d.step(i, epoch=1) for i in idx] +steps = [b - a for a, b in zip(out, out[1:])] +check("grating forward: every step +1.8° across the wraps", all(close(s, 1.8) for s in steps), + f"steps {sorted(set(round(s, 3) for s in steps))}") +check("grating forward: 60 steps = 108°", close(out[-1] - out[0], 108.0)) + +# 2. Backwards through the wrap 0 → 19: −1.8°, not +34.2°. +d = DisplayAngle(1.8, frames=20) +seq = [3, 2, 1, 0, 19, 18] +out = [d.step(i) for i in seq] +check("grating backward through 0→19: −1.8° steps", all(close(b - a, -1.8) for a, b in zip(out, out[1:]))) + +# 3. Learned modulus (frames=0): the same backward sequence, where 19 is first seen AT the wrap. +d = DisplayAngle(1.8) +out = [d.step(i, epoch=1) for i in seq] +check("learned modulus: wrap at the first sight of the top index", all(close(b - a, -1.8) for a, b in zip(out, out[1:])), + f"{[round(b - a, 2) for a, b in zip(out, out[1:])]}") + +# 4. Learned modulus re-learns at an epoch change (a pre-run test pattern with 129 frames, then +# the 20-frame grating): after the tare, 19→0 is again a +1 step, not −19. +d = DisplayAngle(1.8) +for i in range(129): + d.step(i, epoch=0) +before = d.step(0, epoch=1) # the tare: jump to the start frame +for i in range(1, 20): + d.step(i, epoch=1) +last19 = d.deg +after = d.step(0, epoch=1) +check("learned modulus resets per epoch (129-frame test pattern, then 20)", close(after - last19, 1.8), + f"19→0 step {after - last19:+.2f}°") + +# 5. A pattern spanning the arena (200 frames × 1.8° = 360°): wrap180(display) is the azimuth. +d = DisplayAngle(1.8, frame_dir=1, az0=10.0, frames=200) +ok = True +for i in list(range(0, 200, 7)) + list(range(199, -1, -9)): + deg = d.step(i) + ok &= close(wrap180(deg), wrap180(1.8 * i + 10.0), 1e-6) +check("full-arena pattern: wrap180(display) == feature azimuth", ok) + +# 6. frame_dir −1 mirrors the angle. +a, b = DisplayAngle(1.8, frame_dir=1, frames=20), DisplayAngle(1.8, frame_dir=-1, frames=20) +for i in [0, 1, 2, 3, 2, 1, 0, 19, 18]: + x, y = a.step(i), b.step(i) +check("frame_dir −1 mirrors the rotation", close(x, -y)) + +# 7. Rates of the integrated FicTrac columns. +r = per_frame_rate_dps(None, 10, 1.0, 100.0) +check("rate: none on the first sample", r is None) +r = per_frame_rate_dps((10, 1.0), 11, 1.0 + math.radians(0.5), 100.0) +check("rate: 0.5° in one frame at 100 Hz = 50 °/s", r is not None and close(r, 50.0, 1e-9)) +r = per_frame_rate_dps((10, 1.0), 12, 1.0 + math.radians(1.0), 100.0) +check("rate: a dropped frame does not double it (1° over 2 frames = 50 °/s)", r is not None and close(r, 50.0, 1e-9)) +r = per_frame_rate_dps((500, 1.0), 3, 0.0, 100.0) +check("rate: none when the frame counter goes back (FicTrac restarted)", r is None) + +print(f"\n{'FAIL' if failures else 'PASS'}: {failures} failure(s)") +sys.exit(1 if failures else 0)