"""Measured-ball adjudication. No outgoing simulation or virtual contact model.""" from __future__ import annotations from collections import deque from dataclasses import dataclass, replace import math import queue import threading import time import numpy as np from ..ball import BALL_RADIUS, TABLE_HEIGHT, TABLE_X_RANGE, TABLE_HALF_WIDTH, NET_X, NET_HEIGHT from .config import RLConfig from .position_reward import RacketHistory ADJUDICATION_VERSION = "adapter-velocity-first-landing-v2" @dataclass(frozen=True) class BallSample: source_s: float receipt_s: float monotonic_s: float position: np.ndarray velocity: np.ndarray velocity_valid: bool def __post_init__(self): for name in ("position", "velocity"): value = np.asarray(getattr(self, name), dtype=np.float64) if value.shape != (3,) or not np.isfinite(value).all(): raise ValueError(f"invalid ball {name}") object.__setattr__(self, name, value.copy()) if not isinstance(self.velocity_valid, (bool, np.bool_)): raise ValueError("invalid ball velocity_valid: expected bool") if not np.isfinite([self.source_s, self.receipt_s, self.monotonic_s]).all(): raise ValueError("invalid ball timestamps") def row(self): return [self.source_s, self.receipt_s, self.monotonic_s, *self.position.tolist()] def velocity_row(self): return [*self.velocity.tolist(), int(self.velocity_valid)] @dataclass(frozen=True) class BallEvent: name: str source_s: float monotonic_s: float confirmed_s: float position: tuple[float, float, float] def to_dict(self): return dict(event=self.name, source_s=self.source_s, monotonic_s=self.monotonic_s, confirmed_s=self.confirmed_s, position=list(self.position)) class BallDecoder: def __init__(self, origin): self.origin = np.asarray(origin, dtype=np.float64).reshape(3) if not np.isfinite(self.origin).all(): raise ValueError("invalid mocap origin") self.last_source_s = None self.offset_s = None def decode(self, payload, receipt_s): if not isinstance(payload, dict) or payload.get("type") != "mocap": raise ValueError("expected combined type=mocap payload") ball = payload.get("ball") if ball is None: return None if not isinstance(ball, dict) or ball.get("type") != "ball" or ball.get("unit") != "mm": raise ValueError("expected nested type=ball, unit=mm") stamp = float(ball.get("t", math.nan)) pos = np.asarray(ball.get("pos"), dtype=np.float64) if not math.isfinite(stamp) or stamp <= 0 or pos.shape != (3,) or not np.isfinite(pos).all(): raise ValueError("invalid ball position/source timestamp") velocity = np.asarray(ball.get("vel"), dtype=np.float64) velocity_valid = ball.get("vel_valid") if velocity.shape != (3,) or not np.isfinite(velocity).all(): raise ValueError("invalid_ball_velocity: expected finite vel[3]") if not isinstance(velocity_valid, bool): raise ValueError("invalid_ball_velocity_valid: expected bool vel_valid") if self.last_source_s is not None: if stamp < self.last_source_s - 1e-9: self.last_source_s = stamp self.offset_s = receipt_s - stamp raise ValueError("ball_time_backwards") if stamp <= self.last_source_s + 1e-9: return None # robot-only updates cannot refresh ball freshness self.last_source_s = stamp offset = receipt_s - stamp # Nested source stamps can use an epoch unrelated to monotonic time. # Minimum observed transport offset removes queueing delay; source and # receipt stamps are both retained so the alignment remains auditable. self.offset_s = offset if self.offset_s is None else min(self.offset_s, offset) return BallSample(stamp, receipt_s, stamp + self.offset_s, pos * 0.001 - self.origin, velocity * 0.001, velocity_valid) class BallStream: """Dedicated ordered SUB thread; never CONFLATE or run ROS on the policy thread.""" def __init__(self, config: RLConfig, origin): self.config = config self.decoder = BallDecoder(origin) self.racket = RacketHistory(origin) self.latest_ball = None self.pair_lock = threading.Lock() self.samples = queue.Queue(maxsize=config.ball_queue_capacity) self.stop = threading.Event() self.ready = threading.Event() self.last_receipt_s = 0.0 self.error_generation = 0 self.last_error = "" self.thread = threading.Thread(target=self._run, name="h2-real-ball-sub", daemon=True) self.thread.start() def _error(self, reason): self.last_error = str(reason) self.error_generation += 1 def _run(self): import zmq context = zmq.Context() socket = context.socket(zmq.SUB) socket.setsockopt(zmq.RCVHWM, self.config.ball_queue_capacity) socket.setsockopt(zmq.LINGER, 0) socket.setsockopt_string(zmq.SUBSCRIBE, "") try: socket.connect(self.config.zmq_endpoint) self.ready.set() while not self.stop.is_set(): if not socket.poll(10): continue try: payload, receipt = socket.recv_pyobj(), time.monotonic() with self.pair_lock: sample = self.decoder.decode(payload, receipt) # Decode racket-only snapshots even when the ball stamp # repeats; ROS callback order must not lose paired poses. self.racket.update(payload, receipt) if sample is not None: self.latest_ball = sample if sample is None: continue self.last_receipt_s = sample.receipt_s self.samples.put_nowait(sample) except queue.Full: self._error("ball_queue_overflow") except (ValueError, TypeError, KeyError, OverflowError) as error: self._error(error) except Exception as error: self._error(f"ball_source_failed:{error}") finally: socket.close() context.term() def drain(self): result = [] for _ in range(self.config.ball_queue_capacity): try: result.append(self.samples.get_nowait()) except queue.Empty: break return result def racket_distance(self, now_s): with self.pair_lock: return self.racket.distance(self.latest_ball, now_s, self.config.max_ball_gap_s) def close(self): self.stop.set() self.thread.join(timeout=2) @dataclass(frozen=True) class CourtGeometry: table_x: tuple[float, float] = TABLE_X_RANGE table_half_width: float = TABLE_HALF_WIDTH table_center_y: float = 0.0 contact_z: float = TABLE_HEIGHT + BALL_RADIUS net_x: float = NET_X net_top: float = NET_HEIGHT radius: float = BALL_RADIUS hit_x: tuple[float, float] = (0.35, 0.45) hit_y: tuple[float, float] = (-0.76, 0.76) hit_z: tuple[float, float] = (0.80, 1.50) @classmethod def from_spec(cls, spec, origin=(-1.87, 0.0, -0.76)): # Re-express the audited physical court if CLI calibration changes. shift = np.array([-1.87, 0.0, -0.76]) - np.asarray(origin, dtype=float) return cls(table_x=tuple(float(x+shift[0]) for x in TABLE_X_RANGE), table_center_y=float(shift[1]), contact_z=float(TABLE_HEIGHT+BALL_RADIUS+shift[2]), net_x=float(NET_X+shift[0]), net_top=float(NET_HEIGHT+shift[2]), hit_x=spec.hit_x_range, hit_y=spec.hit_y_range, hit_z=spec.hit_z_range) def on_table(self, position): x, y = position[:2] return self.table_x[0] <= x <= self.table_x[1] and abs(y-self.table_center_y) <= self.table_half_width def hit_region(self, position): return all(lo <= value <= hi for value, (lo, hi) in zip(position, (self.hit_x, self.hit_y, self.hit_z))) class RealBallDetector: """Conservative trajectory evidence; ambiguous outcomes are discarded upstream.""" def __init__(self, config: RLConfig, geometry: CourtGeometry): self.config, self.geometry = config, geometry # velocity_window is retained for checkpoint/config compatibility. It # now controls only the local position window used to locate a landing. self.history = deque(maxlen=config.velocity_window) self.before_return = [] self.last_sample = None self.phase = "pre_hit" self.incoming_seen = False self.outgoing_count = 0 self.miss_candidate = None self.hit_event = None self.terminal = None self.invalid_reason = None self.last_bounce_source = -math.inf self.net_reverse_count = 0 self.net_turning = None self.terminal_candidates = [] def _event(self, name, sample, *, source_s=None, position=None): source_s = sample.source_s if source_s is None else source_s position = sample.position if position is None else position return BallEvent(name, source_s, sample.monotonic_s - (sample.source_s - source_s), sample.receipt_s, tuple(float(v) for v in position)) @staticmethod def _fit_positions(samples): """Local geometric interpolation for landing time/XY only.""" times = np.array([s.source_s for s in samples]) design = np.column_stack((times - times[-1], np.ones(len(times)))) positions = np.array([s.position for s in samples]) slope, intercept = np.linalg.lstsq(design, positions, rcond=None)[0] return slope, intercept def _bounce(self, sample): window = list(self.history)[-self.config.velocity_window:] if len(window) < self.config.velocity_window: return None zs = np.array([s.position[2] for s in window]) i = int(np.argmin(zs)) if i == 0 or i == len(window)-1 or window[i].source_s <= self.last_bounce_source: return None g = self.geometry if abs(zs[i] - g.contact_z) > self.config.table_height_tolerance_m: return None before, after = window[:i+1], window[i:] v0, p0 = self._fit_positions(before) v1, p1 = self._fit_positions(after) if v0[2] >= 0 or v1[2] <= 0: return None # Intersection of the two measured local vertical fits, bounded by the # samples adjacent to the observed minimum. No ballistic rollout. ref = window[-1].source_s b0 = p0[2] + v0[2] * (ref - before[-1].source_s) b1 = p1[2] + v1[2] * (ref - after[-1].source_s) t = ref + (b1-b0) / (v0[2]-v1[2]) if not window[i-1].source_s <= t <= window[i+1].source_s: self.invalid_reason = "ambiguous_bounce_fit" return None fit_z = b0 + v0[2] * (t-ref) if abs(fit_z - g.contact_z) > self.config.table_height_tolerance_m: self.invalid_reason = "ambiguous_bounce_height" return None slope, intercept = self._fit_positions(window) position = intercept + slope * (t-ref) position[2] = g.contact_z self.last_bounce_source = window[i].source_s if not g.on_table(position): return self._event("out", sample, source_s=t, position=position) name = "bounce_opponent" if position[0] > g.net_x else "bounce_ours" return self._event(name, sample, source_s=t, position=position) def update(self, sample: BallSample, hit_deadline_s: float): if self.invalid_reason or self.terminal is not None: return [] previous = self.last_sample if previous is not None: gap = sample.source_s - previous.source_s if gap <= 0 or gap > self.config.max_ball_gap_s + 1e-9: self.invalid_reason = "ball_time_backwards" if gap <= 0 else "ball_source_gap" return [] self.last_sample = sample if self.phase == "post_hit": return self._update_post_hit(sample) c, g = self.config, self.geometry if len(self.before_return) >= c.ball_queue_capacity: self.invalid_reason = "ball_history_overflow" return [] self.before_return.append(sample) vx = float(sample.velocity[0]) if sample.velocity_valid else None self.incoming_seen |= vx is not None and vx < c.incoming_vx_mps self.outgoing_count = (self.outgoing_count + 1 if self.incoming_seen and vx is not None and vx > c.outgoing_vx_mps else 0) if self.outgoing_count >= c.outgoing_confirmations: turning_index = min(range(len(self.before_return)), key=lambda i: self.before_return[i].position[0]) turning = self.before_return[turning_index] # Keep the legacy log name, but this is a confirmed return direction, # not an estimate of racket contact or an additional reward gate. self.hit_event = replace(self._event("our_hit", turning), confirmed_s=sample.receipt_s) self.phase = "post_hit" result = [self.hit_event] # Adapter velocity may confirm the return after the first landing. # Replay positions from the X turn, in source order, exactly once. for earlier in self.before_return[turning_index:]: result.extend(self._update_post_hit(earlier, confirmed_s=sample.receipt_s)) if self.terminal is not None or self.invalid_reason: break self.before_return.clear() return result miss_evidence = (self.incoming_seen and vx is not None and vx < c.incoming_vx_mps and sample.monotonic_s >= hit_deadline_s and sample.position[0] < g.hit_x[0] - g.radius and previous is not None and sample.position[0] < previous.position[0]) if miss_evidence: if not (g.hit_y[0] <= sample.position[1] <= g.hit_y[1] and g.hit_z[0] <= sample.position[2] <= g.hit_z[1]): self.invalid_reason = "incoming_outside_h2_hit_region" return [] if self.miss_candidate is not None: self.terminal = replace(self._event("no_hit", self.miss_candidate), confirmed_s=sample.receipt_s) return [self.terminal] self.miss_candidate = sample else: self.miss_candidate = None return [] def _update_post_hit(self, sample, *, confirmed_s=None): self.history.append(sample) if len(self.history) < 2: return [] c, g = self.config, self.geometry candidates = [] bounce = self._bounce(sample) if self.invalid_reason: return [] if bounce and bounce.source_s > self.hit_event.source_s: candidates.append(bounce) prev = self.history[-2] p, q = prev.position, sample.position # Observed intersection with the physical net does not require valid # velocity. Invalid adapter velocity never counts as a net reversal. if p[0] < g.net_x <= q[0] and q[0] > p[0]: alpha = (g.net_x-p[0])/(q[0]-p[0]) cross = p + alpha*(q-p) if abs(cross[1]-g.table_center_y) <= g.table_half_width and g.contact_z-g.radius <= cross[2] <= g.net_top + g.radius: candidates.append(self._event("net_contact", sample, source_s=prev.source_s + alpha*(sample.source_s-prev.source_s), position=cross)) if self.net_turning is None or sample.position[0] > self.net_turning.position[0]: self.net_turning = sample self.net_reverse_count = (self.net_reverse_count + 1 if sample.velocity_valid and sample.velocity[0] < 0 else 0) if self.net_reverse_count >= c.outgoing_confirmations: turning = self.net_turning r = turning.position if (abs(r[0]-g.net_x) <= c.net_x_tolerance_m and abs(r[1]-g.table_center_y) <= g.table_half_width and g.contact_z-g.radius <= r[2] <= g.net_top + g.radius and turning.source_s > self.hit_event.source_s): candidates.append(self._event("net_contact", sample, source_s=turning.source_s, position=r)) # A measured downward crossing outside the exact table footprint is out. if p[2] > g.contact_z >= q[2] and q[2] < p[2]: alpha = (g.contact_z-p[2])/(q[2]-p[2]) cross = p + alpha*(q-p) if not g.on_table(cross): candidates.append(self._event("out", sample, source_s=prev.source_s + alpha*(sample.source_s-prev.source_s), position=cross)) self.terminal_candidates.extend(candidates) if not self.terminal_candidates: return [] first = min(self.terminal_candidates, key=lambda e: e.source_s) if any(e.name != first.name and abs(e.source_s-first.source_s) <= 1e-6 for e in self.terminal_candidates): self.invalid_reason = "conflicting_terminal_events" return [] # Let the local landing window pass the earliest candidate before # committing: an earlier rebound can need more frames than a net/out # crossing. This keeps physical event order, not detection order. if len(self.history) == c.velocity_window and first.source_s <= self.history[0].source_s + 1e-9: self.terminal = replace(first, confirmed_s=max(sample.receipt_s, confirmed_s if confirmed_s is not None else sample.receipt_s)) return [self.terminal] return []