424 lines
13 KiB
Python
424 lines
13 KiB
Python
import pytest
|
|
import math
|
|
|
|
from target_simulator.core.models import (
|
|
Target,
|
|
Waypoint,
|
|
ManeuverType,
|
|
TurnDirection,
|
|
KNOTS_TO_FPS,
|
|
NM_TO_FT,
|
|
G_IN_FPS2,
|
|
)
|
|
from pytest import approx
|
|
|
|
|
|
@pytest.fixture
|
|
def initial_waypoint() -> Waypoint:
|
|
return Waypoint(
|
|
maneuver_type=ManeuverType.FLY_TO_POINT,
|
|
duration_s=0.0,
|
|
target_range_nm=0.0,
|
|
target_azimuth_deg=0.0,
|
|
target_altitude_ft=10000.0,
|
|
target_velocity_fps=500.0,
|
|
target_heading_deg=0.0,
|
|
)
|
|
|
|
|
|
def test_generate_path_straight_fly_for_duration():
|
|
start_velocity_fps = 500.0
|
|
trajectory = [
|
|
Waypoint(
|
|
maneuver_type=ManeuverType.FLY_TO_POINT,
|
|
duration_s=10.0,
|
|
target_range_nm=20.0,
|
|
target_azimuth_deg=90.0,
|
|
target_altitude_ft=10000.0,
|
|
target_velocity_fps=start_velocity_fps,
|
|
target_heading_deg=90.0,
|
|
),
|
|
Waypoint(
|
|
maneuver_type=ManeuverType.FLY_FOR_DURATION,
|
|
duration_s=10.0,
|
|
target_velocity_fps=start_velocity_fps,
|
|
target_heading_deg=90.0,
|
|
target_altitude_ft=10000.0,
|
|
),
|
|
]
|
|
path, total_duration = Target.generate_path_from_waypoints(
|
|
trajectory, use_spline=False
|
|
)
|
|
assert path is not None
|
|
assert len(path) == 2
|
|
assert total_duration == approx(10.0)
|
|
start_point = path[0]
|
|
expected_start_x_ft = 20.0 * NM_TO_FT
|
|
expected_start_y_ft = 0.0
|
|
assert start_point[1] == approx(expected_start_x_ft)
|
|
assert start_point[2] == approx(expected_start_y_ft, abs=1e-9)
|
|
end_point = path[1]
|
|
distance_traveled_ft = start_velocity_fps * 10.0
|
|
expected_end_x_ft = expected_start_x_ft + distance_traveled_ft
|
|
assert end_point[1] == approx(expected_end_x_ft)
|
|
assert end_point[2] == approx(expected_start_y_ft, abs=1e-9)
|
|
assert end_point[3] == approx(10000.0)
|
|
|
|
|
|
def test_generate_path_dynamic_turn(initial_waypoint):
|
|
speed_fps = initial_waypoint.target_velocity_fps
|
|
turn_g = 1.0
|
|
duration_s = 10.0
|
|
trajectory = [
|
|
initial_waypoint,
|
|
Waypoint(
|
|
maneuver_type=ManeuverType.DYNAMIC_MANEUVER,
|
|
duration_s=duration_s,
|
|
maneuver_speed_fps=speed_fps,
|
|
lateral_acceleration_g=turn_g,
|
|
turn_direction=TurnDirection.RIGHT,
|
|
),
|
|
]
|
|
path, total_duration = Target.generate_path_from_waypoints(
|
|
trajectory, use_spline=False
|
|
)
|
|
assert total_duration == approx(duration_s, rel=0.01)
|
|
assert len(path) == 101
|
|
|
|
# --- Verification based on final heading and distance ---
|
|
accel_fps2 = turn_g * G_IN_FPS2
|
|
turn_rate_rad_s = accel_fps2 / speed_fps
|
|
expected_final_heading_deg = (
|
|
initial_waypoint.target_heading_deg + math.degrees(turn_rate_rad_s * duration_s)
|
|
) % 360
|
|
|
|
final_point = path[-1]
|
|
prev_point = path[-2]
|
|
dx = final_point[1] - prev_point[1]
|
|
dy = final_point[2] - prev_point[2]
|
|
final_heading_deg = math.degrees(math.atan2(dx, dy)) % 360
|
|
|
|
# Assert that final heading is close to what's expected
|
|
assert final_heading_deg == approx(expected_final_heading_deg, abs=1.0)
|
|
|
|
# Assert that the final position is on the circle radius
|
|
radius_ft = speed_fps**2 / accel_fps2
|
|
center_x = radius_ft # Center for a right turn starting North
|
|
final_dist_from_center = math.sqrt(
|
|
(final_point[1] - center_x) ** 2 + (final_point[2] - 0) ** 2
|
|
)
|
|
assert final_dist_from_center == approx(radius_ft, rel=0.01)
|
|
|
|
|
|
def test_generate_path_dynamic_climb_accelerate(initial_waypoint):
|
|
duration_s = 10.0
|
|
trajectory = [
|
|
initial_waypoint,
|
|
Waypoint(
|
|
maneuver_type=ManeuverType.DYNAMIC_MANEUVER,
|
|
duration_s=duration_s,
|
|
longitudinal_acceleration_g=0.5,
|
|
vertical_acceleration_g=0.2,
|
|
),
|
|
]
|
|
path, total_duration = Target.generate_path_from_waypoints(
|
|
trajectory, use_spline=False
|
|
)
|
|
assert total_duration == approx(duration_s, rel=0.01)
|
|
assert len(path) == 101
|
|
start_speed = initial_waypoint.target_velocity_fps
|
|
accel_lon_fps2 = 0.5 * G_IN_FPS2
|
|
accel_ver_fps2 = 0.2 * G_IN_FPS2
|
|
expected_y = (start_speed * duration_s) + (0.5 * accel_lon_fps2 * duration_s**2)
|
|
expected_z_gain = 0.5 * accel_ver_fps2 * duration_s**2
|
|
expected_z = initial_waypoint.target_altitude_ft + expected_z_gain
|
|
final_point = path[-1]
|
|
assert final_point[1] == approx(0, abs=1e-9)
|
|
assert final_point[2] == approx(expected_y, rel=0.01)
|
|
assert final_point[3] == approx(expected_z, rel=0.01)
|
|
|
|
|
|
def test_target_update_state_interpolation():
|
|
trajectory = [
|
|
Waypoint(
|
|
maneuver_type=ManeuverType.FLY_TO_POINT,
|
|
target_range_nm=10.0,
|
|
target_azimuth_deg=0.0,
|
|
),
|
|
Waypoint(
|
|
maneuver_type=ManeuverType.FLY_TO_POINT,
|
|
duration_s=10.0,
|
|
target_range_nm=20.0,
|
|
target_azimuth_deg=0.0,
|
|
),
|
|
]
|
|
target = Target(target_id=1, trajectory=trajectory)
|
|
target.update_state(delta_time_s=5.0)
|
|
assert target.current_range_nm == approx(15.0)
|
|
assert target.current_azimuth_deg == approx(0.0)
|
|
expected_speed_fps = (10.0 * NM_TO_FT) / 10.0
|
|
|
|
assert target.current_velocity_fps == approx(expected_speed_fps, rel=0.05)
|
|
|
|
|
|
# --- Test aggiuntivi per models.py ---
|
|
def test_waypoint_to_dict():
|
|
wp = Waypoint(
|
|
maneuver_type=ManeuverType.FLY_TO_POINT,
|
|
duration_s=5.0,
|
|
target_range_nm=2.0,
|
|
target_azimuth_deg=45.0,
|
|
target_altitude_ft=5000.0,
|
|
target_velocity_fps=300.0,
|
|
target_heading_deg=90.0,
|
|
turn_direction=TurnDirection.LEFT,
|
|
)
|
|
d = wp.to_dict()
|
|
assert d["maneuver_type"] == "Fly to Point"
|
|
assert d["duration_s"] == 5.0
|
|
assert d["target_range_nm"] == 2.0
|
|
assert d["target_azimuth_deg"] == 45.0
|
|
assert d["target_altitude_ft"] == 5000.0
|
|
assert d["target_velocity_fps"] == 300.0
|
|
assert d["target_heading_deg"] == 90.0
|
|
assert d["turn_direction"] == "Left"
|
|
|
|
|
|
def test_target_id_validation():
|
|
with pytest.raises(ValueError):
|
|
Target(target_id=-1, trajectory=[])
|
|
with pytest.raises(ValueError):
|
|
Target(target_id=100, trajectory=[])
|
|
# Valori validi
|
|
t = Target(target_id=0, trajectory=[])
|
|
assert t.target_id == 0
|
|
t = Target(target_id=15, trajectory=[])
|
|
assert t.target_id == 15
|
|
|
|
|
|
def test_target_reset_simulation_sets_state():
|
|
trajectory = [
|
|
Waypoint(
|
|
maneuver_type=ManeuverType.FLY_TO_POINT,
|
|
target_range_nm=1.0,
|
|
target_azimuth_deg=0.0,
|
|
)
|
|
]
|
|
t = Target(target_id=1, trajectory=trajectory)
|
|
t.current_range_nm = 999.0
|
|
t.reset_simulation()
|
|
assert t.current_range_nm == approx(1.0)
|
|
assert t.active is True
|
|
|
|
|
|
def test_target_to_dict():
|
|
trajectory = [
|
|
Waypoint(
|
|
maneuver_type=ManeuverType.FLY_TO_POINT,
|
|
target_range_nm=1.0,
|
|
target_azimuth_deg=0.0,
|
|
)
|
|
]
|
|
t = Target(target_id=2, trajectory=trajectory, traceable=False, use_spline=True)
|
|
d = t.to_dict()
|
|
assert d["target_id"] == 2
|
|
assert d["traceable"] is False
|
|
assert d["use_spline"] is True
|
|
assert isinstance(d["trajectory"], list)
|
|
|
|
|
|
def test_scenario_add_remove_get_targets():
|
|
from target_simulator.core.models import Scenario
|
|
|
|
s = Scenario(name="TestScenario")
|
|
t1 = Target(target_id=1, trajectory=[])
|
|
t2 = Target(target_id=2, trajectory=[])
|
|
s.add_target(t1)
|
|
s.add_target(t2)
|
|
assert s.get_target(1) is t1
|
|
assert s.get_target(2) is t2
|
|
s.remove_target(1)
|
|
assert s.get_target(1) is None
|
|
assert len(s.get_all_targets()) == 1
|
|
|
|
|
|
def test_scenario_reset_and_update_state():
|
|
from target_simulator.core.models import Scenario
|
|
|
|
trajectory = [
|
|
Waypoint(
|
|
maneuver_type=ManeuverType.FLY_TO_POINT,
|
|
target_range_nm=1.0,
|
|
target_azimuth_deg=0.0,
|
|
),
|
|
Waypoint(
|
|
maneuver_type=ManeuverType.FLY_TO_POINT,
|
|
duration_s=10.0,
|
|
target_range_nm=2.0,
|
|
target_azimuth_deg=0.0,
|
|
),
|
|
]
|
|
t = Target(target_id=1, trajectory=trajectory)
|
|
s = Scenario()
|
|
s.add_target(t)
|
|
t.update_state(5.0)
|
|
assert t.current_range_nm != approx(1.0)
|
|
s.reset_simulation()
|
|
assert t.current_range_nm == approx(1.0)
|
|
s.update_state(10.0)
|
|
assert t.current_range_nm == approx(2.0)
|
|
|
|
|
|
def test_generate_path_fly_to_point_with_velocity_and_heading():
|
|
# Copre le righe 193-194
|
|
trajectory = [
|
|
Waypoint(
|
|
maneuver_type=ManeuverType.FLY_TO_POINT,
|
|
duration_s=5.0,
|
|
target_range_nm=1.0,
|
|
target_azimuth_deg=0.0,
|
|
target_altitude_ft=1000.0,
|
|
target_velocity_fps=300.0,
|
|
target_heading_deg=90.0,
|
|
),
|
|
Waypoint(
|
|
maneuver_type=ManeuverType.FLY_TO_POINT,
|
|
duration_s=5.0,
|
|
target_range_nm=2.0,
|
|
target_azimuth_deg=0.0,
|
|
target_altitude_ft=1000.0,
|
|
target_velocity_fps=350.0,
|
|
target_heading_deg=180.0,
|
|
),
|
|
]
|
|
path, total_duration = Target.generate_path_from_waypoints(
|
|
trajectory, use_spline=False
|
|
)
|
|
assert total_duration == approx(5.0)
|
|
assert len(path) > 0
|
|
|
|
|
|
def test_generate_path_with_spline():
|
|
# Copre le righe 261-275
|
|
trajectory = [
|
|
Waypoint(
|
|
maneuver_type=ManeuverType.FLY_TO_POINT,
|
|
target_range_nm=1.0,
|
|
target_azimuth_deg=0.0,
|
|
),
|
|
Waypoint(
|
|
maneuver_type=ManeuverType.FLY_TO_POINT,
|
|
target_range_nm=2.0,
|
|
target_azimuth_deg=45.0,
|
|
),
|
|
Waypoint(
|
|
maneuver_type=ManeuverType.FLY_TO_POINT,
|
|
target_range_nm=3.0,
|
|
target_azimuth_deg=90.0,
|
|
),
|
|
Waypoint(
|
|
maneuver_type=ManeuverType.FLY_TO_POINT,
|
|
target_range_nm=4.0,
|
|
target_azimuth_deg=135.0,
|
|
),
|
|
]
|
|
path, total_duration = Target.generate_path_from_waypoints(
|
|
trajectory, use_spline=True
|
|
)
|
|
assert total_duration >= 0.0
|
|
assert len(path) >= 4
|
|
|
|
|
|
def test_generate_path_empty_or_wrong_type():
|
|
# Copre la riga 280
|
|
path, total_duration = Target.generate_path_from_waypoints([], use_spline=False)
|
|
assert path == []
|
|
assert total_duration == 0.0
|
|
# Waypoint con tipo diverso
|
|
trajectory = [Waypoint(maneuver_type=ManeuverType.FLY_FOR_DURATION)]
|
|
path, total_duration = Target.generate_path_from_waypoints(
|
|
trajectory, use_spline=False
|
|
)
|
|
assert path == []
|
|
assert total_duration == 0.0
|
|
|
|
|
|
def test_scenario_is_finished():
|
|
# Copre la riga 370
|
|
from target_simulator.core.models import Scenario
|
|
|
|
t1 = Target(target_id=1, trajectory=[])
|
|
t1.active = False
|
|
t2 = Target(target_id=2, trajectory=[])
|
|
t2.active = False
|
|
s = Scenario()
|
|
s.add_target(t1)
|
|
s.add_target(t2)
|
|
assert s.is_finished() is True
|
|
|
|
|
|
def test_scenario_from_dict_constant_turn():
|
|
# Copre le righe 386-388
|
|
from target_simulator.core.models import Scenario
|
|
|
|
scenario_dict = {
|
|
"name": "TestConstantTurn",
|
|
"targets": [
|
|
{
|
|
"target_id": 1,
|
|
"active": True,
|
|
"traceable": True,
|
|
"use_spline": False,
|
|
"trajectory": [
|
|
{
|
|
"maneuver_type": "Constant Turn",
|
|
"duration_s": 5.0,
|
|
"target_range_nm": 1.0,
|
|
"target_azimuth_deg": 0.0,
|
|
"target_altitude_ft": 1000.0,
|
|
"target_velocity_fps": 300.0,
|
|
"target_heading_deg": 90.0,
|
|
"turn_direction": "Right",
|
|
}
|
|
],
|
|
}
|
|
],
|
|
}
|
|
s = Scenario.from_dict(scenario_dict)
|
|
assert s.get_target(1) is not None
|
|
assert isinstance(s.get_target(1).trajectory[0], Waypoint)
|
|
assert s.get_target(1).trajectory[0].maneuver_type == ManeuverType.DYNAMIC_MANEUVER
|
|
|
|
|
|
def test_scenario_from_dict_invalid_target_data(capfd):
|
|
# Copre le righe 407-408
|
|
from target_simulator.core.models import Scenario
|
|
|
|
scenario_dict = {
|
|
"name": "TestInvalid",
|
|
"targets": [
|
|
{"target_id": 1, "trajectory": ["not_a_dict"]},
|
|
],
|
|
}
|
|
s = Scenario.from_dict(scenario_dict)
|
|
out, err = capfd.readouterr()
|
|
assert "Skipping invalid target data" in out
|
|
from target_simulator.core.models import Scenario
|
|
|
|
trajectory = [
|
|
Waypoint(
|
|
maneuver_type=ManeuverType.FLY_TO_POINT,
|
|
target_range_nm=1.0,
|
|
target_azimuth_deg=0.0,
|
|
),
|
|
]
|
|
t = Target(target_id=1, trajectory=trajectory)
|
|
s = Scenario(name="Scenario1")
|
|
s.add_target(t)
|
|
d = s.to_dict()
|
|
assert d["name"] == "Scenario1"
|
|
assert isinstance(d["targets"], list)
|
|
loaded = Scenario.from_dict(d)
|
|
assert loaded.name == "Scenario1"
|
|
assert loaded.get_target(1) is not None
|