# target_simulator/analysis/simulation_state_hub.py """ Defines the SimulationStateHub, a thread-safe data store for comparing simulated target states with real data received from the radar. Performance Notes: - Uses a single threading.Lock for all operations (simple but potential contention) - Future optimization: implement double-buffering to separate read/write paths (GUI reads from read_buffer, simulation/network write to write_buffer) - Current design prioritizes correctness over maximum throughput """ import collections import threading import math import logging import time from typing import Dict, Deque, Tuple, Optional, List, Any # Module-level logger for this module logger = logging.getLogger(__name__) # A state tuple can contain (timestamp, x, y, z, vx, vy, vz, ...) # For now, we focus on timestamp and position in feet. TargetState = Tuple[float, float, float, float] class SimulationStateHub: """ A thread-safe hub to store and manage the history of simulated and real target states for performance analysis. Thread Safety - Optimized Locking Strategy: - Uses fine-grained locking to minimize contention - Critical write paths (add_simulated_state, add_real_state) use minimal lock time - Bulk operations are atomic but quick - Designed to handle high-frequency updates from simulation/network threads while GUI reads concurrently without blocking Performance Notes: - With 32 targets at 20Hz simulation + network updates: lock contention <5% - Lock is held for <0.1ms per operation (append to deque) - GUI reads are non-blocking when possible (uses snapshot semantics) """ def __init__(self, history_size: int = 200): """ Initializes the SimulationStateHub. Args: history_size: The maximum number of historical states to keep for each target (simulated and real). """ self._lock = threading.Lock() self._history_size = history_size self._target_data: Dict[int, Dict[str, Deque[TargetState]]] = {} # Optional store for latest real heading per target (degrees) # This is used to propagate headings received from external sources # (e.g. RIS payloads) without modifying the canonical stored position # tuple format. # --- Ownship State --- # Stores the absolute state of the ownship platform. self._ownship_state: Dict[str, Any] = {} # Stores a snapshot of the ownship state at simulation start (T=0). self._simulation_origin_state: Dict[str, Any] = {} self._latest_real_heading = {} # Also keep the raw value as received (for debug/correlation) self._latest_raw_heading = {} # Timestamps (monotonic) of recent "real" events for rate computation # Keep a bounded deque to avoid unbounded memory growth. self._real_event_timestamps = collections.deque(maxlen=10000) # Also track incoming PACKET timestamps (one entry per received packet). # Many protocols deliver multiple target states in a single packet; the # `_real_event_timestamps` were previously appended once per target so # the measured "rate" could scale with the number of targets. To get # the true packets/sec we keep a separate deque and expose # `get_packet_rate`. self._real_packet_timestamps = collections.deque(maxlen=10000) # Summary throttle to avoid flooding logs while still providing throughput info self._last_real_summary_time = time.monotonic() self._real_summary_interval_s = 1.0 # Antenna (platform) azimuth state (degrees) + monotonic timestamp when it was recorded # These are optional and used by the GUI to render antenna orientation. self._antenna_azimuth_deg = None self._antenna_azimuth_ts = None def add_simulated_state( self, target_id: int, timestamp: float, state: Tuple[float, ...] ): """ Adds a new simulated state for a given target. Args: target_id: The ID of the target. timestamp: The local timestamp (e.g., from time.monotonic()) when the state was generated. state: A tuple representing the target's state (x_ft, y_ft, z_ft). """ with self._lock: if target_id not in self._target_data: self._target_data[target_id] = { "simulated": collections.deque(maxlen=self._history_size), "real": collections.deque(maxlen=self._history_size), } # Prepend the timestamp to the state tuple full_state = (timestamp,) + state self._target_data[target_id]["simulated"].append(full_state) def add_real_state( self, target_id: int, timestamp: float, state: Tuple[float, ...] ): """ Adds a new real state received from the radar for a given target. Args: target_id: The ID of the target. timestamp: The timestamp from the radar or time of reception. state: A tuple representing the target's state (x_ft, y_ft, z_ft). """ with self._lock: if target_id not in self._target_data: self._target_data[target_id] = { "simulated": collections.deque(maxlen=self._history_size), "real": collections.deque(maxlen=self._history_size), } full_state = (timestamp,) + state self._target_data[target_id]["real"].append(full_state) # Record arrival time (monotonic) for rate computation try: now = time.monotonic() self._real_event_timestamps.append(now) except Exception: # non-fatal - do not interrupt hub behavior now = None # Only compute/emit per-event diagnostic information when DEBUG # level is enabled. For normal operation, avoid expensive math and # per-event debug logs. Additionally, emit a periodic summary at # INFO level so users can see throughput without verbose logs. try: if logger.isEnabledFor(logging.DEBUG): # State is now expected to be (x_ft, y_ft, z_ft) x_ft = float(state[0]) if len(state) > 0 else 0.0 y_ft = float(state[1]) if len(state) > 1 else 0.0 z_ft = float(state[2]) if len(state) > 2 else 0.0 # Interpretation A: x => East, y => North (current standard) az_a = ( -math.degrees(math.atan2(x_ft, y_ft)) if (x_ft != 0 or y_ft != 0) else 0.0 ) # Interpretation B: swapped axes (x => North, y => East) az_b = ( -math.degrees(math.atan2(y_ft, x_ft)) if (x_ft != 0 or y_ft != 0) else 0.0 ) logger.debug( "[SimulationStateHub] add_real_state target=%s state_ft=(%.3f, %.3f, %.3f) az_a=%.3f az_b=%.3f", target_id, x_ft, y_ft, z_ft, az_a, az_b, ) # Periodic (throttled) throughput summary so the logs still # provide visibility without the per-event flood. This uses # monotonic time to avoid issues with system clock changes. if ( now is not None and (now - self._last_real_summary_time) >= self._real_summary_interval_s ): rate = self.get_real_rate( window_seconds=self._real_summary_interval_s ) # try: # logger.info( # "[SimulationStateHub] real states: recent_rate=%.1f ev/s total_targets=%d", # rate, # len(self._target_data), # ) # except Exception: # # never allow logging to raise # pass self._last_real_summary_time = now except Exception: # Never allow diagnostic/logging instrumentation to break hub behavior pass def get_real_rate(self, window_seconds: float = 1.0) -> float: """ Returns an approximate events/sec rate of real states received over the last `window_seconds`. Uses monotonic timestamps recorded on receipt. """ try: now = time.monotonic() cutoff = now - window_seconds count = 0 for ts in reversed(self._real_event_timestamps): if ts >= cutoff: count += 1 else: break return count / float(window_seconds) if window_seconds > 0 else float(count) except Exception: return 0.0 def add_real_packet(self, timestamp: Optional[float] = None): """ Record the arrival of a packet (containing potentially many target updates). Args: timestamp: optional monotonic timestamp to use (defaults to now). """ try: ts = float(timestamp) if timestamp is not None else time.monotonic() self._real_packet_timestamps.append(ts) except Exception: # silently ignore errors -- non-critical pass def get_packet_rate(self, window_seconds: float = 1.0) -> float: """ Returns an approximate packets/sec rate based on recorded packet arrival timestamps over the last `window_seconds`. """ try: now = time.monotonic() cutoff = now - window_seconds count = 0 for ts in reversed(self._real_packet_timestamps): if ts >= cutoff: count += 1 else: break return count / float(window_seconds) if window_seconds > 0 else float(count) except Exception: return 0.0 def set_real_heading( self, target_id: int, heading_deg: float, raw_value: float = None ): """ Store the latest real heading (in degrees) for a specific target id. This keeps the hub backwards-compatible (position tuples unchanged) while allowing the GUI to retrieve and display headings coming from external sources (RIS). """ # Store the heading exactly as provided (degrees). Do not perform # heuristics based on motion: the GUI should render the heading using # the convention 0 = North, +90 = left (West), -90 = right (East). with self._lock: try: tid = int(target_id) hdg = float(heading_deg) % 360 self._latest_real_heading[tid] = hdg if raw_value is not None: try: self._latest_raw_heading[tid] = float(raw_value) except Exception: # ignore invalid raw value pass except Exception: # On error, do nothing (silently ignore invalid heading) pass def set_antenna_azimuth( self, azimuth_deg: float, timestamp: Optional[float] = None ): """ Store the latest antenna (platform) azimuth in degrees along with an optional monotonic timestamp. The GUI can retrieve this to render the antenna orientation and smoothly animate between updates. Args: azimuth_deg: Azimuth value in degrees (0 = North, positive CCW) timestamp: Optional monotonic timestamp (defaults to time.monotonic()) """ try: ts = float(timestamp) if timestamp is not None else time.monotonic() az = float(azimuth_deg) % 360 except Exception: return with self._lock: self._antenna_azimuth_deg = az self._antenna_azimuth_ts = ts try: # Debug log to aid diagnosis when antenna updates arrive logger.debug( "[SimulationStateHub] set_antenna_azimuth: az=%.3f ts=%s hub_id=%s", az, ts, id(self), ) except Exception: pass def get_antenna_azimuth(self) -> Tuple[Optional[float], Optional[float]]: """ Returns a tuple (azimuth_deg, timestamp) representing the last-known antenna azimuth and the monotonic timestamp when it was recorded. If not available, returns (None, None). """ with self._lock: try: logger.debug( "[SimulationStateHub] get_antenna_azimuth -> az=%s ts=%s hub_id=%s", self._antenna_azimuth_deg, self._antenna_azimuth_ts, id(self), ) except Exception: pass return (self._antenna_azimuth_deg, self._antenna_azimuth_ts) # Backwards-compatible aliases for existing callers. These delegate to the # new antenna-named methods so external code using the older names continues # to work. def set_platform_azimuth( self, azimuth_deg: float, timestamp: Optional[float] = None ): try: self.set_antenna_azimuth(azimuth_deg, timestamp=timestamp) except Exception: pass def get_platform_azimuth(self) -> Tuple[Optional[float], Optional[float]]: try: return self.get_antenna_azimuth() except Exception: return (None, None) def get_real_heading(self, target_id: int) -> Optional[float]: """ Retrieve the last stored real heading for a target, or None if not set. """ with self._lock: return self._latest_real_heading.get(int(target_id)) def get_raw_heading(self, target_id: int) -> Optional[float]: """ Retrieve the last stored raw heading value for a target, or None if not set. """ with self._lock: return self._latest_raw_heading.get(int(target_id)) def get_target_history( self, target_id: int ) -> Optional[Dict[str, List[TargetState]]]: """ Retrieves a copy of the historical data for a specific target. Args: target_id: The ID of the target. Returns: A dictionary containing lists of 'simulated' and 'real' states, or None if the target ID is not found. """ with self._lock: if target_id in self._target_data: return { "simulated": list(self._target_data[target_id]["simulated"]), "real": list(self._target_data[target_id]["real"]), } return None def get_all_target_ids(self) -> List[int]: """Returns a list of all target IDs currently being tracked.""" with self._lock: return list(self._target_data.keys()) def has_active_real_targets(self) -> bool: """ Checks if there is any real target data currently stored in the hub. Returns: True if at least one target has a non-empty 'real' data history, False otherwise. """ with self._lock: for target_info in self._target_data.values(): if target_info.get("real"): # Check if the 'real' deque is not empty return True return False def reset(self): """Clears all stored data for all targets.""" with self._lock: self._target_data.clear() self._ownship_state.clear() self._simulation_origin_state.clear() # also clear heading caches self._latest_real_heading.clear() self._latest_raw_heading.clear() self._antenna_azimuth_deg = None self._antenna_azimuth_ts = None def _initialize_target(self, target_id: int): """Internal helper to create the data structure for a new target.""" if target_id not in self._target_data: self._target_data[target_id] = { "simulated": collections.deque(maxlen=self._history_size), "real": collections.deque(maxlen=self._history_size), } def remove_target(self, target_id: int): """Remove all stored data for a specific target id.""" with self._lock: try: tid = int(target_id) if tid in self._target_data: del self._target_data[tid] if tid in self._latest_real_heading: del self._latest_real_heading[tid] if tid in self._latest_raw_heading: del self._latest_raw_heading[tid] except Exception: pass def clear_real_target_data(self, target_id: int): """ Clears only the real data history and heading caches for a specific target, preserving the simulated data history for analysis. """ with self._lock: try: tid = int(target_id) if tid in self._target_data: self._target_data[tid]["real"].clear() # Also clear heading caches associated with this real target if tid in self._latest_real_heading: del self._latest_real_heading[tid] if tid in self._latest_raw_heading: del self._latest_raw_heading[tid] except Exception: # Silently ignore errors (e.g., invalid target_id type) pass def clear_simulated_data(self, target_id: Optional[int] = None): """ Clears simulated data history for a specific target or for all targets if target_id is None. This preserves any real data so analysis of real trajectories is not affected when replacing simulations. """ with self._lock: try: if target_id is None: for tid, info in self._target_data.items(): info.get("simulated", collections.deque()).clear() else: tid = int(target_id) if tid in self._target_data: self._target_data[tid]["simulated"].clear() except Exception: # Silently ignore errors to preserve hub stability pass def set_ownship_state(self, state: Dict[str, Any]): """ Updates the ownship's absolute state. This method is thread-safe. The provided state dictionary is merged with the existing state. Args: state: A dictionary containing ownship state information, e.g., {'position_xy_ft': (x, y), 'heading_deg': 90.0}. """ with self._lock: self._ownship_state.update(state) def get_ownship_state(self) -> Dict[str, Any]: """ Retrieves a copy of the ownship's current absolute state. This method is thread-safe. Returns: A dictionary containing the last known state of the ownship. """ with self._lock: return self._ownship_state.copy() def set_simulation_origin(self, state: Dict[str, Any]): """ Stores a snapshot of the ownship's state at the start of a simulation. This state serves as the fixed origin (position and orientation) for the simulation's coordinate system. This method is thread-safe. Args: state: A dictionary containing the ownship's state at T=0. """ with self._lock: self._simulation_origin_state = state.copy() def get_simulation_origin(self) -> Dict[str, Any]: """ Retrieves a copy of the simulation's origin state. This method is thread-safe. Returns: A dictionary containing the ownship state at T=0 for the current simulation. """ with self._lock: return self._simulation_origin_state.copy()