62 lines
1.9 KiB
Python
62 lines
1.9 KiB
Python
import math
|
|
import json
|
|
import os
|
|
from target_simulator.analysis.simulation_archive import SimulationArchive
|
|
|
|
try:
|
|
from pyproj import Geod
|
|
|
|
GEOD = Geod(ellps="WGS84")
|
|
except Exception:
|
|
GEOD = None
|
|
|
|
|
|
def test_geopos_computation(tmp_path):
|
|
# Create a minimal dummy scenario object expected by SimulationArchive
|
|
D = type("D", (), {})()
|
|
D.name = "geo_test"
|
|
D.to_dict = lambda: {"dummy": True}
|
|
|
|
sa = SimulationArchive(D)
|
|
|
|
# Ownship at lat=45.0, lon=9.0, with local origin (0,0) ft
|
|
ownship = {
|
|
"timestamp": 100.0,
|
|
"latitude": 45.0,
|
|
"longitude": 9.0,
|
|
"position_xy_ft": (0.0, 0.0),
|
|
}
|
|
sa.add_ownship_state(ownship)
|
|
|
|
# Place a target 100 meters East and 200 meters North of ownship
|
|
delta_east_m = 100.0
|
|
delta_north_m = 200.0
|
|
# Convert to feet used by the archive
|
|
target_x_ft = delta_east_m / 0.3048
|
|
target_y_ft = delta_north_m / 0.3048
|
|
|
|
# Add a real state; timestamp close to ownship timestamp
|
|
sa.add_real_state(1, 100.5, (target_x_ft, target_y_ft, 30.0))
|
|
|
|
# The archive should have computed geopos for target id 1
|
|
assert 1 in sa.recorded_geopos
|
|
gps = sa.recorded_geopos[1]
|
|
assert len(gps) >= 1
|
|
sample = gps[-1]
|
|
|
|
# Compute expected lat/lon using geodetic forward if available
|
|
if GEOD is not None:
|
|
dist = math.hypot(delta_east_m, delta_north_m)
|
|
az_rad = math.atan2(delta_east_m, delta_north_m)
|
|
az_deg = math.degrees(az_rad)
|
|
lon2, lat2, _ = GEOD.fwd(
|
|
ownship["longitude"], ownship["latitude"], az_deg, dist
|
|
)
|
|
# Compare within ~0.5 meter -> ~5e-6 degrees
|
|
assert abs(sample["lat"] - lat2) < 5e-6
|
|
assert abs(sample["lon"] - lon2) < 5e-6
|
|
else:
|
|
# Fallback check: lat/lon should be numbers and within reasonable bounds
|
|
assert isinstance(sample["lat"], float)
|
|
assert isinstance(sample["lon"], float)
|