Add predictive TRACK zoom for calibrated PTZ cameras

feature/ptz-predictive-zoom
Ярослав Карташов 2 weeks ago
parent 273fb58c31
commit 0733a4b95c

2
.gitattributes vendored

@ -2,3 +2,5 @@ integration/ptz_zoom_zero_decision/server.patch text eol=lf -whitespace
integration/ptz_native_floor_continuation/server.patch text eol=lf -whitespace integration/ptz_native_floor_continuation/server.patch text eol=lf -whitespace
integration/ptz_same_native_diagnostics/server.patch text eol=lf -whitespace integration/ptz_same_native_diagnostics/server.patch text eol=lf -whitespace
integration/ptz_zoom_threshold_diagnostics/server.patch text eol=lf -whitespace integration/ptz_zoom_threshold_diagnostics/server.patch text eol=lf -whitespace
integration/ptz_predictive_zoom/server.patch text eol=lf -whitespace
integration/ptz_predictive_zoom/*.py text eol=lf

@ -0,0 +1,23 @@
# Предсказательный зум Chameleon-C
Кандидат поверх установленного PR №7. Точки интеграции находятся в TRACK, обработчике ISAPI-статуса и обработчике подтверждённых команд транспорта. Контрольные суммы исходных и изменённых файлов приведены в [pins.json](pins.json). [server.patch](server.patch) применяется к точной серверной копии трёх файлов, а [zoom_controller.py](zoom_controller.py) размещается рядом с ними в пакете PTZ.
## Профиль камеры
Ускоренный путь включается индивидуально через ключ predictive_zoom в CAMERA_CONFIG. Его значения передаются в ZoomConfig. Требуются nominal_gain и upper_gain для native10, измеренные на этой камере. Коэффициенты масштабируют логарифмический темп изменения фокусного расстояния. Паспортные 5,5 с для 6–252 мм задают единицу отсчёта. Для верхней границы используют измеренный максимальный отклик с запасом. Числа из тестов относятся к виртуальному приводу. Камера без настроенного профиля использует политику PR №7.
Первый импульс использует три последовательных кадра OSTrack с согласованной траекторией и последний принятый YOLO-матч того же объекта в пределах 650 мс. На 59° допуск составляет до 30% полуширины кадра, сужаясь до штатной зелёной зоны при 35°. Контроллер отдельно проверяет остаточное угловое движение и прогнозирует запас до обеих границ кадра. Целевой размер по умолчанию 48 × 48 px относится к остановке приближения. Качество распознавания зависит от изображения и модели.
Статус корректирует оценку HFOV асинхронно. После подтверждённого STOP уже подтверждённый оптический шаг может дать следующий импульс на ближайшем свежем кадре. Это снимает ожидание дополнительного опроса после STOP. Если подтверждение оптики задерживается, продолжение остаётся за действующим учётом доставки и оптики. Предсказанный HFOV используется для запаса кадра и компенсации Pan/Tilt. Подтверждение оптики поступает из ISAPI.
Длительность от 340 мс при HFOV от 25° до 120 мс при HFOV до 15°. У каждого импульса собственный неизменяемый дедлайн с верхним пределом watchdog 400 мс. Ранний STOP использует действующий приоритетный канал. Он сохраняет свежий доворот там, где аппаратная политика разрешает раздельную остановку осей. Требуемый полный STOP сохраняется. Весовые файлы, YOLO, OSTrack и тревоги используют прежние пути.
## Телеметрия и проверки
Укажите PREDICTIVE_ZOOM_TELEMETRY_DIR для записи JSONL отдельно по каждому TRACK. Первый импульс содержит времена до отправки PUT и до успешного ответа. Строки кадров содержат причину решения, ширину и высоту bbox, исходный YOLO confidence, OSTrack score, измеренный и оценочный HFOV, время измерения и нижнюю границу HFOV. Запись идёт в отдельном потоке через ограниченную очередь.
- Тесты контроллера и логирования: python -m unittest discover -s integration/ptz_predictive_zoom -v
- График эпизода: python integration/ptz_predictive_zoom/plot_episode.py episode.jsonl episode.svg
- Повтор полных путей TRACK и транспорта: run_local_replay.py принимает каталог снимков Codex, каталог этого пакета, корень Electro-pribory и вариант before или after. Для after рядом со снимками нужна пропатченная копия ptz_predictive_zoom_candidate_20261111. Повтор использует локальные fixtures PR №7, файлы телеметрии и имитацию сети и камеры.
В одном синтетическом пролёте за 3,6 с получено 7 принятых импульсов вместо 4. Пять повторов запущены до нового статуса после STOP. При исходном смещении 26% полуширины кандидат запустил первый импульс, а исходный контроллер за этот пролёт до зума не дошёл. Линза, кадры и валидация в этом сравнении моделировались.

@ -0,0 +1,17 @@
{
"postprocess.py": {
"before": "776b5b337e5f898e69770fec517c4459560f4cc6adeb951af40ac8f0b2178534",
"after": "82ba632e7e2696e5b48daa99e77741482b98a02103eaf7fb416c58fd686e4327"
},
"ptz_io.py": {
"before": "13d31539193b1a773a5d1db2187cc2a0c8db67d0aacaad89082face64df07499",
"after": "0b0651b0e0344e4f78aeaf01f76c969d0043148a4ac92e4b7b6fd4a5f0d6a2e7"
},
"status.py": {
"before": "2fca9a2000c2cac9af42ad3f637416c66e1bb87f2f8166c7aefbbebbe5d07b06",
"after": "820db8615638c728d1017ea365f3d16b95edfe61dbb684ba0cda8de051ea8bd1"
},
"zoom_controller.py": {
"after": "7197baba29976215b05f6f86ae1563e8b5c28b314a80894600ede517de00f91e"
}
}

@ -0,0 +1,73 @@
"""Render an episode JSONL as a static SVG without plotting dependencies."""
import argparse
import json
from pathlib import Path
from xml.sax.saxutils import escape
def render(rows):
samples = sorted((r for r in rows if r["event"] == "tick"), key=lambda r: r["elapsed_ms"])
if not samples:
raise ValueError("The episode has no TRACK ticks.")
last = max(1.0, samples[-1]["elapsed_ms"])
groups = (
("HFOV (deg)", 60.0, (
("ISAPI actual", "#2563eb", lambda r: r["FOV_actual"]),
("Estimated", "#c2410c", lambda r: r["FOV_estimated"]),
("Narrow bound", "#64748b", lambda r: r["FOV_min_estimated"]),
)),
("BBox (px)", max(48.0, max(max(r["bbox_px"]) for r in samples)), (
("Width", "#2563eb", lambda r: r["bbox_px"][0]),
("Height", "#c2410c", lambda r: r["bbox_px"][1]),
)),
("Confidence", 1.0, (
("YOLO last match", "#2563eb", lambda r: r["detector_conf"]),
("OSTrack score", "#c2410c", lambda r: r["tracker_score"]),
)),
)
content = [
'<svg xmlns="http://www.w3.org/2000/svg" width="980" height="680" viewBox="0 0 980 680">',
'<rect width="100%" height="100%" fill="white"/>',
'<g fill="#0f172a" font-family="sans-serif" font-size="15">',
f'<text x="68" y="27">PTZ {escape(str(samples[0]["cam"]))} · TRACK telemetry</text>',
]
first = next((r.get("t_to_first_zoom_ms") for r in rows if r["event"] == "first_zoom"), None)
for panel, (title, ymax, series) in enumerate(groups):
y0 = 76 + 190 * panel
plot_h = 133
content.append(f'<text x="68" y="{y0-12}">{title}</text>')
content.append(f'<path d="M68 {y0} v{plot_h} h850" fill="none" stroke="#94a3b8"/>')
for frac in (0, .5, 1):
content.append(f'<text x="17" y="{y0 + plot_h * (1-frac) + 5}">{ymax*frac:.1f}</text>')
for number, (label, color, extract) in enumerate(series):
content.append(f'<text fill="{color}" x="{265 + 170 * number}" y="{y0-12}">{label}</text>')
path = []
for row in samples:
value = extract(row)
if value is None:
if path and path[-1]:
path.append("")
continue
x = 68 + 850 * row["elapsed_ms"] / last
y = y0 + plot_h * (1 - value / ymax)
command = "L" if path and path[-1] else "M"
path.append(f"{command}{x:.1f} {y:.1f}")
content.append(f'<path d="{" ".join(path)}" stroke="{color}" stroke-width="2" fill="none"/>')
if first is not None and 0 <= first <= last:
x = 68 + 850 * first / last
content.append(f'<path d="M{x:.1f} {y0} v{plot_h}" stroke="#334155" stroke-dasharray="3 3"/>')
for k in range(6):
content.append(f'<text x="{68 + 170*k}" y="623">{last*k/5000:.2f}</text>')
content.append('<text x="68" y="654">Seconds since TRACK_START. Dashed marker = first acknowledged zoom.</text>')
content.append('</g></svg>')
return "\n".join(content) + "\n"
if __name__ == "__main__":
parser = argparse.ArgumentParser(description=__doc__)
parser.add_argument("episode", type=Path)
parser.add_argument("output", type=Path)
args = parser.parse_args()
rows = [json.loads(line) for line in args.episode.read_text(encoding="utf-8").splitlines() if line]
args.output.write_text(render(rows), encoding="utf-8")

@ -0,0 +1,42 @@
from pathlib import Path
import json, os, sys
v,z,root,variant=map(Path,sys.argv[1:])
candidate=v/"ptz_predictive_zoom_candidate_20261111"
pr7=v/"ptz_zoom_threshold_diagnostics_20261111"
pins=json.loads((z/"pins.json").read_text())
dest=v/f"predictive_zoom_regression_{variant}.json"
before=str(variant)=="before"
p=(v/"ptz_zoom_threshold_maintenance_20261111" if before else candidate)/"postprocess.py"
i=(v/"ptz_zoom_zero_decision_20261109" if before else candidate)/"ptz_io.py"
s=(v/"ptz_monitor/server_unknown_source_20261109_043525" if before else candidate)/"status.py"
source=v/"ptz_monitor/server_unknown_source_20261109_043525"
key="before" if before else "after"
sys.path.insert(0,str(pr7))
if not before:
sys.path.insert(0,str(z))
sys.argv=[str(pr7/"replay.py"),str(source),str(i),
str(v/"ptz_zoom_stop_maintenance_20261109/ssh_7c7ae84464cc400dbc7dc1327124bbf7/telemetry.json"),
str(v/"cc1_cc2_control_replay_20261026/test_control_replay.py"),str(p),
pins["postprocess.py"][key],pins["ptz_io.py"][key],str(root),str(dest),
str(v/"ptz_zoom_zero_maintenance_20261109/ssh_8f0f9accd24b4f40b6466d0566cafbce/telemetry.json"),"fixed","--thresholds"]
r=(pr7/"replay.py").read_text(encoding="utf-8")
if not before:
r=r.replace(" ptz_io=load",f" load(PKG+'.zoom_controller', Path({str(z/'zoom_controller.py')!r}))\n ptz_io=load")
r=r.replace(" pp=load",f" status=load(PKG+'.status', Path({str(s)!r}))\n pp=load")
r=r.replace(" result=unittest.TextTestRunner", """ from track_integration import TRACE as zoom_trace, build_track_cases
integration = build_track_cases(before, pp, ptz_io, config, state, status)
for case in ("test_track_runs_burst_without_post_stop_status",
"test_track_same_flight_legacy_reference",
"test_track_starts_at_26_percent_of_half_frame",
"test_track_legacy_reference_at_26_percent_of_half_frame",
"test_priority_zoom_stop_honors_axis_policy",
"test_priority_zoom_stop_does_not_override_full_stop"):
suite.addTest(integration(case))
result=unittest.TextTestRunner""")
r=r.replace(" result_file=Path", f""" if result.wasSuccessful():
Path({str(v/"predictive_zoom_simulated_episode.jsonl")!r}).write_text(
"".join(json.dumps(row, allow_nan=False) + "\\n" for row in zoom_trace), encoding="utf-8")
result_file=Path""")
code=compile(r,str(pr7/"replay.py"),"exec")
exec(code,{"__file__":str(pr7/"replay.py"),"__name__":"__main__"})

@ -0,0 +1,377 @@
--- a/postprocess.py
+++ b/postprocess.py
@@ -10,6 +10,7 @@
import cv2
from . import config, state, visual_control
+from .zoom_controller import ZoomFrame, controller_for_track, telemetry_tick
from . import motion_detector
from .utils import (
clipf,
@@ -30,6 +31,7 @@
_zoom_command_lock,
zoom_step_optics_delta,
_zoom_speed_to_int,
+ stop_predictive_zoom_async,
)
from .geom import ang_diff_signed, sector_contains
from .patrol import (
@@ -4597,6 +4599,7 @@
optics_observed_now: float | None = None,
stop_trace: list[str] | None = None,
continuation_trace: list[tuple[int, int, float, float, float, float, float]] | None = None,
+ predictive=None,
) -> float:
"""Break continuous zoom-in into short feedback-controlled steps."""
pulse_until = float(st.get("zoom_step_until", 0.0) or 0.0)
@@ -4706,18 +4709,23 @@
_finish_zoom_step(st, now, stable_key=stable_key)
return 0.0
if pulse_until <= 0.0:
- if (
+ observed = now if optics_observed_now is None else optics_observed_now
+ early_repeat = bool(predictive and predictive.repeat_after_stop(st, observed))
+ if not early_repeat and (
float(now) < cooldown_until
or float(now) < float(st.get("zoom_recenter_until", 0.0) or 0.0)
):
return 0.0
- if (
+ if early_repeat:
+ # The original transaction is already committed. Consume its single
+ # credit under the transport lock, before publishing a new request.
+ predictive.confirmed_stop = None
+ st["zoom_feedback_required_after"] = 0.0
+ elif (
int(st.get("zoom_in_pulse_count", 0) or 0) > 0
or float(st.get("zoom_feedback_required_after", 0.0) or 0.0) > 0.0
):
- feedback_ready, feedback_reason = _zoom_optics_feedback_state(
- st, now if optics_observed_now is None else optics_observed_now,
- )
+ feedback_ready, feedback_reason = _zoom_optics_feedback_state(st, observed)
if not feedback_ready:
st["zoom_followup_reason"] = str(feedback_reason)
return 0.0
@@ -8778,6 +8786,74 @@
react_margin = clipf(react_margin_base + react_margin_gain * vel, react_margin_base, react_margin_max)
near_react_edge = min_margin < react_margin
+ predictive = None
+ predictive_frame = None
+ predictive_decision = None
+ if cfg_ptz.get("predictive_zoom") is not None:
+ with _zoom_command_lock(cam_idx):
+ predictive = controller_for_track(st, cfg_ptz)
+ if predictive is not None:
+ decision_wall = time.time()
+ sample_is_current = bool(
+ visual_sample is not None
+ and visual_control.current(
+ cam_idx, visual_sample, decision_wall,
+ max_age=predictive.cfg.frame_age_s,
+ )
+ )
+ predictive_frame = ZoomFrame(
+ at=now, seq=int(source_seq or 0),
+ offset=(measured_dx, measured_dy),
+ bbox_frac=(w_frac, h_frac), bbox_px=(x2 - x1, y2 - y1),
+ angles=(los_pan_angle_deg, los_tilt_angle_deg),
+ rates=(los_pan_rate_deg_s, los_tilt_rate_deg_s),
+ aspect=h / w,
+ tracker_score=(
+ float(st.get("visual_tracker_score", 0.0) or 0.0)
+ if current_track_source == "visual" else None
+ ),
+ detector_at=(last_visual_zoom_validation if normal_zoom_quality_ok
+ and last_visual_zoom_validation == last_visual_control_validation else 0.0),
+ detector_conf=float(st.get("visual_tracker_last_detector_conf", 0.0) or 0.0),
+ geometry_ok=bool(
+ sample_is_current and not st.get("visual_tracker_mismatch_frames")
+ and _zoom_revalidation_ready(st)
+ ),
+ los_valid=bool(
+ st.get("aim_los_rate_valid")
+ or predictive.request is not None
+ or predictive.repeat_after_stop(st, decision_wall)
+ or (st.get("zoom_step_pending") and predictive.history)
+ ),
+ )
+ skip_wait = predictive.repeat_after_stop(st, decision_wall)
+ same_pending = bool(
+ st.get("zoom_step_pending")
+ and st.get("predictive_zoom_request") == st.get("zoom_step_pending_id")
+ and not st.get("zoom_step_cancel_requested")
+ and not st.get("zoom_step_discard_on_finalize")
+ )
+ predictive_decision = predictive.decide(
+ predictive_frame, decision_wall,
+ first=pulse_count == 0 and predictive.first_ack_at is None,
+ cooldown=not skip_wait and decision_wall < max(
+ float(st.get("zoom_step_cooldown_until", 0.0) or 0.0),
+ float(st.get("zoom_recenter_until", 0.0) or 0.0),
+ ),
+ transport_clear=bool(
+ same_pending or skip_wait or (pulse_count == 0
+ and not st.get("zoom_feedback_required_after")
+ and not st.get("zoom_step_feedback_pending"))
+ or _zoom_optics_feedback_state(st, decision_wall)[0]
+ ) and not (
+ st.get("zoom_stop_obligation_active")
+ or st.get("lens_move_stop_required")
+ or st.get("zoom_step_cancel_requested")
+ or st.get("zoom_no_effect_blocked")
+ or (st.get("zoom_step_pending") and not same_pending)
+ ),
+ )
+
zoom_quality_ok = bool(
normal_zoom_quality_ok
or probe_allowed
@@ -8819,6 +8895,21 @@
pan_rate_deg_s=float(los_pan_rate_deg_s),
tilt_rate_deg_s=float(los_tilt_rate_deg_s),
)
+ predictive_entry = bool(
+ predictive_decision and predictive_decision.window
+ and normal_zoom_quality_ok and not bounded_identification_request
+ and not st.get("zoom_probe_active", False)
+ and not _zoom_identify_matches_current_track(st)
+ and not st.get("zoom_stop_obligation_active")
+ and not st.get("zoom_step_cancel_requested")
+ )
+ predictive_continuing = bool(
+ predictive_decision and same_pending and not predictive_decision.stop
+ and (predictive_decision.window or predictive_decision.reason == "pulse_active")
+ and normal_zoom_quality_ok and not bounded_identification_request
+ )
+ if (predictive_entry and (skip_wait or same_pending)) or predictive_continuing:
+ followup_ready = True
angular_rejection = {} if pulse_active else None
zoom_angular_safe = followup_ready and _zoom_angular_motion_safe(
pan_error_deg=float(los_pan_angle_deg),
@@ -8832,6 +8923,9 @@
)
if approach_margin is not None and followup_ready:
zoom_angular_safe = True
+ if (predictive_entry or predictive_continuing) and followup_ready:
+ zoom_angular_safe = True
+ zoom_safe, zoom_safe_scale = True, 1.0
st["zoom_safe_pred_margin"] = float(zoom_pred_margin)
st["zoom_safe_center_limit"] = float(zoom_center_max)
st["zoom_quality_ok"] = bool(zoom_quality_ok)
@@ -8952,6 +9046,7 @@
or bounded_identification_request
)
zoom_rearm_ready = _zoom_revalidation_ready(st)
+ stable_ok = stable_ok or predictive_entry or predictive_continuing
can_zoom_in = (
zoom_safe
and zoom_angular_safe
@@ -9099,6 +9194,14 @@
zoom_zero_reason = "deadband"
zoom_cmd = 0.0
+ predictive_drive = bool(
+ (predictive_entry or predictive_continuing) and can_zoom_in and raw_zoom_needed
+ and prev_z >= 0.0 and not want_out
+ and not normal_size_hold and not edge_recovery_active
+ and float(getattr(config, "ZOOM_STEP_MAX_IN_CMD", 0.12)) >= 0.10
+ )
+ if predictive_drive:
+ zoom_cmd = 0.10
zoom_after_size_minimum = zoom_cmd
cmd_mag_est = max(
abs(float(pan_speed) + float(pan_rate_ff)),
@@ -9151,7 +9254,7 @@
center_err=float(center_err),
moving_out=bool(moving_out),
recent_zoom=bool(recent_zoom_steering),
- approach_margin=approach_margin,
+ approach_margin=(predictive_decision.margin if predictive_drive else approach_margin),
)
if before_recenter > 0.0 and zoom_cmd <= 0.0:
zoom_zero_reason = "recenter"
@@ -9272,6 +9375,25 @@
proposed_zoom_cmd = max(zoom_cmd, min(pre_ramp_zoom_cmd, pulse_cap, .12))
if _zoom_native_response_available(st, _zoom_speed_to_int(proposed_zoom_cmd), hfov_deg, shared_wide_hfov):
zoom_cmd = proposed_zoom_cmd
+ # A separately checked predictive window supplies the normal native-10
+ # shape. Preserve the original ticket and timing on subsequent frames.
+ predictive_active = bool(
+ predictive_continuing and predictive_drive and can_zoom_in and zoom_cmd > 0.0
+ and pulse_family == "normal" and st.get("zoom_step_pending")
+ and predictive.owns(st)
+ and st.get("zoom_step_pending_id") == decision_request
+ and st.get("predictive_zoom_request") == decision_request
+ and 0 <= zoom_gate_now - now <= predictive.cfg.frame_age_s
+ )
+ predictive_start = bool(
+ predictive_entry and predictive_drive and pulse_family == "normal"
+ and zoom_cmd > 0.0 and not st.get("zoom_step_pending") and predictive.owns(st)
+ and not st.get("zoom_stop_obligation_active") and not st.get("lens_move_stop_required")
+ and 0 <= zoom_gate_now - now <= predictive.cfg.frame_age_s
+ and 0 <= zoom_gate_now - predictive_frame.detector_at <= predictive.cfg.detector_age_s
+ and predictive.optics.fresh(zoom_gate_now)
+ and (not skip_wait or predictive.repeat_after_stop(st, zoom_gate_now))
+ )
before_shape = zoom_cmd
zoom_cmd, useful_reason = _shape_useful_zoom_pulse(
zoom_cmd,
@@ -9299,6 +9421,21 @@
requested_native=timing_native,
rejection=response_rejection,
)
+ if predictive_start or (predictive_active and not near_edge and not edge_recovery_active
+ and zoom_rearm_ready and not normal_size_hold and not want_out):
+ zoom_cmd = 0.10
+ useful_reason = "useful"
+ st["zoom_useful_pulse_reason"] = useful_reason
+ response_timing = predictive_decision.window if predictive_start else (
+ float(st["zoom_step_pending_pulse_sec"]),
+ float(st["zoom_step_pending_hard_sec"]),
+ )
+ if predictive_decision and (
+ predictive_decision.stop or (predictive_decision.reason == "size_reached"
+ and pulse_family == "normal")
+ ):
+ zoom_cmd = min(zoom_cmd, 0.0)
+ zoom_zero_reason = predictive_decision.reason
if useful_reason == "useful" and response_timing == (0.0, 0.0):
zoom_cmd = 0.0
st["zoom_useful_pulse_reason"] = "wait_response_framing"
@@ -9321,7 +9458,10 @@
optics_observed_now=zoom_gate_now,
stop_trace=gate_stop,
continuation_trace=continuation_trace,
- )
+ predictive=predictive if predictive_start else None,
+ )
+ if predictive_start and zoom_cmd > 0.0 and st["zoom_step_request_id"] > request_id_before_gate:
+ st["predictive_zoom_request"] = st["zoom_step_pending_id"]
if gate_stop:
zoom_zero_reason = gate_stop[-1]
decision_generation = st.get("ptz_generation")
@@ -9380,6 +9520,12 @@
adj_pan, adj_tilt = _shape_tracking_cmd(st, now, adj_pan, adj_tilt, center_err)
pan_feedback_cmd, tilt_feedback_cmd = float(adj_pan), float(adj_tilt)
+ if predictive_active and predictive.request is not None and zoom_cmd > 0.0:
+ pan_optical, tilt_optical = predictive_decision.ff_deg_s
+ pan_ref = max(1.0, float(cfg_ptz.get("ptz_pan_max_speed_deg_s", getattr(config, "AIM_PAN_SPEED_DEG_S", 160.0))))
+ tilt_ref = max(1.0, float(cfg_ptz.get("ptz_tilt_max_speed_deg_s", getattr(config, "AIM_TILT_SPEED_DEG_S", 120.0))))
+ pan_rate_ff += pan_optical / pan_ref
+ tilt_rate_ff += tilt_optical / tilt_ref
adj_pan, adj_tilt = _merge_gimbal_rate_feedforward(
st,
now,
@@ -9844,6 +9990,14 @@
("brake", final_guard_active),
) if blocked)
zoom_decision = (source_seq, decision_request, zoom_zero_reason, "+".join(blockers) or "-")
+ if predictive_decision is not None:
+ with _zoom_command_lock(cam_idx):
+ telemetry_tick(
+ config, cam_idx, predictive, predictive_frame, predictive_decision,
+ zoom_cmd, zoom_zero_reason, time.time(),
+ )
+ if predictive_decision.stop and predictive.request is not None:
+ stop_predictive_zoom_async(cam_idx, predictive, predictive_decision.reason)
dispatch_kwargs = {"force": force_track_ptz}
if zoom_decision is not None:
dispatch_kwargs["zoom_decision"] = zoom_decision
--- a/ptz_io.py
+++ b/ptz_io.py
@@ -14,6 +14,7 @@
from requests.adapters import HTTPAdapter
from . import config, state
+from .zoom_controller import transport_accepted, transport_confirmed, transport_stopped
from .utils import clipf, hfov_ptz_from_abs_zoom, hfov_ptz_from_zoom_ratio, quantf
from .geom import sector_contains, ang_diff_signed, normalize360
@@ -582,6 +583,10 @@
st["zoom_last_motion_at"] = float(now)
st["zoom_followup_reason"] = "physical_pulse"
st["zoom_step_last_delivery_result"] = "physical_started"
+ transport_accepted(
+ config, cam_idx, st, request_id, native_cmd, deadline_origin, now,
+ deadline_origin + hard_sec,
+ )
logger.info(
"[ZOOM DELIVERY] cam=%s request=%s kind=%s physical start seq=%s pulse=%.3fs hard=%.3fs",
cam_idx,
@@ -744,6 +749,8 @@
float(st.get("zoom_recenter_until", 0.0) or 0.0),
float(now) + float(getattr(config, "ZOOM_STEP_RECENTER_SEC", 0.14)),
)
+ if reason == "status_poll":
+ transport_confirmed(config, st, request_id, now)
logger.info(
"[ZOOM DELIVERY] cam=%s request=%s optics confirmed source=%s zstep=%s",
cam_idx,
@@ -922,6 +929,12 @@
+ max(0.0, float(getattr(config, "ZOOM_STEP_RETRY_COOLDOWN_SEC", 0.12))),
)
+ transport_stopped(
+ config, cam_idx, st, request_id, now, reason=reason,
+ confirmed=confirmed and not discard,
+ clean=bool(not discard and kind == "normal" and delivered + 1e-6 >= full_window
+ and stop_axes is not None and stop_axes[2] == 0),
+ )
st["zoom_step_last_delivered_sec"] = float(delivered)
st["zoom_step_pending"] = False
st["zoom_step_pending_id"] = 0
@@ -2119,6 +2132,35 @@
if zoom_decision is not None:
return cmd_source, cmd_generation, move_seq, zoom_i
return None
+
+
+def stop_predictive_zoom_async(cam_idx, controller, reason):
+ """Order an early lens STOP independently of the ordinary MOVE queue."""
+ with _zoom_command_lock(cam_idx):
+ st = state.ptz_states.get(cam_idx, {})
+ request = controller.request
+ if (not controller.owns(st) or request is None
+ or st.get("zoom_step_physical_request_id") != request
+ or st.get("predictive_zoom_early_stop") == request):
+ return
+ st["predictive_zoom_early_stop"] = request
+ st["zoom_step_cancel_requested"] = True
+
+ def stop_if_current():
+ with _zoom_command_lock(cam_idx):
+ current = state.ptz_states.get(cam_idx, {})
+ if (controller.owns(current)
+ and current.get("zoom_step_physical_request_id") == request):
+ _deliver_priority_lens_stop_locked(
+ cam_idx, current, reason=f"predictive:{reason}",
+ guard_inflight_after_put=_generic_put_has_lens_motion(current),
+ )
+
+ try:
+ threading.Thread(target=stop_if_current, name="ptz-predictive-stop", daemon=True).start()
+ except RuntimeError:
+ # The scheduled watchdog remains the independent hard STOP.
+ logger.exception("Unable to start early lens STOP cam=%s", cam_idx)
def stop_ptz(cam_idx: int, *, source: str = "stop", generation: Optional[int] = None) -> None:
--- a/status.py
+++ b/status.py
@@ -170,6 +170,13 @@
if isinstance(hfov_deg, (int, float)) and float(hfov_deg) > 0.0:
st["hfov_deg"] = float(hfov_deg)
st["zoom_status_at"] = zoom_observed_at
+ if config.CAMERA_CONFIG.get(int(cam_id), {}).get("predictive_zoom") is not None:
+ previous = st.get("predictive_zoom_hfov", (0.0, 0.0))
+ if zoom_observed_at > previous[0]:
+ st["predictive_zoom_hfov"] = (zoom_observed_at, float(hfov_deg))
+ controller = st.get("predictive_zoom_controller")
+ if controller is not None and controller.owns(st):
+ controller.optics.observe(zoom_observed_at, float(hfov_deg), float(now))
if isinstance(zoom_raw, (int, float)):
st["zoom_position_raw"] = float(zoom_raw)
st["zoom_position_tag"] = str(zoom_tag or "")

@ -0,0 +1,172 @@
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()

@ -0,0 +1,156 @@
"""Integration cases for the PR #7 mock transport/control replay fixture."""
import math
from types import SimpleNamespace
from unittest.mock import patch
TRACE = []
def build_track_cases(fixture, pp, ptz_io, config, state, status):
class PredictiveTrack(fixture.Replay):
def setUp(self):
cameras = [8, *[cam for cam in config.PTZ_CAM_IDS if cam != 8]]
with patch.object(config, 'PTZ_CAM_IDS', cameras):
super().setUp()
def flight(self, enabled, center_x=720, *, record=False):
f, st = self.fixture, self.st
if enabled:
self.stack.enter_context(patch.dict(config.CAMERA_CONFIG[self.cam], {
"predictive_zoom": {"nominal_gain": .45, "upper_gain": .8},
}))
self.stack.enter_context(patch.object(config, "PTZ_ZOOM_STOP_FORCE_AXES_ZERO", False, create=True))
self.stack.enter_context(patch.object(config, "PREDICTIVE_ZOOM_TELEMETRY_DIR", None, create=True))
decisions = []
capture = pp.telemetry_tick
def remember(config, cam, controller, frame, decision, zoom, why, at):
decisions.append((round(at-100, 2), decision.reason, round(zoom, 3), why,
st.get("zoom_followup_reason"), st.get("zoom_step_last_delivery_result")))
estimated, narrowest = controller.optics.estimate(at)
if record:
TRACE.append({
"event": "tick", "cam": cam + 11, "elapsed_ms": round((at-100)*1000, 1),
"FOV_actual": controller.optics.actual, "FOV_estimated": estimated,
"FOV_min_estimated": narrowest, "bbox_px": frame.bbox_px,
"detector_conf": frame.detector_conf, "tracker_score": frame.tracker_score,
"reason": decision.reason,
})
capture(config, cam, controller, frame, decision, zoom, why, at)
self.stack.enter_context(patch.object(pp, "telemetry_tick", side_effect=remember))
box = (center_x - 13, 347, center_x + 13, 371)
st.update(track_started_at=99.95, last_bbox=box,
prev_cx=center_x / 1280, prev_cy=359 / 720, stable_frames=0)
fov, zoom = 59.0, 1.0
status._update_runtime_state(self.cam, None, None, 99.99, zoom_ratio=zoom, hfov_deg=fov)
delivered = []
last_status = 99.99
completed = []
early = []
for k in range(46):
f._advance(.08 if k else 0)
now = f.wall
if k % 6 == 0:
st.update(visual_tracker_last_detector_at=now - .015,
visual_tracker_last_zoom_detector_at=now - .015,
visual_tracker_last_detector_conf=.8)
if k % 3 == 1:
if st.get("zoom_step_physical_request_id"):
fov *= .97
zoom *= 1.03
status._update_runtime_state(self.cam, None, None, now - .012,
zoom_ratio=zoom, hfov_deg=fov)
last_status = now - .012
st.update(status_rate_at=now - .01, pan_rate_at=now - .01,
tilt_rate_at=now - .01, pan_rate_deg_s=.4, tilt_rate_deg_s=0.0)
timer = st.get("zoom_step_stop_timer")
if (timer and timer.is_alive()
and f.mono + 1e-9 >= st["zoom_step_stop_started_mono"] + timer.delay):
timer.fire()
request_before = st.get("zoom_step_physical_request_id")
self.seq += 1
scale = math.tan(math.radians(59 / 2)) / math.tan(math.radians(fov / 2))
half_w, half_h = round(13 * scale), round(12 * scale)
box = (center_x - half_w, 359 - half_h, center_x + half_w, 359 + half_h)
self.assertTrue(0 < box[0] < box[2] < 1280 and 0 < box[1] < box[3] < 720)
track = fixture.tracker.VisualTrackResult(True, box, .8, 0, .8, 30,
"synthetic captured", self.seq)
sample = fixture.control.VisualSample(fixture.control.owner(st), self.seq, now-.01, track)
pp.postprocess_control(self.cam, self.frame, SimpleNamespace(boxes=fixture.Boxes([])),
True, self.seq, visual_sample=sample, visual_only=True)
if not state.ptz_cmd_q.empty():
f._flush()
request = st.get("zoom_step_physical_request_id")
if request and request != request_before:
delivered.append((round(now-100, 2), request, last_status))
if st.get("predictive_zoom_request") == request and st["zoom_step_finished_at"] > last_status:
early.append(request)
if request_before and not request:
completed.append(request_before)
self.assertEqual(st["mode"], "TRACK")
return delivered, early, fov, decisions, completed
def test_track_runs_burst_without_post_stop_status(self):
delivered, early, fov, decisions, completed = self.flight(True, record=True)
self.assertTrue(delivered, decisions)
TRACE.insert(0, {"event": "first_zoom", "t_to_first_zoom_ms": delivered[0][0]*1000})
self.assertGreaterEqual(len(delivered), 3, (delivered, decisions))
self.assertLess(delivered[0][0], .5, decisions)
self.assertGreaterEqual(len(early), 1, (early, delivered, decisions))
print(f"3.6s TRACK: {len(delivered)} starts, first {delivered[0][0]:.2f}s, "
f"{len(early)} before a post-STOP poll, terminal simulated HFOV {fov:.1f} deg")
self.assertTrue(any(row[1] == "burst" for row in decisions), decisions)
self.assertGreaterEqual(self.st["zoom_in_pulse_count"], 2, (completed, decisions))
self.assertLess(fov, 59.0)
def test_track_same_flight_legacy_reference(self):
delivered, early, fov, decisions, completed = self.flight(False)
first = delivered[0][0] if delivered else None
print(f"3.6s legacy TRACK: {len(delivered)} starts, first {first}s, terminal simulated HFOV {fov:.1f} deg")
self.assertLess(len(delivered), 7)
self.assertFalse(early)
def test_track_starts_at_26_percent_of_half_frame(self):
delivered, _, _, decisions, _ = self.flight(True, 806)
self.assertTrue(delivered, decisions)
self.assertLess(delivered[0][0], .5)
self.assertTrue(any(reason == "fast_start" and zoom > 0
for _, reason, zoom, *_ in decisions), decisions)
def test_track_legacy_reference_at_26_percent_of_half_frame(self):
delivered, _, _, _, _ = self.flight(False, 806)
print(f"3.6s legacy TRACK at 26% half-frame: {len(delivered)} starts")
self.assertFalse(delivered)
def assert_early_stop_axes(self, full_stop):
f, st = self.fixture, self.st
from ptz_tracker_modular.zoom_controller import controller_for_track
self.stack.enter_context(patch.dict(config.CAMERA_CONFIG[self.cam], {
"predictive_zoom": {"nominal_gain": .45, "upper_gain": .8},
}))
controller = controller_for_track(st, config.CAMERA_CONFIG[self.cam])
controller.optics.observe(99.95, 59.0, f.wall)
cmd = pp._gate_zoom_in_pulse(st, f.wall, .10, True, pulse_floor=.10, pulse_timing=(.34, .40))
st["predictive_zoom_request"] = st["zoom_step_pending_id"]
f._enqueue(cmd, pan=.12, tilt=-.08)
f._flush()
request = controller.request
self.assertIsNotNone(request)
class InlineThread:
def __init__(self, *, target, **_):
self.target = target
def start(self):
self.target()
self.stack.enter_context(patch.object(ptz_io.threading, "Thread", InlineThread))
self.stack.enter_context(patch.object(config, "PTZ_ZOOM_STOP_FORCE_AXES_ZERO", full_stop, create=True))
ptz_io.stop_predictive_zoom_async(self.cam, controller, "edge_acceleration")
axes = tuple(fixture.transport.xml_axis(f.sent[-1], axis) for axis in ("pan", "tilt", "zoom"))
self.assertEqual(axes, (0, 0, 0) if full_stop else (12, -8, 0))
self.assertIsNone(controller.request)
self.assertFalse(controller.repeat_after_stop(st, f.wall))
def test_priority_zoom_stop_honors_axis_policy(self):
self.assert_early_stop_axes(full_stop=False)
def test_priority_zoom_stop_does_not_override_full_stop(self):
self.assert_early_stop_axes(full_stop=True)
return PredictiveTrack

@ -0,0 +1,524 @@
"""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)
Loading…
Cancel
Save