#!/usr/bin/env python3 """Read-only H2BR LowState inspector; this script never enables LowCmd.""" from __future__ import annotations import argparse from dataclasses import dataclass import math from pathlib import Path import sys import time import numpy as np ROOT = Path(__file__).resolve().parents[1] if str(ROOT) not in sys.path: sys.path.insert(0, str(ROOT)) from runtime.h2_contract import HARDWARE_JOINT_NAMES, H2_DOF, MODE_PR # noqa: E402 from runtime.h2_shm_bridge import ( # noqa: E402 H2BridgeState, H2SharedMemoryBridge, SHM_NAME, ) from runtime.motion_attestation import interface_hash # noqa: E402 DEFAULT_MAX_STATE_AGE_S = 0.10 DEFAULT_MAX_PLANNER_AGE_S = 0.12 @dataclass(frozen=True) class SnapshotValidation: failures: tuple[str, ...] state_age_s: float planner_age_s: float @property def passed(self) -> bool: return not self.failures def _stamp_age_s(stamp_ns: int, now_ns: int) -> float: if isinstance(stamp_ns, bool) or not isinstance(stamp_ns, (int, np.integer)): return math.inf if int(stamp_ns) <= 0: return math.inf return (int(now_ns) - int(stamp_ns)) * 1.0e-9 def _validate_age( failures: list[str], *, label: str, stamp_ns: int, age_s: float, max_age_s: float, ) -> None: if isinstance(stamp_ns, bool) or not isinstance(stamp_ns, (int, np.integer)): failures.append(f"{label}_stamp_ns is not an integer") elif int(stamp_ns) <= 0: failures.append(f"{label}_stamp_ns must be positive") elif age_s < 0.0: failures.append(f"{label}_stamp_ns is in the future (age={age_s:.6f}s)") elif not math.isfinite(age_s) or age_s > max_age_s: failures.append(f"{label} is stale (age={age_s:.6f}s > {max_age_s:.6f}s)") def validate_snapshot( state: H2BridgeState, *, require_healthy: bool = False, require_planner: bool = False, require_disarmed: bool = False, expected_interface: str | None = None, max_state_age_s: float = DEFAULT_MAX_STATE_AGE_S, max_planner_age_s: float = DEFAULT_MAX_PLANNER_AGE_S, now_ns: int | None = None, ) -> SnapshotValidation: """Pure validation of one already-read H2BR snapshot. ``require_planner`` intentionally implies all healthy-state checks, while ``require_disarmed`` is independent and only checks the two request bits. Passing ``now_ns`` makes timestamp behavior deterministic in unit tests. """ if not math.isfinite(max_state_age_s) or max_state_age_s <= 0.0: raise ValueError("max_state_age_s must be positive and finite") if not math.isfinite(max_planner_age_s) or max_planner_age_s <= 0.0: raise ValueError("max_planner_age_s must be positive and finite") if now_ns is None: now_ns = time.monotonic_ns() if ( isinstance(now_ns, bool) or not isinstance(now_ns, (int, np.integer)) or int(now_ns) <= 0 ): raise ValueError("now_ns must be a positive integer") state_age_s = _stamp_age_s(state.state_stamp_ns, int(now_ns)) planner_age_s = _stamp_age_s(state.tt_stamp_ns, int(now_ns)) failures: list[str] = [] healthy_required = bool(require_healthy or require_planner) if healthy_required: if not state.robot_connected: failures.append("robot_connected=0 (expected 1)") if not state.state_ready: failures.append("state_ready=0 (expected 1)") if int(state.state_mode_pr) != MODE_PR: failures.append(f"mode_pr={int(state.state_mode_pr)} (expected {MODE_PR})") if state.emergency_stop: failures.append("fault/emergency_stop=1 (expected 0)") for label, values in (("joint_pos", state.joint_pos), ("joint_vel", state.joint_vel)): try: array = np.asarray(values, dtype=np.float64) except (TypeError, ValueError) as error: failures.append(f"{label} cannot be converted to numeric values: {error}") continue if array.shape != (H2_DOF,): failures.append(f"{label} shape={array.shape} (expected ({H2_DOF},))") elif not np.isfinite(array).all(): bad = np.flatnonzero(~np.isfinite(array)).tolist() failures.append(f"{label} contains non-finite values at indices {bad}") _validate_age( failures, label="state", stamp_ns=state.state_stamp_ns, age_s=state_age_s, max_age_s=max_state_age_s, ) if expected_interface is not None: if ( not expected_interface or "/" in expected_interface or expected_interface in {".", ".."} ): failures.append(f"expected interface name is invalid: {expected_interface!r}") else: expected_hash = interface_hash(expected_interface) actual_hash = int(state.bridge_interface_hash) if actual_hash != expected_hash: failures.append( "bridge interface hash mismatch: " f"0x{actual_hash:08x} != 0x{expected_hash:08x} ({expected_interface})" ) if require_planner: if not state.tt_ready: failures.append("tt_ready=0 (expected 1)") _validate_age( failures, label="planner", stamp_ns=state.tt_stamp_ns, age_s=planner_age_s, max_age_s=max_planner_age_s, ) if require_disarmed: if state.start_requested: failures.append("start_requested=1 (expected 0 while disarmed)") if state.stop_requested: failures.append("stop_requested=1 (expected 0 while disarmed)") return SnapshotValidation(tuple(failures), state_age_s, planner_age_s) def _format_age(age_s: float) -> str: return f"{age_s:.6f}" if math.isfinite(age_s) else "inf" def _print_snapshot(state: H2BridgeState, *, now_ns: int) -> None: state_age_s = _stamp_age_s(state.state_stamp_ns, now_ns) planner_age_s = _stamp_age_s(state.tt_stamp_ns, now_ns) print( f"state_ready={int(state.state_ready)} tt_ready={int(state.tt_ready)} " f"mode_pr={state.state_mode_pr} " f"mode_machine={state.mode_machine} start={int(state.start_requested)} " f"stop={int(state.stop_requested)} fault={int(state.emergency_stop)} " f"robot_connected={int(state.robot_connected)} " f"state_age_s={_format_age(state_age_s)} " f"tt_age_s={_format_age(planner_age_s)} " f"interface_hash=0x{int(state.bridge_interface_hash):08x}" ) for index, name in enumerate(HARDWARE_JOINT_NAMES): print( f" {index:02d} {name.removesuffix('_joint'):<24} " f"q={state.joint_pos[index]:+8.4f} dq={state.joint_vel[index]:+8.4f}" ) def main(argv: list[str] | None = None) -> int: parser = argparse.ArgumentParser(description=__doc__) parser.add_argument("--shm-name", default=SHM_NAME) parser.add_argument("--once", action="store_true") parser.add_argument("--period", type=float, default=0.5) parser.add_argument( "--require-healthy", action="store_true", help="Fail unless the robot LowState snapshot is connected, PR(0), finite, fresh, and fault-free.", ) parser.add_argument( "--require-planner", action="store_true", help="Require healthy LowState plus a ready and fresh planner snapshot.", ) parser.add_argument( "--require-disarmed", action="store_true", help="Independently require both start_requested and stop_requested to be clear.", ) parser.add_argument( "--expected-interface", default=None, help="Validate bridge_interface_hash against this exact Linux interface name.", ) parser.add_argument( "--max-state-age-s", type=float, default=DEFAULT_MAX_STATE_AGE_S, help=f"Maximum accepted LowState age (default: {DEFAULT_MAX_STATE_AGE_S:.2f}s).", ) parser.add_argument( "--max-planner-age-s", type=float, default=DEFAULT_MAX_PLANNER_AGE_S, help=f"Maximum accepted planner receipt age (default: {DEFAULT_MAX_PLANNER_AGE_S:.2f}s).", ) args = parser.parse_args(argv) if not math.isfinite(args.max_state_age_s) or args.max_state_age_s <= 0.0: parser.error("--max-state-age-s must be positive and finite") if not math.isfinite(args.max_planner_age_s) or args.max_planner_age_s <= 0.0: parser.error("--max-planner-age-s must be positive and finite") if not math.isfinite(args.period) or args.period <= 0.0: parser.error("--period must be positive and finite") validation_requested = bool( args.require_healthy or args.require_planner or args.require_disarmed or args.expected_interface is not None ) bridge = H2SharedMemoryBridge(args.shm_name) try: while True: state = bridge.read_state() now_ns = time.monotonic_ns() _print_snapshot(state, now_ns=now_ns) result = validate_snapshot( state, require_healthy=args.require_healthy, require_planner=args.require_planner, require_disarmed=args.require_disarmed, expected_interface=args.expected_interface, max_state_age_s=args.max_state_age_s, max_planner_age_s=args.max_planner_age_s, now_ns=now_ns, ) if validation_requested: if result.passed: scopes: list[str] = [] if args.require_planner: scopes.append("planner+healthy") elif args.require_healthy: scopes.append("healthy") if args.require_disarmed: scopes.append("disarmed") if not scopes: scopes.append("interface") scope = "+".join(scopes) details = [f"[PASS] H2BR {scope} snapshot"] if args.require_healthy or args.require_planner: details.append(f"state_age_s={_format_age(result.state_age_s)}") if args.require_planner: details.append(f"planner_age_s={_format_age(result.planner_age_s)}") if args.expected_interface is not None: details.append( f"interface={args.expected_interface} " f"hash=0x{int(state.bridge_interface_hash):08x}" ) print(" ".join(details)) else: print("[FAIL] H2BR snapshot validation") for failure in result.failures: print(f"[FAIL] {failure}") return 1 if args.once: return 0 time.sleep(max(0.05, args.period)) finally: bridge.close() if __name__ == "__main__": raise SystemExit(main())