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