You cannot select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
173 lines
7.9 KiB
Python
173 lines
7.9 KiB
Python
import math
|
|
from pathlib import Path
|
|
import tempfile
|
|
import unittest
|
|
|
|
from zoom_controller import EpisodeTelemetryLogger, OpticalStateEstimator, ZoomConfig, ZoomController, ZoomFrame
|
|
|
|
|
|
def frame(at, seq, *, offset=(0.13, -0.03), pixels=(20.0, 16.0),
|
|
detector_at=None, score=0.8, geometry=True):
|
|
return ZoomFrame(
|
|
at=at, seq=seq, offset=offset, bbox_frac=(pixels[0] / 1280, pixels[1] / 720),
|
|
bbox_px=pixels, angles=(0.0, 0.0), rates=(0.0, 0.0), aspect=720 / 1280,
|
|
tracker_score=score, detector_at=at if detector_at is None else detector_at,
|
|
detector_conf=0.71, geometry_ok=geometry, los_valid=True,
|
|
)
|
|
|
|
|
|
def tick(controller, sample, now=None, **kw):
|
|
flags = {"first": controller.first_ack_at is None, "cooldown": False, "transport_clear": True}
|
|
flags.update(kw)
|
|
return controller.decide(sample, sample.at if now is None else now, **flags)
|
|
|
|
|
|
class PredictiveZoomTests(unittest.TestCase):
|
|
def setUp(self):
|
|
self.cfg = ZoomConfig(nominal_gain=0.65, upper_gain=1.0)
|
|
self.c = ZoomController(self.cfg, (4, 100.0))
|
|
self.c.optics.observe(99.99, 59.0, 100.0)
|
|
|
|
def test_short_3_6_second_flight_with_delayed_yolo_and_status(self):
|
|
c = self.c
|
|
focal = math.tan(math.radians(59 / 2))
|
|
duration = 3.6
|
|
active = None
|
|
pulses, stops = [], []
|
|
actual_status = 99.99
|
|
between_without_poll = 0
|
|
last_stop = None
|
|
visible = 0
|
|
for k in range(181):
|
|
t = k * 0.02
|
|
now = 100.0 + t
|
|
if active is not None:
|
|
focal *= math.exp(-self.cfg.log_rate * 0.02)
|
|
actual = math.degrees(2 * math.atan(focal))
|
|
# One status response every 250 ms, no forced request after STOP.
|
|
if k % 13 == 8:
|
|
c.optics.observe(now, actual, now)
|
|
actual_status = now
|
|
if active and now >= active[1]:
|
|
pulse, _, started, status_at_start = active
|
|
confirmed = actual_status > status_at_start
|
|
c.stopped(pulse, now, confirmed=confirmed, clean=True)
|
|
stops.append(now)
|
|
active = None
|
|
last_stop = now
|
|
if k % 4 != 0:
|
|
continue
|
|
# Same aircraft, steady closing LOS and native-10 synthetic plant.
|
|
x = 0.129 - min(0.069, t * 0.12)
|
|
optical_scale = math.tan(math.radians(59 / 2)) / focal
|
|
px = (20 * optical_scale, 16 * optical_scale)
|
|
self.assertLess(abs(x) + px[0] / 2560, 0.5)
|
|
visible += 1
|
|
detector = 100.0 + math.floor((t + 1e-9) / 0.52) * 0.52
|
|
can_repeat = last_stop is None or c.can_reuse_feedback(now, last_stop) or actual_status > last_stop
|
|
decision = tick(c, frame(now, k + 1, offset=(x, -0.025), pixels=px,
|
|
detector_at=detector),
|
|
transport_clear=active is None and can_repeat)
|
|
if decision.window:
|
|
if last_stop is not None and actual_status <= last_stop:
|
|
between_without_poll += 1
|
|
self.assertIsNone(active)
|
|
pulse = len(pulses) + 1
|
|
nominal, hard = decision.window
|
|
c.accepted(pulse, now, now, now + hard)
|
|
active = pulse, now + nominal, now, actual_status
|
|
pulses.append(t)
|
|
else:
|
|
if decision.stop:
|
|
self.assertEqual(decision.reason, "size_reached")
|
|
c.stopped(active[0], now, clean=False, confirmed=False)
|
|
last_stop = now
|
|
stops.append(now)
|
|
active = None
|
|
self.assertEqual(visible, 46)
|
|
self.assertLess(pulses[0], 0.25)
|
|
self.assertGreaterEqual(len(pulses), 4)
|
|
self.assertGreaterEqual(between_without_poll, 2)
|
|
self.assertGreaterEqual(len(stops), 3)
|
|
self.assertLess(actual, 42.0)
|
|
|
|
def test_wide_entry_without_next_detector_and_narrow_entry_is_stricter(self):
|
|
for k in range(3):
|
|
self.assertEqual(tick(self.c, frame(100 + .08 * k, k + 1, detector_at=99.96)).reason,
|
|
"fast_start" if k == 2 else "low_tracker_score")
|
|
other = ZoomController(self.cfg, (4, 100))
|
|
other.optics.observe(99.99, 28.0, 100.0)
|
|
for k in range(3):
|
|
d = tick(other, frame(100 + .08 * k, k + 1, detector_at=99.96))
|
|
self.assertEqual(d.reason, "not_centered")
|
|
|
|
def test_reject_outward_low_score_and_expired_detector(self):
|
|
for k in range(3):
|
|
d = tick(self.c, frame(100 + .08 * k, k + 1, offset=(.075 + .003 * k, -.02)))
|
|
self.assertEqual(d.reason, "not_centered")
|
|
self.assertEqual(tick(self.c, frame(100.20, 4, score=.2)).reason, "low_tracker_score")
|
|
self.assertEqual(tick(self.c, frame(100.28, 5, detector_at=99.0)).reason, "waiting_yolo")
|
|
|
|
def test_positive_ff_and_edge_stop_preserve_request_deadline(self):
|
|
self.c.accepted(4, 100.0, 100.01, 100.4)
|
|
active = self.c.optics.active
|
|
d = tick(self.c, frame(100.05, 1, offset=(.08, -.05)))
|
|
self.assertGreater(d.ff_deg_s[0], 0)
|
|
self.assertLess(d.ff_deg_s[1], 0)
|
|
d = tick(self.c, frame(100.12, 2, offset=(.32, -.05)))
|
|
self.assertTrue(d.stop)
|
|
self.assertEqual(d.reason, "edge_margin_low")
|
|
self.assertEqual(d.ff_deg_s, (0.0, 0.0))
|
|
self.assertEqual(self.c.optics.active, active)
|
|
|
|
def test_edge_acceleration_can_stop_before_margin_crosses(self):
|
|
c = ZoomController(ZoomConfig(.28, .45, edge_accel=.1, edge_watch_margin=.35), (4, 100))
|
|
c.optics.observe(99.99, 59, 100)
|
|
c.accepted(2, 100, 100.01, 100.4)
|
|
for seq, off in enumerate((.145, .145, .153), 1):
|
|
decision = tick(c, frame(100 + seq * .06, seq, offset=(off, -.02)))
|
|
self.assertGreater(decision.margin, c.cfg.edge_margin)
|
|
self.assertEqual(decision.reason, "edge_acceleration")
|
|
self.assertTrue(decision.stop)
|
|
|
|
def test_causal_status_and_failed_or_ambiguous_stop_cannot_rearm(self):
|
|
c = self.c
|
|
c.accepted(4, 100.0, 100.01, 100.4)
|
|
before = c.optics.estimate(100.1)
|
|
c.optics.observe(100.18, 55, 100.2)
|
|
c.optics.observe(100.08, 57, 100.21)
|
|
self.assertEqual(c.optics.estimate(100.1), before)
|
|
self.assertEqual(c.optics.status_at, 100.18)
|
|
self.assertFalse(c.can_reuse_feedback(100.35, 100.34))
|
|
c.stopped(4, 100.34, confirmed=True, clean=False)
|
|
self.assertFalse(c.can_reuse_feedback(100.35, 100.34))
|
|
|
|
def test_fov_curves_and_watchdog_bounds(self):
|
|
self.assertEqual(self.cfg.window(59), (.34, .40))
|
|
self.assertEqual(self.cfg.window(10), (.12, .18))
|
|
self.assertGreater(self.cfg.window(21)[0], self.cfg.window(13)[0])
|
|
for invalid in (dict(wide_pulse_s=.5), dict(native=20), dict(frame_age_s=.26),
|
|
dict(upper_gain=.4), dict(nominal_gain=float("nan"))):
|
|
args = {"nominal_gain": .65, "upper_gain": 1.0}
|
|
args.update(invalid)
|
|
with self.assertRaises(ValueError):
|
|
ZoomConfig(**args)
|
|
|
|
def test_telemetry_writes_first_and_ticks_separately(self):
|
|
import json
|
|
with tempfile.TemporaryDirectory() as directory:
|
|
logger = EpisodeTelemetryLogger(directory)
|
|
logger.submit(8, (4, 100.0), "first_zoom", 100.2, t_to_first_zoom_ms=200)
|
|
logger.submit(8, (4, 100.0), "tick", 100.25, FOV_actual=59.0,
|
|
FOV_estimated=57.0, bbox_px=[20, 16], detector_conf=.71)
|
|
logger.queue.join()
|
|
path, = Path(directory).glob("*.jsonl")
|
|
first, second = map(json.loads, path.read_text().splitlines())
|
|
self.assertEqual(first["t_to_first_zoom_ms"], 200)
|
|
self.assertEqual(second["elapsed_ms"], 250)
|
|
self.assertEqual(second["FOV_estimated"], 57)
|
|
|
|
|
|
if __name__ == "__main__":
|
|
unittest.main()
|