160 lines
6.5 KiB
Python
160 lines
6.5 KiB
Python
"""PPI adapter utilities.
|
|
|
|
This module converts absolute states stored in the SimulationStateHub into
|
|
lightweight Target objects positioned relative to ownship and suitable for
|
|
consumption by the :class:`target_simulator.gui.ppi_display.PPIDisplay`.
|
|
|
|
Public API:
|
|
- build_display_data(simulation_hub, ...): returns a dict with keys
|
|
``'simulated'`` and ``'real'`` containing lists of targets for display.
|
|
"""
|
|
|
|
from typing import Dict, List, Optional, TYPE_CHECKING
|
|
|
|
if TYPE_CHECKING:
|
|
# Avoid runtime imports; enable type checking for forward references
|
|
from target_simulator.core.simulation_engine import SimulationEngine
|
|
from target_simulator.gui.ppi_display import PPIDisplay
|
|
from target_simulator.core.models import Scenario
|
|
from logging import Logger
|
|
import math
|
|
from target_simulator.core.models import Target
|
|
from target_simulator.analysis.simulation_state_hub import SimulationStateHub
|
|
|
|
|
|
def build_display_data(
|
|
simulation_hub: SimulationStateHub,
|
|
scenario: Optional["Scenario"] = None,
|
|
engine: Optional["SimulationEngine"] = None,
|
|
ppi_widget: Optional["PPIDisplay"] = None,
|
|
logger: Optional["Logger"] = None,
|
|
) -> Dict[str, List[Target]]:
|
|
"""
|
|
Builds PPI display data from the simulation hub, converting absolute
|
|
'real' coordinates to relative coordinates based on the ownship's position.
|
|
|
|
Returns a dict with keys 'simulated' and 'real' containing lightweight
|
|
Target objects suitable for passing to PPIDisplay.
|
|
"""
|
|
simulated_targets_for_ppi: List[Target] = []
|
|
real_targets_for_ppi: List[Target] = []
|
|
|
|
if not simulation_hub:
|
|
return {"simulated": [], "real": []}
|
|
|
|
# Get ownship state for coordinate transformation
|
|
ownship_state = simulation_hub.get_ownship_state()
|
|
ownship_pos_xy_ft = ownship_state.get("position_xy_ft")
|
|
|
|
# Get the fixed simulation origin state (ownship state at T=0)
|
|
simulation_origin = simulation_hub.get_simulation_origin()
|
|
|
|
target_ids = simulation_hub.get_all_target_ids()
|
|
|
|
for tid in target_ids:
|
|
history = simulation_hub.get_target_history(tid)
|
|
if not history:
|
|
continue
|
|
|
|
# --- Process Simulated Data (transforming from simulation frame to world frame) ---
|
|
if history.get("simulated"):
|
|
last_sim_state = history["simulated"][-1]
|
|
|
|
if len(last_sim_state) >= 6:
|
|
_ts, x_sim_ft, y_sim_ft, z_sim_ft, vel_fps, vert_vel_fps = (
|
|
last_sim_state[:6]
|
|
)
|
|
else:
|
|
_ts, x_sim_ft, y_sim_ft, z_sim_ft = last_sim_state
|
|
vel_fps, vert_vel_fps = 0.0, 0.0
|
|
|
|
# Default to identity transformation if origin is not set
|
|
x_origin_ft, y_origin_ft = 0.0, 0.0
|
|
|
|
if simulation_origin:
|
|
origin_pos = simulation_origin.get("position_xy_ft", (0.0, 0.0))
|
|
x_origin_ft, y_origin_ft = origin_pos
|
|
# NOTE: heading_origin_rad is intentionally not used for position anymore
|
|
|
|
# 1. Translate by the initial position of the ownship to get absolute world position.
|
|
x_abs_ft = x_sim_ft + x_origin_ft
|
|
y_abs_ft = y_sim_ft + y_origin_ft
|
|
|
|
# 2. Convert absolute world coordinates to coordinates relative to current ownship.
|
|
rel_x_ft, rel_y_ft = x_abs_ft, y_abs_ft
|
|
if ownship_pos_xy_ft:
|
|
rel_x_ft = x_abs_ft - ownship_pos_xy_ft[0]
|
|
rel_y_ft = y_abs_ft - ownship_pos_xy_ft[1]
|
|
|
|
sim_target = Target(target_id=tid, trajectory=[])
|
|
setattr(sim_target, "_pos_x_ft", rel_x_ft)
|
|
setattr(sim_target, "_pos_y_ft", rel_y_ft)
|
|
setattr(sim_target, "_pos_z_ft", z_sim_ft)
|
|
sim_target.current_velocity_fps = vel_fps
|
|
sim_target.current_vertical_velocity_fps = vert_vel_fps
|
|
sim_target._update_current_polar_coords()
|
|
|
|
try:
|
|
# --- START OF HEADING CORRECTION ---
|
|
# The heading from the simulation engine is already in the "world"
|
|
# frame (North-Up). No further rotation is needed.
|
|
heading = None
|
|
if engine and getattr(engine, "scenario", None):
|
|
t = engine.scenario.get_target(tid)
|
|
if t:
|
|
heading = getattr(t, "current_heading_deg", 0.0)
|
|
|
|
if heading is None and scenario:
|
|
t2 = scenario.get_target(tid)
|
|
if t2:
|
|
heading = getattr(t2, "current_heading_deg", 0.0)
|
|
# --- END OF HEADING CORRECTION ---
|
|
|
|
if heading is not None:
|
|
sim_target.current_heading_deg = float(heading)
|
|
|
|
except Exception:
|
|
pass
|
|
|
|
sim_target.active = True
|
|
# Try to get active status from engine first, then fall back to scenario
|
|
if engine and getattr(engine, "scenario", None):
|
|
t_engine = engine.scenario.get_target(tid)
|
|
if t_engine is not None:
|
|
sim_target.active = bool(getattr(t_engine, "active", True))
|
|
elif scenario:
|
|
# If engine is None (e.g., after simulation finishes), use the main scenario
|
|
t_scenario = scenario.get_target(tid)
|
|
if t_scenario is not None:
|
|
sim_target.active = bool(getattr(t_scenario, "active", True))
|
|
|
|
simulated_targets_for_ppi.append(sim_target)
|
|
|
|
# --- Process Real Data (transforming from absolute to relative) ---
|
|
if history.get("real"):
|
|
last_real_state = history["real"][-1]
|
|
_ts, abs_x_ft, abs_y_ft, abs_z_ft = last_real_state[:4]
|
|
|
|
rel_x_ft, rel_y_ft = abs_x_ft, abs_y_ft
|
|
if ownship_pos_xy_ft:
|
|
rel_x_ft = abs_x_ft - ownship_pos_xy_ft[0]
|
|
rel_y_ft = abs_y_ft - ownship_pos_xy_ft[1]
|
|
|
|
ownship_alt_ft = ownship_state.get("altitude_ft", 0.0)
|
|
rel_z_ft = abs_z_ft - ownship_alt_ft
|
|
|
|
real_target = Target(target_id=tid, trajectory=[])
|
|
setattr(real_target, "_pos_x_ft", rel_x_ft)
|
|
setattr(real_target, "_pos_y_ft", rel_y_ft)
|
|
setattr(real_target, "_pos_z_ft", rel_z_ft)
|
|
real_target._update_current_polar_coords()
|
|
|
|
hdg = simulation_hub.get_real_heading(tid)
|
|
if hdg is not None:
|
|
real_target.current_heading_deg = float(hdg) % 360
|
|
|
|
real_target.active = True
|
|
real_targets_for_ppi.append(real_target)
|
|
|
|
return {"simulated": simulated_targets_for_ppi, "real": real_targets_for_ppi}
|