| |
| """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()) |
|
|
