# target_simulator/utils/clock_synchronizer.py """Clock synchronization helpers. This module provides ``ClockSynchronizer``, a small utility that maps a remote 32-bit wrapping timetag into the local monotonic clock. It uses a linear regression model (NumPy) to estimate offset and drift and estimates one-way latency. Note: NumPy is required by this module; an ImportError is raised when it is not available. """ import collections import threading import time from typing import List, Tuple # NumPy is a strong recommendation for linear regression. # If it's not already a dependency, it should be added. try: import numpy as np NUMPY_AVAILABLE = True except ImportError: NUMPY_AVAILABLE = False class ClockSynchronizer: """Synchronize a remote 32-bit wrapping counter to local monotonic time. The synchronizer records pairs of (server_timetag, client_reception_time), unwraps the server counter across wrap-around events and fits a linear model (client_time = m * server_ticks + b). The fit is performed using NumPy's polyfit when a configurable minimum number of samples is available. """ # Constants for a 32-bit counter _COUNTER_MAX = 2**32 _WRAP_THRESHOLD = 2**31 # Detect wrap if decrease is > half the max value def __init__( self, history_size: int = 100, min_samples_for_fit: int = 20, update_interval: int = 100, ): """Create a new ClockSynchronizer. Args: history_size: Maximum number of recent samples retained for fitting. min_samples_for_fit: Minimum samples required before performing a regression fit. update_interval: The model will be refit every `update_interval` samples. Raises: ImportError: If NumPy is not available on the system. """ if not NUMPY_AVAILABLE: raise ImportError("NumPy is required for the ClockSynchronizer.") self._lock = threading.Lock() self._history: collections.deque = collections.deque(maxlen=history_size) self._min_samples = min_samples_for_fit self._update_interval = max(1, update_interval) self._update_counter = 0 # State for timestamp unwrapping self._wrap_count: int = 0 self._last_raw_timetag: int | None = None # Linear model parameters: client_time = m * server_unwrapped_ticks + b self._m: float = 0.0 # Slope (client seconds per server tick) self._b: float = 0.0 # Intercept (client time when server time was 0) # Estimated one-way latency from server to client self._average_latency_s: float = 0.0 def add_sample(self, raw_server_timetag: int, client_reception_time: float): """Add a new (server_timetag, client_time) sample. The method will unwrap the provided 32-bit timetag accounting for wrap events and append the unwrapped pair to the internal history. The regression fit is only recomputed periodically based on `update_interval`. Args: raw_server_timetag: Raw 32-bit server timetag. client_reception_time: Local monotonic time of the packet reception. """ with self._lock: # --- Timestamp Unwrapping Logic --- if self._last_raw_timetag is None: # First sample, assume no wraps yet. self._last_raw_timetag = raw_server_timetag else: # Check for a wrap-around diff = self._last_raw_timetag - raw_server_timetag if diff > self._WRAP_THRESHOLD: self._wrap_count += 1 self._last_raw_timetag = raw_server_timetag unwrapped_timetag = ( raw_server_timetag + self._wrap_count * self._COUNTER_MAX ) # Add the new sample to history self._history.append((unwrapped_timetag, client_reception_time)) # --- Throttled Model Update --- self._update_counter += 1 if self._update_counter >= self._update_interval: self._update_counter = 0 self._update_model() def _update_model(self): """Internal: fit a linear model to the sample history. This updates slope/intercept (m, b) and computes an estimated average one-way latency. Must be called while holding the instance lock. """ if len(self._history) < self._min_samples: # Not enough data for a reliable fit return x_vals = np.array([sample[0] for sample in self._history]) y_vals = np.array([sample[1] for sample in self._history]) try: m, b = np.polyfit(x_vals, y_vals, 1) self._m = m self._b = b # --- Calculate Average Latency --- # Estimated generation time for each sample based on the new model estimated_generation_times = self._m * x_vals + self._b # Latency is the difference between reception and estimated generation latencies = y_vals - estimated_generation_times # Update the average latency, filtering out negative values which are artifacts positive_latencies = latencies[latencies >= 0] if len(positive_latencies) > 0: self._average_latency_s = np.mean(positive_latencies) else: self._average_latency_s = 0.0 except np.linalg.LinAlgError: pass def to_client_time(self, raw_server_timetag: int) -> float: """Map a raw server timetag to an estimated client monotonic time. The method unwraps the provided 32-bit timetag using the current wrap count and applies the linear model (m, b) to estimate the corresponding local monotonic time. Args: raw_server_timetag: Raw 32-bit server timetag. Returns: Estimated client monotonic time (float). """ with self._lock: current_wrap_count = self._wrap_count if self._last_raw_timetag is not None: # Handle cases where timetag might be from a slightly older packet diff = self._last_raw_timetag - raw_server_timetag if diff < -self._WRAP_THRESHOLD: # Wrapped in the other direction current_wrap_count -= 1 unwrapped_timetag = ( raw_server_timetag + current_wrap_count * self._COUNTER_MAX ) estimated_time = self._m * unwrapped_timetag + self._b return estimated_time def get_average_latency_s(self) -> float: """Return the estimated average one-way server->client latency in seconds. Returns 0.0 when insufficient data is available to compute the metric. """ with self._lock: return self._average_latency_s