|
|
"""Predictive TRACK zoom policy. All time inputs are seconds on the same clock."""
|
|
|
|
|
|
from collections import deque
|
|
|
from dataclasses import dataclass, replace
|
|
|
import json
|
|
|
import math
|
|
|
from pathlib import Path
|
|
|
from queue import Full, Queue
|
|
|
from threading import Lock, Thread
|
|
|
|
|
|
|
|
|
@dataclass(frozen=True)
|
|
|
class ZoomConfig:
|
|
|
# Fit per camera and native command. Gains multiply ln(252/6)/full_travel_s.
|
|
|
nominal_gain: float
|
|
|
upper_gain: float
|
|
|
wide_fov: float = 59.0
|
|
|
tele_fov: float = 2.0
|
|
|
full_travel_s: float = 5.5
|
|
|
native: int = 10
|
|
|
entry_fov: float = 35.0
|
|
|
entry_center: float = 0.15 # fraction of full frame, 30% of half-width
|
|
|
repeat_center: float = 0.09
|
|
|
edge_margin: float = 0.16
|
|
|
tracker_score: float = 0.55
|
|
|
tracker_frames: int = 3
|
|
|
frame_age_s: float = 0.25
|
|
|
detector_age_s: float = 0.65
|
|
|
status_age_s: float = 0.65
|
|
|
evidence_gap_s: float = 0.20
|
|
|
max_outward_deg_s: float = 0.15
|
|
|
trajectory_slack_deg: float = 0.6
|
|
|
edge_accel: float = 0.55 # full frame / sВІ, outward
|
|
|
edge_watch_margin: float = 0.24
|
|
|
steering_delay_s: float = 0.18
|
|
|
max_residual_deg_s: float = 12.0
|
|
|
wide_pulse_fov: float = 25.0
|
|
|
tele_pulse_fov: float = 15.0
|
|
|
wide_pulse_s: float = 0.34
|
|
|
tele_pulse_s: float = 0.12
|
|
|
stop_slack_s: float = 0.06
|
|
|
stop_ack_slack_s: float = 0.08
|
|
|
target_px: float = 48.0
|
|
|
status_gain: float = 0.7
|
|
|
status_error_log: float = 0.08
|
|
|
ff_gain: float = 0.7
|
|
|
ff_max_deg_s: float = 3.0
|
|
|
|
|
|
def __post_init__(self):
|
|
|
for name, value in vars(self).items():
|
|
|
if not math.isfinite(value) or value <= 0:
|
|
|
raise ValueError(f"invalid zoom setting {name}")
|
|
|
if not (
|
|
|
self.native == 10 and 0 < self.nominal_gain <= self.upper_gain
|
|
|
and 0 < self.tele_fov < self.tele_pulse_fov < self.wide_pulse_fov
|
|
|
< self.entry_fov < self.wide_fov < 90
|
|
|
and 0 < self.repeat_center <= self.entry_center <= 0.15
|
|
|
and 0 < self.edge_margin <= self.edge_watch_margin < 0.5
|
|
|
and isinstance(self.tracker_frames, int) and 2 <= self.tracker_frames <= 12
|
|
|
and self.tracker_score <= 1 and self.status_gain <= 1
|
|
|
and self.frame_age_s <= 0.25 and self.detector_age_s <= 0.65
|
|
|
and self.status_age_s <= 0.65
|
|
|
and 0.10 <= self.tele_pulse_s <= self.wide_pulse_s <= 0.34
|
|
|
and self.wide_pulse_s + self.stop_slack_s <= 0.40 + 1e-9
|
|
|
and self.stop_ack_slack_s <= 0.12
|
|
|
):
|
|
|
raise ValueError("zoom settings exceed the native/geometry/watchdog limits")
|
|
|
|
|
|
@property
|
|
|
def log_rate(self):
|
|
|
return self.nominal_gain * math.log(252 / 6) / self.full_travel_s
|
|
|
|
|
|
@property
|
|
|
def upper_log_rate(self):
|
|
|
return self.upper_gain * math.log(252 / 6) / self.full_travel_s
|
|
|
|
|
|
def window(self, fov):
|
|
|
scale = min(1.0, max(0.0, (fov - self.tele_pulse_fov)
|
|
|
/ (self.wide_pulse_fov - self.tele_pulse_fov)))
|
|
|
nominal = self.tele_pulse_s + scale * (self.wide_pulse_s - self.tele_pulse_s)
|
|
|
return nominal, min(0.40, nominal + self.stop_slack_s)
|
|
|
|
|
|
|
|
|
def _log_focal(fov):
|
|
|
return -math.log(math.tan(math.radians(fov / 2)))
|
|
|
|
|
|
|
|
|
def _fov(log_focal):
|
|
|
return math.degrees(2 * math.atan(math.exp(-log_focal)))
|
|
|
|
|
|
|
|
|
class OpticalStateEstimator:
|
|
|
"""Integrate accepted native-10 windows, then correct with timestamped status."""
|
|
|
|
|
|
def __init__(self, config):
|
|
|
self.cfg = config
|
|
|
self.actual = None
|
|
|
self.status_at = 0.0
|
|
|
self.nominal = self.upper = 0.0
|
|
|
self.spans = deque()
|
|
|
self.samples = deque()
|
|
|
self.active = None
|
|
|
self.valid = False
|
|
|
|
|
|
def _seconds_since_status(self, at, status_at=None):
|
|
|
status_at = self.status_at if status_at is None else status_at
|
|
|
spans = self.spans
|
|
|
if self.active:
|
|
|
start, hard_end = self.active
|
|
|
spans = (*spans, (start, hard_end))
|
|
|
return sum(max(0.0, min(end, at) - max(start, status_at))
|
|
|
for start, end in spans)
|
|
|
|
|
|
def estimate(self, at):
|
|
|
for status_at, nominal, upper, _ in reversed(self.samples):
|
|
|
if status_at <= at:
|
|
|
elapsed = self._seconds_since_status(at, status_at)
|
|
|
tele = _log_focal(self.cfg.tele_fov)
|
|
|
return (_fov(min(tele, nominal + self.cfg.log_rate * elapsed)),
|
|
|
_fov(min(tele, upper + self.cfg.upper_log_rate * elapsed)))
|
|
|
return None, None
|
|
|
|
|
|
def observe(self, at, hfov, now):
|
|
|
if not (self.status_at < at <= now and math.isfinite(hfov)
|
|
|
and self.cfg.tele_fov - 0.1 <= hfov <= self.cfg.wide_fov + 1.0):
|
|
|
return
|
|
|
measured = _log_focal(hfov)
|
|
|
estimated, upper = self.estimate(at)
|
|
|
had_motion = self._seconds_since_status(at) > 0.0
|
|
|
discrepancy = (had_motion and estimated is not None
|
|
|
and (measured > _log_focal(upper) + self.cfg.status_error_log
|
|
|
or measured < _log_focal(self.actual) - self.cfg.status_error_log))
|
|
|
if discrepancy:
|
|
|
self.valid = False
|
|
|
elif not self.active:
|
|
|
self.valid = True
|
|
|
if not had_motion or estimated is None or discrepancy:
|
|
|
self.nominal = measured
|
|
|
else:
|
|
|
prediction = _log_focal(estimated)
|
|
|
self.nominal = prediction + self.cfg.status_gain * (measured - prediction)
|
|
|
# The filtered estimate never reduces the conservative edge envelope.
|
|
|
self.upper = max(self.nominal, measured + self.cfg.status_error_log)
|
|
|
self.actual, self.status_at = hfov, at
|
|
|
self.samples.append((at, self.nominal, self.upper, measured))
|
|
|
cutoff = at - self.cfg.status_age_s - self.cfg.frame_age_s
|
|
|
while len(self.samples) > 1 and self.samples[1][0] <= cutoff:
|
|
|
self.samples.popleft()
|
|
|
while self.spans and self.spans[0][1] <= self.samples[0][0]:
|
|
|
self.spans.popleft()
|
|
|
|
|
|
def start(self, put_at, ack_at, hard_end):
|
|
|
if self.active or not self.fresh(ack_at) or not put_at <= ack_at < hard_end:
|
|
|
self.valid = False
|
|
|
self.active = (put_at, hard_end)
|
|
|
|
|
|
def stop(self, at, *, clean):
|
|
|
if self.active:
|
|
|
start, hard_end = self.active
|
|
|
if at > hard_end + self.cfg.stop_ack_slack_s or at < start or not clean:
|
|
|
self.valid = False
|
|
|
if at > start:
|
|
|
self.spans.append((start, min(at, hard_end + self.cfg.stop_ack_slack_s)))
|
|
|
self.active = None
|
|
|
|
|
|
def fresh(self, now):
|
|
|
return self.valid and 0 <= now - self.status_at <= self.cfg.status_age_s
|
|
|
|
|
|
|
|
|
@dataclass(frozen=True)
|
|
|
class ZoomFrame:
|
|
|
at: float
|
|
|
seq: int
|
|
|
offset: tuple[float, float] # positive right/down; fractions of full frame
|
|
|
bbox_frac: tuple[float, float]
|
|
|
bbox_px: tuple[float, float]
|
|
|
angles: tuple[float, float] # camera-signed LOS error in degrees
|
|
|
rates: tuple[float, float] # camera-signed LOS residual, deg/s
|
|
|
aspect: float # image height / width
|
|
|
tracker_score: float | None
|
|
|
detector_at: float
|
|
|
detector_conf: float | None
|
|
|
geometry_ok: bool
|
|
|
los_valid: bool
|
|
|
|
|
|
|
|
|
@dataclass(frozen=True)
|
|
|
class ZoomDecision:
|
|
|
reason: str
|
|
|
window: tuple[float, float] | None = None
|
|
|
stop: bool = False
|
|
|
ff_deg_s: tuple[float, float] = (0.0, 0.0)
|
|
|
margin: float | None = None
|
|
|
|
|
|
|
|
|
class ZoomController:
|
|
|
"""Per TRACK epoch; the existing worker retains ownership and all STOPs."""
|
|
|
|
|
|
def __init__(self, config, owner):
|
|
|
self.cfg = config
|
|
|
self.owner = owner
|
|
|
self.optics = OpticalStateEstimator(config)
|
|
|
self.history = deque(maxlen=config.tracker_frames)
|
|
|
self.request = None
|
|
|
self.first_ack_at = None
|
|
|
self.confirmed_stop = None
|
|
|
self.completed_request = None
|
|
|
self.stop_cooldowns = None
|
|
|
|
|
|
def accepted(self, request, put_at, ack_at, hard_end):
|
|
|
if self.request == request:
|
|
|
return
|
|
|
self.request = request
|
|
|
self.confirmed_stop = None
|
|
|
self.completed_request = None
|
|
|
self.stop_cooldowns = None
|
|
|
if self.first_ack_at is None:
|
|
|
self.first_ack_at = ack_at
|
|
|
self.optics.start(put_at, ack_at, hard_end)
|
|
|
|
|
|
def stopped(self, request, at, *, confirmed, clean):
|
|
|
if self.request != request:
|
|
|
return
|
|
|
self.optics.stop(at, clean=clean)
|
|
|
self.request = None
|
|
|
if clean and self.optics.fresh(at):
|
|
|
self.completed_request = request
|
|
|
if confirmed:
|
|
|
self.confirmed_stop = (at, self.optics.status_at)
|
|
|
|
|
|
def optical_confirmation(self, request, at):
|
|
|
if self.completed_request == request and self.request is None and self.optics.fresh(at):
|
|
|
self.confirmed_stop = (at, self.optics.status_at)
|
|
|
|
|
|
def can_reuse_feedback(self, now, feedback_after):
|
|
|
return (self.confirmed_stop is not None and self.request is None
|
|
|
and self.confirmed_stop[0] == feedback_after and self.optics.fresh(now)
|
|
|
and self.optics.status_at >= self.confirmed_stop[1])
|
|
|
|
|
|
def owns(self, st):
|
|
|
return (st.get("predictive_zoom_controller") is self and _owner(st) == self.owner
|
|
|
and st.get("mode") == "TRACK" and st.get("tracking_active"))
|
|
|
|
|
|
def repeat_after_stop(self, st, now):
|
|
|
return bool(
|
|
|
self.owns(st) and self.can_reuse_feedback(
|
|
|
now, float(st.get("zoom_feedback_required_after", 0.0) or 0.0)
|
|
|
)
|
|
|
and st.get("zoom_step_last_delivery_result") == "confirmed"
|
|
|
and not any(st.get(key) for key in (
|
|
|
"zoom_step_feedback_pending", "zoom_step_pending",
|
|
|
"zoom_stop_obligation_active", "zoom_no_effect_blocked",
|
|
|
"zoom_step_cancel_requested", "zoom_rearm_required", "lens_move_stop_required",
|
|
|
"lens_move_put_inflight",
|
|
|
))
|
|
|
and self.stop_cooldowns == _cooldowns(st)
|
|
|
and tuple(st.get("last_ptz_physical_cmd", (None, None, None)))[2] == 0
|
|
|
)
|
|
|
|
|
|
def _record(self, frame, fresh):
|
|
|
history = self.history
|
|
|
if (not fresh or frame.tracker_score is None
|
|
|
or frame.tracker_score < self.cfg.tracker_score):
|
|
|
history.clear()
|
|
|
return
|
|
|
if history:
|
|
|
prev = history[-1]
|
|
|
if frame.seq <= prev.seq or frame.at <= prev.at:
|
|
|
history.clear()
|
|
|
return
|
|
|
dt = frame.at - prev.at
|
|
|
if (dt > self.cfg.evidence_gap_s
|
|
|
or any(abs(b - a - rate * dt) > self.cfg.trajectory_slack_deg
|
|
|
for a, b, rate in zip(prev.angles, frame.angles, prev.rates))):
|
|
|
history.clear()
|
|
|
history.append(frame)
|
|
|
|
|
|
def decide(self, frame, now, *, first, cooldown, transport_clear):
|
|
|
cfg = self.cfg
|
|
|
values = (*frame.offset, *frame.bbox_frac, *frame.bbox_px,
|
|
|
*frame.angles, *frame.rates, frame.at, frame.aspect)
|
|
|
fresh = (frame.geometry_ok and frame.los_valid and frame.seq > 0
|
|
|
and all(math.isfinite(x) for x in values)
|
|
|
and 0 < frame.aspect <= 4 and min(frame.bbox_frac + frame.bbox_px) > 0
|
|
|
and 0 <= now - frame.at <= cfg.frame_age_s)
|
|
|
prev = self.history[-1] if self.history else None
|
|
|
moving = self.request is not None
|
|
|
if frame.tracker_score is not None and not (
|
|
|
math.isfinite(frame.tracker_score) and frame.tracker_score >= cfg.tracker_score
|
|
|
):
|
|
|
self.history.clear()
|
|
|
return ZoomDecision("low_tracker_score", stop=moving)
|
|
|
if not fresh or (prev is not None and (frame.at <= prev.at or frame.seq <= prev.seq)):
|
|
|
self.history.clear()
|
|
|
return ZoomDecision("stale_or_geometry", stop=moving)
|
|
|
if not self.optics.fresh(now):
|
|
|
return ZoomDecision("fov_cooldown", stop=moving)
|
|
|
|
|
|
fov, min_fov = self.optics.estimate(now)
|
|
|
captured_fov, captured_min = self.optics.estimate(frame.at)
|
|
|
if captured_fov is None:
|
|
|
return ZoomDecision("fov_cooldown", stop=moving)
|
|
|
captured_base = next(measured for at, _, _, measured in reversed(self.optics.samples)
|
|
|
if at <= frame.at)
|
|
|
def projection(hfov):
|
|
|
horizontal = math.tan(math.radians(hfov / 2))
|
|
|
return horizontal, horizontal * frame.aspect
|
|
|
|
|
|
axes_at_capture = projection(captured_fov)
|
|
|
angles = tuple(math.degrees(math.atan(2 * offset * axis))
|
|
|
for offset, axis in zip(frame.offset, axes_at_capture))
|
|
|
if prev and 0 < frame.at - prev.at <= cfg.evidence_gap_s:
|
|
|
dt = frame.at - prev.at
|
|
|
rates = tuple((value - old) / dt for value, old in zip(angles, prev.angles))
|
|
|
frame = replace(frame, rates=rates)
|
|
|
frame = replace(frame, angles=angles)
|
|
|
self._record(frame, fresh)
|
|
|
if max(map(abs, frame.rates)) > cfg.max_residual_deg_s:
|
|
|
return ZoomDecision("edge_margin_low", stop=moving)
|
|
|
window = cfg.window(min_fov)
|
|
|
if moving:
|
|
|
remaining = max(0.0, self.optics.active[1] - now)
|
|
|
else:
|
|
|
remaining = window[1]
|
|
|
remaining += cfg.stop_ack_slack_s
|
|
|
horizon = cfg.steering_delay_s + (now - frame.at) + remaining
|
|
|
# Residual is signed in camera coordinates. Use its magnitude for the
|
|
|
# edge projection, so a sign flip or overshoot cannot shrink the bound.
|
|
|
narrowed = min(_log_focal(cfg.tele_fov),
|
|
|
_log_focal(min_fov) + cfg.upper_log_rate * remaining)
|
|
|
# Use the widest supported captured view as the reference so a
|
|
|
# narrower prediction never understates the pixel magnification.
|
|
|
growth = math.exp(max(0.0, narrowed - captured_base + cfg.status_error_log))
|
|
|
half_tan = projection(captured_min)
|
|
|
motion = [math.tan(math.radians(min(80, abs(rate) * horizon))) / (2 * axis)
|
|
|
for rate, axis in zip(frame.rates, half_tan)]
|
|
|
margin = min(0.5 - (abs(off) + size / 2 + move) * growth
|
|
|
for off, size, move in zip(frame.offset, frame.bbox_frac, motion))
|
|
|
if margin < cfg.edge_margin:
|
|
|
return ZoomDecision("edge_margin_low", stop=moving, margin=margin)
|
|
|
if moving and prev and 0 < frame.at - prev.at <= cfg.evidence_gap_s:
|
|
|
dt = frame.at - prev.at
|
|
|
for off, rate, old_rate, axis in zip(frame.angles, frame.rates, prev.rates, half_tan):
|
|
|
outward_accel = math.copysign(1, off) * (rate - old_rate) / (dt * 2 * math.degrees(axis))
|
|
|
outward_rate = math.copysign(1, off) * rate
|
|
|
if margin < cfg.edge_watch_margin and outward_rate > 0 and outward_accel > cfg.edge_accel:
|
|
|
return ZoomDecision("edge_acceleration", stop=True, margin=margin)
|
|
|
|
|
|
norm = math.hypot(*frame.angles)
|
|
|
outward = sum(a * r for a, r in zip(frame.angles, frame.rates)) / norm if norm else 0.0
|
|
|
wide_fraction = max(0.0, (fov - cfg.entry_fov) / (cfg.wide_fov - cfg.entry_fov))
|
|
|
center_limit = (cfg.repeat_center + min(1.0, wide_fraction)
|
|
|
* (cfg.entry_center - cfg.repeat_center)) if first else cfg.repeat_center
|
|
|
detector_fresh = (0 <= frame.at - frame.detector_at <= cfg.detector_age_s
|
|
|
and 0 <= now - frame.detector_at <= cfg.detector_age_s)
|
|
|
if moving:
|
|
|
if not detector_fresh:
|
|
|
return ZoomDecision("waiting_yolo", stop=True, margin=margin)
|
|
|
if min(frame.bbox_px) >= cfg.target_px:
|
|
|
return ZoomDecision("size_reached", stop=True, margin=margin)
|
|
|
if now >= self.optics.active[1]:
|
|
|
return ZoomDecision("fov_cooldown", stop=True, margin=margin)
|
|
|
# d(theta)/dt needed to cancel (df/f)*tan(theta) on each axis.
|
|
|
ff = tuple(max(-cfg.ff_max_deg_s, min(cfg.ff_max_deg_s,
|
|
|
math.degrees(cfg.ff_gain * cfg.log_rate * math.sin(math.radians(a))
|
|
|
* math.cos(math.radians(a))))) for a in frame.angles)
|
|
|
return ZoomDecision("pulse_active", ff_deg_s=ff, margin=margin)
|
|
|
if min(frame.bbox_px) >= cfg.target_px:
|
|
|
return ZoomDecision("size_reached", margin=margin)
|
|
|
if not transport_clear or cooldown or min_fov <= cfg.tele_fov + 0.05:
|
|
|
return ZoomDecision("fov_cooldown", margin=margin)
|
|
|
if not detector_fresh:
|
|
|
return ZoomDecision("waiting_yolo", margin=margin)
|
|
|
if frame.tracker_score is None or len(self.history) < cfg.tracker_frames:
|
|
|
return ZoomDecision("low_tracker_score", margin=margin)
|
|
|
if max(map(abs, frame.offset)) > center_limit or outward > cfg.max_outward_deg_s:
|
|
|
return ZoomDecision("not_centered", margin=margin)
|
|
|
return ZoomDecision("fast_start" if first else "burst", window=window, margin=margin)
|
|
|
|
|
|
|
|
|
class EpisodeTelemetryLogger:
|
|
|
"""Write JSONL off the control/STOP threads, with a bounded queue."""
|
|
|
|
|
|
def __init__(self, directory, capacity=4096):
|
|
|
self.directory = Path(directory)
|
|
|
self.queue = Queue(maxsize=capacity)
|
|
|
self.dropped = 0
|
|
|
Thread(target=self._write, name="ptz-zoom-telemetry", daemon=True).start()
|
|
|
|
|
|
def submit(self, camera, owner, event, at, **fields):
|
|
|
generation, started = owner
|
|
|
try:
|
|
|
self.queue.put_nowait((camera, generation, started, {
|
|
|
"cam": camera, "generation": generation, "track_start_utc": started,
|
|
|
"utc": at, "elapsed_ms": round((at - started) * 1000, 1),
|
|
|
"event": event, **fields,
|
|
|
}))
|
|
|
except Full:
|
|
|
self.dropped += 1
|
|
|
|
|
|
def _write(self):
|
|
|
import logging
|
|
|
log = logging.getLogger("PTZTracker")
|
|
|
while True:
|
|
|
camera, generation, started, row = self.queue.get()
|
|
|
try:
|
|
|
self.directory.mkdir(parents=True, exist_ok=True)
|
|
|
path = self.directory / f"ptz-{camera}-{generation}-{started:.6f}.jsonl"
|
|
|
with path.open("a", encoding="utf-8") as out:
|
|
|
out.write(json.dumps(row, allow_nan=False) + "\n")
|
|
|
except (OSError, ValueError):
|
|
|
log.exception("Unable to write PTZ zoom telemetry")
|
|
|
finally:
|
|
|
self.queue.task_done()
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
def _owner(st):
|
|
|
return int(st.get("ptz_generation", 0) or 0), float(st.get("track_started_at", 0.0) or 0.0)
|
|
|
|
|
|
|
|
|
def controller_for_track(st, camera_config):
|
|
|
"""Called only from TRACK, under the same lock used by status and the worker."""
|
|
|
if not st.get("tracking_active") or st.get("mode") != "TRACK" or _owner(st)[1] <= 0:
|
|
|
return None
|
|
|
current = st.get("predictive_zoom_controller")
|
|
|
if current is None or not current.owns(st):
|
|
|
try:
|
|
|
settings = ZoomConfig(**camera_config["predictive_zoom"])
|
|
|
except (TypeError, ValueError):
|
|
|
# A malformed per-camera calibration must leave legacy control available.
|
|
|
return None
|
|
|
current = ZoomController(settings, _owner(st))
|
|
|
st["predictive_zoom_controller"] = current
|
|
|
sample = st.get("predictive_zoom_hfov")
|
|
|
if sample is not None:
|
|
|
current.optics.observe(*sample, now=sample[0])
|
|
|
return current
|
|
|
|
|
|
|
|
|
def transport_accepted(config, camera, st, request, native, put_at, ack_at, hard_end):
|
|
|
controller = st.get("predictive_zoom_controller")
|
|
|
if controller is None or not controller.owns(st):
|
|
|
return
|
|
|
if controller.first_ack_at is None:
|
|
|
controller.first_ack_at = ack_at
|
|
|
_telemetry(config, camera, controller, "first_zoom", ack_at,
|
|
|
t_to_first_zoom_ms=round((ack_at - controller.owner[1]) * 1000, 1),
|
|
|
t_to_first_zoom_put_ms=round((put_at - controller.owner[1]) * 1000, 1),
|
|
|
request=request, native=native)
|
|
|
if (st.get("predictive_zoom_request") == request
|
|
|
and native == controller.cfg.native):
|
|
|
controller.accepted(request, put_at, ack_at, hard_end)
|
|
|
|
|
|
|
|
|
def _cooldowns(st):
|
|
|
return (float(st.get("zoom_step_cooldown_until", 0.0) or 0.0),
|
|
|
float(st.get("zoom_recenter_until", 0.0) or 0.0))
|
|
|
|
|
|
|
|
|
def _reusable_optics(controller, st, at):
|
|
|
before = st.get("zoom_step_start_hfov_deg")
|
|
|
actual = controller.optics.actual
|
|
|
return bool(before and actual and actual < before
|
|
|
and controller.optics.status_at == st.get("zoom_status_at")
|
|
|
and float(st.get("zoom_step_started_at", 0.0)) < controller.optics.status_at <= at)
|
|
|
|
|
|
|
|
|
def _normal_stop_cooldowns(config, st, at):
|
|
|
expected = (getattr(config, "ZOOM_STEP_COOLDOWN_SEC", 0.14),
|
|
|
getattr(config, "ZOOM_STEP_RECENTER_SEC", 0.14))
|
|
|
return all(observed <= at + delay for observed, delay in zip(_cooldowns(st), expected))
|
|
|
|
|
|
|
|
|
def transport_confirmed(config, st, request, at):
|
|
|
controller = st.get("predictive_zoom_controller")
|
|
|
if (controller is not None and controller.owns(st)
|
|
|
and _reusable_optics(controller, st, at) and _normal_stop_cooldowns(config, st, at)):
|
|
|
controller.optical_confirmation(request, at)
|
|
|
controller.stop_cooldowns = _cooldowns(st)
|
|
|
|
|
|
|
|
|
def transport_stopped(config, camera, st, request, at, *, reason, confirmed, clean):
|
|
|
controller = st.get("predictive_zoom_controller")
|
|
|
if controller is None or not controller.owns(st) or controller.request != request:
|
|
|
return
|
|
|
clean = clean and st.get("predictive_zoom_early_stop") != request
|
|
|
reusable = confirmed and _reusable_optics(controller, st, at)
|
|
|
controller.stopped(request, at, clean=clean, confirmed=reusable and _normal_stop_cooldowns(config, st, at))
|
|
|
controller.stop_cooldowns = _cooldowns(st)
|
|
|
_telemetry(config, camera, controller, "stop", at,
|
|
|
request=request, reason=reason, confirmed=confirmed, clean=clean)
|
|
|
|
|
|
|
|
|
_telemetry_loggers = {}
|
|
|
_telemetry_lock = Lock()
|
|
|
|
|
|
|
|
|
def _telemetry(config, camera, controller, event, at, **fields):
|
|
|
directory = getattr(config, "PREDICTIVE_ZOOM_TELEMETRY_DIR", None)
|
|
|
if directory is None:
|
|
|
return
|
|
|
with _telemetry_lock:
|
|
|
log = _telemetry_loggers.get(directory)
|
|
|
if log is None:
|
|
|
log = EpisodeTelemetryLogger(directory)
|
|
|
_telemetry_loggers[directory] = log
|
|
|
log.submit(camera, controller.owner, event, at, **fields, dropped_before=log.dropped)
|
|
|
|
|
|
|
|
|
def telemetry_tick(config, camera, controller, frame, decision, zoom, legacy_reason, at):
|
|
|
estimated, narrowest = controller.optics.estimate(at)
|
|
|
reason = decision.reason
|
|
|
if zoom <= 0.0 and decision.window is not None:
|
|
|
reason = legacy_reason
|
|
|
_telemetry(config, camera, controller, "tick", at, seq=frame.seq,
|
|
|
frame_utc=frame.at, zoom_cmd=zoom, reason=reason,
|
|
|
stop_requested=decision.stop, FOV_actual=controller.optics.actual,
|
|
|
FOV_status_utc=controller.optics.status_at or None,
|
|
|
FOV_estimated=estimated, FOV_min_estimated=narrowest,
|
|
|
bbox_px=frame.bbox_px, tracker_score=frame.tracker_score,
|
|
|
detector_conf=frame.detector_conf, detector_utc=frame.detector_at or None,
|
|
|
margin=decision.margin, ff_deg_s=decision.ff_deg_s)
|