#!/usr/bin/env python3 """Read-only PoseStamped inspector; no robot, adapter or RL connection.""" from __future__ import annotations import argparse from collections import deque import csv from pathlib import Path import time import numpy as np def racket_center(position, quaternion_xyzw, offset_m): """Rotate a measured rigid-body-local offset into the pose's parent frame.""" position = np.asarray(position, dtype=float) quaternion = np.asarray(quaternion_xyzw, dtype=float) offset = np.asarray(offset_m, dtype=float) if (position.shape != (3,) or quaternion.shape != (4,) or offset.shape != (3,) or not np.isfinite(np.concatenate((position, quaternion, offset))).all()): raise ValueError("expected finite position[3], quaternion xyzw[4], offset[3]") norm = np.linalg.norm(quaternion) if norm < 1e-8: raise ValueError("zero quaternion does not define a rotation") quaternion = quaternion / norm xyz, w = quaternion[:3], quaternion[3] rotated = offset + 2 * np.cross(xyz, np.cross(xyz, offset) + w * offset) return position + rotated class PoseWindow: """Statistics from unique source frames, never from repeated receipt updates.""" def __init__(self, offset=None): self.offset = offset self.samples = deque(maxlen=500) self.latest = None self.last_stamp_ns = None self.last_receipt_s = None self.clock_resets = 0 self.frame_changes = 0 def update(self, stamp_ns, frame_id, position, quaternion, receipt_s): # Validate orientation even when only the rigid-body origin is displayed. center = racket_center(position, quaternion, self.offset if self.offset is not None else (0, 0, 0)) if stamp_ns <= 0: raise ValueError("source stamp must be positive") if stamp_ns == self.last_stamp_ns: return None if self.last_stamp_ns is not None and stamp_ns < self.last_stamp_ns: self.samples.clear() self.clock_resets += 1 if self.latest is not None and frame_id != self.latest["frame_id"]: self.samples.clear() self.frame_changes += 1 gap = (stamp_ns - self.last_stamp_ns) * 1e-9 if self.samples else None self.last_stamp_ns, self.last_receipt_s = stamp_ns, receipt_s self.samples.append((stamp_ns, np.asarray(position), center)) row = dict(source_s=stamp_ns * 1e-9, receipt_monotonic_s=receipt_s, frame_id=frame_id, x_m=position[0], y_m=position[1], z_m=position[2], qx=quaternion[0], qy=quaternion[1], qz=quaternion[2], qw=quaternion[3], gap_s=gap, center_x_m=center[0] if self.offset is not None else None, center_y_m=center[1] if self.offset is not None else None, center_z_m=center[2] if self.offset is not None else None) self.latest = row return row def summary(self, now_s): count = len(self.samples) span = (self.samples[-1][0] - self.samples[0][0]) * 1e-9 if count > 1 else 0 position_std = np.std([p for _, p, _ in self.samples], axis=0).tolist() if count else None center_std = (np.std([c for _, _, c in self.samples], axis=0).tolist() if count and self.offset is not None else None) return dict(samples=count, source_hz=(count - 1) / span if span > 0 else None, stale=self.last_receipt_s is None or now_s - self.last_receipt_s > 0.5, origin_std_m=position_std, center_std_m=center_std, clock_resets=self.clock_resets, frame_changes=self.frame_changes) def main(): parser = argparse.ArgumentParser(description=__doc__) parser.add_argument("--list", action="store_true", help="List discovered ROS topics and types, then exit.") parser.add_argument("--topic", default="/test_pat1/pose") parser.add_argument("--duration-s", type=float, default=10.0) parser.add_argument("--output", type=Path, help="Optional pose CSV; existing files are not overwritten.") parser.add_argument("--center-offset-m", nargs=3, type=float, metavar=("X", "Y", "Z"), help="Calibrated offset in rigid-body local axes, in meters.") args = parser.parse_args() if not np.isfinite(args.duration_s) or args.duration_s <= 0: parser.error("--duration-s must be positive and finite") if args.center_offset_m is not None and not np.isfinite(args.center_offset_m).all(): parser.error("--center-offset-m must be finite") # Keep the geometry/statistics helpers importable in tests without ROS. import rclpy from geometry_msgs.msg import PoseStamped from rclpy.qos import qos_profile_sensor_data rclpy.init() node = rclpy.create_node("inspect_racket_mocap") output = None window = PoseWindow(args.center_offset_m) try: if args.list: deadline = time.monotonic() + args.duration_s while time.monotonic() < deadline: rclpy.spin_once(node, timeout_sec=0.1) for name, types in sorted(node.get_topic_names_and_types()): print(f"{name}: {', '.join(types)}") return 0 print(f"[RACKET] topic={args.topic} unit=m quaternion=xyzw; candidate name=test_pat1 streaming_id=7") print("PoseStamped contains no streaming ID or tracking-valid flag; verify the mapping in Motive/driver.") print(f"center_offset_m={args.center_offset_m}; absent offset means rigid-body origin only.") print("std uses the last 500 unique frames; hold still to measure jitter. No court/robot transform is applied.") writer = None if args.output: output = args.output.open("x", newline="", encoding="utf-8") def receive(msg): nonlocal writer p, q = msg.pose.position, msg.pose.orientation stamp_ns = msg.header.stamp.sec * 1_000_000_000 + msg.header.stamp.nanosec try: row = window.update(stamp_ns, msg.header.frame_id, (p.x, p.y, p.z), (q.x, q.y, q.z, q.w), time.monotonic()) except ValueError as error: node.get_logger().warning(str(error), throttle_duration_sec=1.0) return if row is not None and output is not None: if writer is None: writer = csv.DictWriter(output, fieldnames=list(row)) writer.writeheader() writer.writerow(row) node.create_subscription(PoseStamped, args.topic, receive, qos_profile_sensor_data) started = last_print = time.monotonic() while time.monotonic() - started < args.duration_s: rclpy.spin_once(node, timeout_sec=0.1) now = time.monotonic() if now - last_print >= 1: stats = window.summary(now) print(f"[{'NO NEW SOURCE FRAME' if stats['stale'] else 'LIVE'}] {window.latest} stats={stats}") if output is not None: output.flush() last_print = now print(f"[RACKET END] {window.summary(time.monotonic())}") return 0 if window.latest is not None else 1 except KeyboardInterrupt: return 0 finally: if output is not None: output.close() node.destroy_node() rclpy.try_shutdown() if __name__ == "__main__": raise SystemExit(main())