#!/usr/bin/env python3 """Read-only native DDS inspector for the H2 ``rt/lowstate`` contract. This tool deliberately uses Unitree's native DDS binding instead of a ROS 2 topic name. It never creates a publisher and never opens H2BR shared memory. """ from __future__ import annotations import argparse from dataclasses import dataclass, field import math import threading import time from typing import Any H2_DOF = 31 TICK_MODULUS = 1 << 32 @dataclass class LowStateStats: callbacks: int = 0 unique_ticks: int = 0 duplicate_ticks: int = 0 out_of_order_ticks: int = 0 first_callback_s: float | None = None last_callback_s: float | None = None max_callback_gap_s: float = 0.0 last_tick: int | None = None mode_pr_values: set[int] = field(default_factory=set) mode_machine_values: set[int] = field(default_factory=set) validation_errors: list[str] = field(default_factory=list) def observe(self, msg: Any, now_s: float) -> None: self.callbacks += 1 if self.first_callback_s is None: self.first_callback_s = now_s if self.last_callback_s is not None: self.max_callback_gap_s = max(self.max_callback_gap_s, now_s - self.last_callback_s) self.last_callback_s = now_s tick = int(msg.tick) & (TICK_MODULUS - 1) if self.last_tick is None: self.unique_ticks = 1 elif tick == self.last_tick: self.duplicate_ticks += 1 else: delta = (tick - self.last_tick) % TICK_MODULUS if delta >= TICK_MODULUS // 2: self.out_of_order_ticks += 1 else: self.unique_ticks += 1 self.last_tick = tick self.mode_pr_values.add(int(msg.mode_pr)) self.mode_machine_values.add(int(msg.mode_machine)) try: motors = msg.motor_state if len(motors) < H2_DOF: raise ValueError(f"motor_state has {len(motors)} slots; expected at least {H2_DOF}") values = [] for motor in motors[:H2_DOF]: values.extend((motor.q, motor.dq, motor.tau_est)) imu = msg.imu_state values.extend(imu.quaternion) values.extend(imu.gyroscope) values.extend(imu.accelerometer) if not all(math.isfinite(float(value)) for value in values): raise ValueError("LowState contains NaN or Inf") quat_norm = math.sqrt(sum(float(value) ** 2 for value in imu.quaternion)) if not 0.5 <= quat_norm <= 1.5: raise ValueError(f"IMU quaternion norm {quat_norm:.6f} is outside [0.5,1.5]") except (AttributeError, TypeError, ValueError) as exc: if len(self.validation_errors) < 20: self.validation_errors.append(str(exc)) def evaluate(stats: LowStateStats, elapsed_s: float, args: argparse.Namespace) -> list[str]: failures: list[str] = [] unique_hz = stats.unique_ticks / elapsed_s if elapsed_s > 0.0 else 0.0 if stats.callbacks == 0: failures.append("no native rt/lowstate samples received") if not args.min_unique_hz <= unique_hz <= args.max_unique_hz: failures.append( f"unique tick rate {unique_hz:.1f} Hz is outside " f"[{args.min_unique_hz:.1f},{args.max_unique_hz:.1f}] Hz" ) if stats.out_of_order_ticks: failures.append(f"observed {stats.out_of_order_ticks} out-of-order ticks") duplicate_fraction = stats.duplicate_ticks / max(1, stats.callbacks) if duplicate_fraction > args.max_duplicate_fraction: failures.append( f"duplicate callback fraction {duplicate_fraction:.3f} exceeds " f"{args.max_duplicate_fraction:.3f}" ) if stats.max_callback_gap_s > args.max_gap_s: failures.append( f"max callback gap {stats.max_callback_gap_s:.6f}s exceeds {args.max_gap_s:.6f}s" ) if stats.mode_pr_values != {args.expected_mode_pr}: failures.append( f"mode_pr values {sorted(stats.mode_pr_values)}; expected only {args.expected_mode_pr}" ) if stats.validation_errors: failures.append("; ".join(stats.validation_errors[:3])) return failures def parse_args() -> argparse.Namespace: parser = argparse.ArgumentParser(description=__doc__) parser.add_argument("--network-interface", required=True) parser.add_argument("--duration-s", type=float, default=5.0) parser.add_argument("--min-unique-hz", type=float, default=900.0) parser.add_argument("--max-unique-hz", type=float, default=1100.0) parser.add_argument("--max-gap-s", type=float, default=0.05) parser.add_argument("--max-duplicate-fraction", type=float, default=0.20) parser.add_argument("--expected-mode-pr", type=int, default=0) args = parser.parse_args() if not math.isfinite(args.duration_s) or args.duration_s < 2.0: parser.error("--duration-s must be finite and at least 2 seconds") if not 0.0 < args.min_unique_hz <= args.max_unique_hz: parser.error("invalid unique-rate bounds") if not 0.0 < args.max_gap_s <= 0.10: parser.error("--max-gap-s must be in (0,0.10]") if not 0.0 <= args.max_duplicate_fraction <= 1.0: parser.error("--max-duplicate-fraction must be in [0,1]") return args def main() -> int: args = parse_args() try: from unitree_sdk2py.core.channel import ChannelFactoryInitialize, ChannelSubscriber from unitree_sdk2py.idl.unitree_hg.msg.dds_ import LowState_ except ImportError as exc: print(f"[FAIL] Unitree native Python SDK is unavailable: {exc}") return 1 stats = LowStateStats() lock = threading.Lock() def callback(msg: Any) -> None: with lock: stats.observe(msg, time.monotonic()) print( "[INFO] read-only native DDS inspector: " f"interface={args.network_interface} topic=rt/lowstate duration={args.duration_s:.1f}s" ) ChannelFactoryInitialize(0, args.network_interface) subscriber = ChannelSubscriber("rt/lowstate", LowState_) subscriber.Init(callback, 10) start_s = time.monotonic() time.sleep(args.duration_s) elapsed_s = time.monotonic() - start_s with lock: failures = evaluate(stats, elapsed_s, args) unique_hz = stats.unique_ticks / elapsed_s duplicate_fraction = stats.duplicate_ticks / max(1, stats.callbacks) print( f"callbacks={stats.callbacks} callback_rate={stats.callbacks / elapsed_s:.1f}Hz " f"unique_ticks={stats.unique_ticks} unique_rate={unique_hz:.1f}Hz " f"duplicates={stats.duplicate_ticks} duplicate_fraction={duplicate_fraction:.3f} " f"max_gap_s={stats.max_callback_gap_s:.6f} " f"mode_pr={sorted(stats.mode_pr_values)} " f"mode_machine={sorted(stats.mode_machine_values)}" ) if failures: for failure in failures: print(f"[FAIL] {failure}") return 1 print("[PASS] native H2 rt/lowstate is finite, PR mode, ordered, and approximately 1000 Hz") return 0 if __name__ == "__main__": raise SystemExit(main())