File size: 7,213 Bytes
d8290eb | 1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63 64 65 66 67 68 69 70 71 72 73 74 75 76 77 78 79 80 81 82 83 84 85 86 87 88 89 90 91 92 93 94 95 96 97 98 99 100 101 102 103 104 105 106 107 108 109 110 111 112 113 114 115 116 117 118 119 120 121 122 123 124 125 126 127 128 129 130 131 132 133 134 135 136 137 138 139 140 141 142 143 144 145 146 147 148 149 150 151 152 153 154 155 156 157 158 159 160 161 162 163 164 165 166 167 168 169 170 171 172 173 174 175 176 177 178 179 180 | #!/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())
|