497 lines
19 KiB
Python
497 lines
19 KiB
Python
# 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.
|
|
"""
|
|
|
|
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.
|
|
"""
|
|
|
|
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._initialize_target(target_id)
|
|
|
|
# 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._initialize_target(target_id)
|
|
|
|
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 - float(window_seconds)
|
|
# Count timestamps >= cutoff
|
|
count = 0
|
|
# Iterate from the right (newest) backwards until below cutoff
|
|
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 - float(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() |