| |
| """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 |
| from runtime.h2_shm_bridge import ( |
| H2BridgeState, |
| H2SharedMemoryBridge, |
| SHM_NAME, |
| ) |
| from runtime.motion_attestation import interface_hash |
|
|
|
|
| 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()) |
|
|
