File size: 7,535 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 | #!/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())
|