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.
Chameleon/integration/ptz_predictive_zoom/zoom_controller.py

794 lines
37 KiB
Python

This file contains ambiguous Unicode characters!

This file contains ambiguous Unicode characters that may be confused with others in your current locale. If your use case is intentional and legitimate, you can safely ignore this warning. Use the Escape button to highlight these characters.

"""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
initial_gate_relax_factor: float = 1.45
tele_edge_margin_ratio: float = 0.32
min_allowed_hfov_deg: float = 10.0
latency_horizon_ms: float = 245.0
tele_pan_native10_deg_s: float = 0.65
tele_tilt_native10_deg_s: float = 0.85
tele_position_gain: float = 2.7
tele_rate_gain: float = 0.85
tele_native_max: float = 35.0
tele_steering_start_fov: float = 20.0
tele_full_steering_fov: float = 16.0
step_back_cmd: float = 0.10
tele_calibration_enabled: bool = True
calibration_settle_s: float = .35
calibration_timeout_s: float = 1.2
calibration_stable_deg: float = .10
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 name == "tele_calibration_enabled" and isinstance(value, bool):
continue
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 1 <= self.initial_gate_relax_factor <= 1.8
and self.edge_margin <= self.tele_edge_margin_ratio <= .38
and self.tele_fov <= self.min_allowed_hfov_deg <= self.tele_pulse_fov
and self.wide_pulse_fov > 12
and 40 <= self.latency_horizon_ms <= 400
and 0 < self.tele_full_steering_fov < self.tele_steering_start_fov <= self.wide_pulse_fov
and self.tele_native_max <= 45 and self.tele_rate_gain <= 1
and self.step_back_cmd <= .12
and isinstance(self.tele_calibration_enabled, bool)
and 0.25 <= self.calibration_settle_s < self.calibration_timeout_s <= 2
and self.calibration_stable_deg <= .25
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 margin_for(self, fov):
weight = min(1.0, max(0.0, (self.wide_pulse_fov - fov) / (self.wide_pulse_fov - 12.0)))
return self.edge_margin + weight * (self.tele_edge_margin_ratio - self.edge_margin)
def limits_window(self, fov, seconds):
next_focal = _log_focal(fov) + self.upper_log_rate * seconds
return next_focal <= _log_focal(self.min_allowed_hfov_deg)
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
rearm_ready: bool = True
@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
step_back: bool = False
gate: str = "tracking_zoom"
edge_margin_required: 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
self.latest_pose = None
self.calibration = 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 calibration_hold(self, now):
item = self.calibration
return bool(self.cfg.tele_calibration_enabled and item and item.get("stop_ack_at") is not None
and item.get("HFOV_before") and item["HFOV_before"] < 15.0
and 0 <= now - item["stop_ack_at"] <= self.cfg.calibration_timeout_s)
def tele_aim(self, frame, now, camera_rates, optical_rates=(0.0, 0.0)):
# Angular position predicts the image at actuation. The rate term
# reconstructs target motion from residual plus measured gimbal motion.
sample = self.latest_pose
cfg = self.cfg
if (sample is None or sample[0] != frame.seq or not self.optics.fresh(now)
or self.optics.actual >= cfg.tele_steering_start_fov
or not all(math.isfinite(rate) for rate in camera_rates)):
return None
_, angles, rates, reliable = sample
if not reliable:
return None
horizon = max(cfg.latency_horizon_ms / 1000, now - frame.at)
blend = min(1.0, (cfg.tele_steering_start_fov - self.optics.actual) /
(cfg.tele_steering_start_fov - cfg.tele_full_steering_fov))
command = []
for angle, residual, gimbal, optical, reference in zip(
angles, rates, camera_rates, optical_rates,
(cfg.tele_pan_native10_deg_s, cfg.tele_tilt_native10_deg_s)):
predicted = angle + residual * horizon
target_rate = gimbal + residual
demand = cfg.tele_position_gain * predicted + cfg.tele_rate_gain * target_rate + optical
native = min(cfg.tele_native_max, 10 * math.sqrt(abs(demand) / reference))
command.append(math.copysign(native / 100, demand))
return tuple(command), blend
def decide(self, frame, now, *, first, cooldown, transport_clear):
cfg = self.cfg
gate = "initial" if first else "tracking_zoom"
self.latest_pose = None
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, gate=gate)
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, gate=gate)
if not self.optics.fresh(now):
return ZoomDecision("fov_cooldown", stop=moving, gate=gate)
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, gate=gate)
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)
residual_ok = max(map(abs, frame.rates)) <= cfg.max_residual_deg_s
self.latest_pose = (frame.seq, frame.angles, frame.rates,
residual_ok and len(self.history) >= 2)
if not residual_ok:
return ZoomDecision("edge_margin_low", stop=moving, gate=gate)
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 = max(cfg.latency_horizon_ms / 1000, now - frame.at) + remaining
broad_initial = first and fov > cfg.entry_fov and len(self.history) >= cfg.tracker_frames
# 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)]
bounds = []
for off, size, move, rate in zip(frame.offset, frame.bbox_frac, motion, frame.rates):
# A converging trajectory may reduce the projected excursion at wide
# entry. Preserve the current offset as a bound against early braking.
extent = (max(abs(off), abs(off + math.copysign(move, rate)))
if broad_initial and off * rate < 0 else abs(off) + move)
bounds.append(extent + size / 2)
margin = min(0.5 - extent * growth for extent in bounds)
required = cfg.margin_for(min_fov)
observed_margin = min(.5 - abs(off) - size / 2 for off,size in zip(frame.offset,frame.bbox_frac))
detector_fresh = (0 <= frame.at - frame.detector_at <= cfg.detector_age_s
and 0 <= now - frame.detector_at <= cfg.detector_age_s)
steering_horizon = max(cfg.latency_horizon_ms / 1000, now - frame.at)
forecast_margin = min(
.5 - abs(math.tan(math.radians(max(-80, min(80, angle + rate * steering_horizon))))
/ (2 * axis)) - size/2
for angle, rate, axis, size in zip(frame.angles, frame.rates, half_tan, frame.bbox_frac)
)
step_back = (min_fov <= 12.0 and not moving and detector_fresh
and min(observed_margin, forecast_margin) < required + .025)
if min_fov <= cfg.min_allowed_hfov_deg or not cfg.limits_window(min_fov, remaining):
return ZoomDecision("min_hfov_limit", stop=moving, margin=margin,
step_back=step_back, gate=gate, edge_margin_required=required)
if margin < required:
return ZoomDecision("edge_margin_low", stop=moving, margin=margin,
step_back=step_back, gate=gate, edge_margin_required=required)
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 < max(cfg.edge_watch_margin, required + .04) and outward_rate > 0 and outward_accel > cfg.edge_accel:
return ZoomDecision("edge_acceleration", stop=True, margin=margin, gate=gate)
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
if broad_initial:
center_limit = cfg.entry_center
if outward < 0.0:
center_limit *= 1 + min(1.0, wide_fraction) * (cfg.initial_gate_relax_factor - 1)
outward_limit = cfg.max_outward_deg_s * (cfg.initial_gate_relax_factor if broad_initial else 1)
if moving:
if not detector_fresh:
return ZoomDecision("waiting_yolo", stop=True, margin=margin, gate=gate)
if min(frame.bbox_px) >= cfg.target_px:
return ZoomDecision("size_reached", stop=True, margin=margin, gate=gate)
if now >= self.optics.active[1]:
return ZoomDecision("fov_cooldown", stop=True, margin=margin, gate=gate)
# d(theta)/dt needed to cancel (df/f)*tan(theta) on each axis.
optical_gain = cfg.ff_gain * min(1.0, fov / cfg.tele_steering_start_fov)
ff = tuple(max(-cfg.ff_max_deg_s, min(cfg.ff_max_deg_s,
math.degrees(optical_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, gate=gate)
if min(frame.bbox_px) >= cfg.target_px:
return ZoomDecision("size_reached", margin=margin, gate=gate)
if not frame.rearm_ready:
return ZoomDecision("geometry_rearm", margin=margin, gate=gate)
if self.calibration_hold(now):
return ZoomDecision("tele_calibration_wait", margin=margin, gate=gate,
edge_margin_required=required)
if not transport_clear or cooldown or min_fov <= cfg.tele_fov + 0.05:
return ZoomDecision("fov_cooldown", margin=margin, gate=gate,
edge_margin_required=required)
if not detector_fresh:
return ZoomDecision("waiting_yolo", margin=margin, gate=gate)
if frame.tracker_score is None or len(self.history) < cfg.tracker_frames:
return ZoomDecision("low_tracker_score", margin=margin, gate=gate)
if max(map(abs, frame.offset)) > center_limit or outward > outward_limit:
return ZoomDecision("not_centered", margin=margin, gate=gate, edge_margin_required=required)
return ZoomDecision("fast_start" if first else "burst", window=window, margin=margin,
gate=gate, edge_margin_required=required)
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,
put_rtt_s=None):
controller = st.get("predictive_zoom_controller")
if controller is None or not controller.owns(st):
return
if controller.calibration and controller.calibration["request"] != request:
_close_calibration(config, camera, controller, ack_at, "next_pulse")
if controller.calibration is None:
before_at = float(st.get("zoom_step_start_status_at", 0.0) or 0.0)
before = st.get("zoom_step_start_hfov_deg")
controller.calibration = dict(
request=request, native=native, put_at=put_at, ack_at=ack_at,
put_rtt_ms=None if put_rtt_s is None else round(1000 * put_rtt_s, 2),
HFOV_before=before, before_at=before_at, stop_issued_at=None,
)
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,
duration_s=None, stop_issued_at=None, stop_rtt_s=None):
controller = st.get("predictive_zoom_controller")
if controller is None or not controller.owns(st):
return
calibration = controller.calibration
if calibration and calibration["request"] == request:
sample = next(((t, _fov(measured)) for t, _, _, measured
in reversed(controller.optics.samples)
if stop_issued_at and t <= stop_issued_at), (None, None))
calibration.update(stop_issued_at=stop_issued_at, stop_ack_at=at,
stop_rtt_ms=None if stop_rtt_s is None else round(1000 * stop_rtt_s, 2),
duration_s=duration_s, stop_reason=reason,
stop_status_at=sample[0], HFOV_stop_sample=sample[1],
clean_stop=bool(clean), first_post_stop=None, latest_post_stop=None)
if stop_issued_at is None:
_close_calibration(config, camera, controller, at, "stop_timing_missing")
if 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):
_episode_telemetry(config, camera, controller.owner, event, at, **fields)
def _episode_telemetry(config, camera, owner, event, at, **fields):
directory = getattr(config, "PREDICTIVE_ZOOM_TELEMETRY_DIR", "logs/ptz_zoom")
if not directory:
return
with _telemetry_lock:
log = _telemetry_loggers.get(directory)
if log is None:
log = EpisodeTelemetryLogger(directory)
_telemetry_loggers[directory] = log
log.submit(camera, 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, gate=decision.gate,
edge_margin_required=decision.edge_margin_required)
def transport_zoom_block_reason(st, camera_config, now, hard_s):
settings = camera_config.get("predictive_zoom")
if settings is None:
return None
controller = st.get("predictive_zoom_controller")
if controller is not None and controller.owns(st):
cfg = controller.cfg
if controller.calibration_hold(now):
return "tele_calibration_wait"
if not controller.optics.fresh(now):
return "min_hfov_limit"
_, narrowest = controller.optics.estimate(now)
if controller.optics.active:
hard_s = max(0.0, controller.optics.active[1] - now)
else:
try:
cfg = ZoomConfig(**settings)
at, hfov = st.get("predictive_zoom_hfov", (0.0, 0.0))
if not (0 <= now - at <= cfg.status_age_s and cfg.tele_fov <= hfov <= cfg.wide_fov + 1):
return "min_hfov_limit"
narrowest = _fov(_log_focal(hfov) + cfg.status_error_log)
except (TypeError, ValueError):
return "min_hfov_limit"
if narrowest is None or not cfg.limits_window(narrowest, hard_s + cfg.stop_ack_slack_s):
return "min_hfov_limit"
return None
def _close_calibration(config, camera, controller, now, outcome):
item = controller.calibration
if item is None:
return
controller.calibration = None
after = item.pop("latest_post_stop", None)
first = item.pop("first_post_stop", None)
before, stop_sample = item["HFOV_before"], item.get("HFOV_stop_sample")
after_fov = after[1] if after else None
# A request-bound measurement always has independent device timestamps.
# HTTP acknowledgement establishes ordering, not physical lens motion.
duration = item.get("duration_s")
qualified = (outcome == "settled" and item["native"] == 10 and before is not None
and before < 15 and after_fov is not None and after_fov < before
and item.get("clean_stop") and duration and duration > 0
and 0 <= item["put_at"] - item["before_at"] <= controller.cfg.status_age_s)
rate = (_log_focal(after_fov) - _log_focal(before)) / duration if qualified else None
_telemetry(config, camera, controller, "native10_pulse_calibration", now, **item,
outcome=outcome, HFOV_after=after_fov, after_at=after[0] if after else None,
delta_hfov_deg=None if before is None or after_fov is None else after_fov - before,
first_post_stop_at=first[0] if first else None,
HFOV_first_post_stop=first[1] if first else None,
post_stop_tail_deg=first[1] - after_fov if first and after else None,
stop_bracket_narrowing_deg=stop_sample - after_fov if stop_sample and after else None,
stop_sample_age_ms=None if not item.get("stop_status_at") else
1000 * (item["stop_issued_at"] - item["stop_status_at"]),
effective_tele_log_rate_s=rate)
def expire_calibration(config, camera, st, now, *, final=False):
controller = st.get("predictive_zoom_controller")
item = controller.calibration if controller else None
if item is None:
return
if final or (item.get("stop_ack_at") and now-item["stop_ack_at"] > controller.cfg.calibration_timeout_s):
_close_calibration(config, camera, controller, now, "episode_ended" if final else "settle_timeout")
def observe_calibration(config, camera, st, at, hfov, now):
controller = st.get("predictive_zoom_controller")
if controller is None or controller.calibration is None:
return
if not controller.owns(st):
_close_calibration(config, camera, controller, now, "owner_changed")
return
item = controller.calibration
ack = item.get("stop_ack_at")
if ack is None or at < ack:
return
if (st.get("zoom_step_pending") or st.get("lens_move_stop_required")
or st.get("lens_move_put_inflight") and (
st.get("lens_move_put_kind") == "goto"
or float(st.get("lens_move_put_zoom", 0.) or 0.) != 0
)
or abs(float(st.get("last_ptz_physical_cmd", (0., 0., 0.))[2])) > 0):
_close_calibration(config, camera, controller, now, "intervening_lens_motion")
return
if item["first_post_stop"] is None:
item["first_post_stop"] = (at, hfov)
previous = item["latest_post_stop"]
item["latest_post_stop"] = (at, hfov)
if at - ack > controller.cfg.calibration_timeout_s:
_close_calibration(config, camera, controller, now, "unsettled")
elif (previous and at - ack >= controller.cfg.calibration_settle_s
and abs(previous[1] - hfov) <= controller.cfg.calibration_stable_deg):
_close_calibration(config, camera, controller, now, "settled")
def record_zoom_blocks(config, camera, st, at, blocked):
if not getattr(config, "PREDICTIVE_ZOOM_TELEMETRY_DIR", "logs/ptz_zoom"):
return
owner = _owner(st)
record = st.get("episode_zoom_blocks")
if record is None or record["owner"] != owner:
if record is not None:
flush_zoom_blocks(config, camera, st, at, final=True)
record = {"owner": owner, "counts": {}, "last_values": {}, "last_write": at - 1}
st["episode_zoom_blocks"] = record
for reason, values in blocked.items():
record["counts"][reason] = record["counts"].get(reason, 0) + 1
record["last_values"][reason] = values
if at - record["last_write"] >= 1.0:
flush_zoom_blocks(config, camera, st, at)
def flush_zoom_blocks(config, camera, st, at, *, final=False):
record = st.get("episode_zoom_blocks")
if record is None:
return
_episode_telemetry(config, camera, record["owner"], "zoom_block_summary", at,
final=final, zoom_block_reason=dict(record["counts"]),
last_values=dict(record["last_values"]))
record["last_write"] = at
if final:
st.pop("episode_zoom_blocks", None)