diff --git a/integration/ptz_predictive_zoom/README.md b/integration/ptz_predictive_zoom/README.md index caf2b2b..e1df04a 100644 --- a/integration/ptz_predictive_zoom/README.md +++ b/integration/ptz_predictive_zoom/README.md @@ -14,14 +14,31 @@ Длительность от 340 мс при HFOV от 25° до 120 мс при HFOV до 15°. У каждого импульса собственный неизменяемый дедлайн с верхним пределом watchdog 400 мс. Ранний STOP использует действующий приоритетный канал. Он сохраняет свежий доворот там, где аппаратная политика разрешает раздельную остановку осей. Требуемый полный STOP сохраняется. Весовые файлы, YOLO, OSTrack и тревоги используют прежние пути. -## Телеметрия и проверки +## Управление широким углом и телеуглом + +Профили PR №8 с двумя коэффициентами по-прежнему загружаются. Дополнительные параметры задаются в том же predictive_zoom для камеры. + +| Параметр | По умолчанию | Назначение | +|---|---:|---| +| initial_gate_relax_factor | 1,45 | Расширяет допуск начального захвата только на широком угле при сходящейся траектории. | +| tele_edge_margin_ratio | 0,32 | Буфер до края на HFOV 12° и ниже. Плавно растёт от 0,16 при 25°. | +| min_allowed_hfov_deg | 10,0° | Нижний предел прогноза для непрерывного положительного зума. Проверяется перед отправкой PUT. | +| latency_horizon_ms | 245 | Горизонт от захвата кадра до действия доворота. | +| tele_pan_native10_deg_s, tele_tilt_native10_deg_s | 0,65 и 0,85 | Калибровка преобразования угловой скорости в команду. | +| tele_position_gain, tele_rate_gain | 2,7 и 0,85 | Угловая коррекция и упреждение. | +| tele_calibration_enabled | true | На HFOV ниже 15° между положительными импульсами допускает ожидание стабилизации статуса до 1,2 с. | -Укажите PREDICTIVE_ZOOM_TELEMETRY_DIR для записи JSONL отдельно по каждому TRACK. Первый импульс содержит времена до отправки PUT и до успешного ответа. Строки кадров содержат причину решения, ширину и высоту bbox, исходный YOLO confidence, OSTrack score, измеренный и оценочный HFOV, время измерения и нижнюю границу HFOV. Запись идёт в отдельном потоке через ограниченную очередь. +Ускоренный вход разрешается при последовательных достоверных кадрах, допустимой текущей рамке и безопасном прогнозе. Неподтверждённые кадры, конфликт владельца и транспортные запреты продолжают блокировать движение. На телеугле профиль уменьшает вес старого PID по мере сужения поля и подмешивает угловую коррекцию с учётом задержки и синхронизированной скорости приводов. При угрозе краю выполняется отдельный короткий импульс расширения с действующими ограничителями отрицательного зума. Полный STOP остаётся приоритетным. + +Предел 10° относится к прогнозу разрешённых импульсов. Настоящая линза и задержки могут отличаться от профиля, поэтому первую полевую серию проверяют по свежему статусу. Абсолютные переходы к пресету обслуживаются прежним кодом. + +## Телеметрия и проверки -- Тесты контроллера и логирования: 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, файлы телеметрии и имитацию сети и камеры. +По умолчанию сводка zoom_block_summary записывается в JSONL в logs/ptz_zoom через ограниченную очередь. Параметр PREDICTIVE_ZOOM_TELEMETRY_DIR задаёт путь или отключает запись значением None. Сводка включает накопленные centering, bbox_check, angle_tol, edge_margin, min_hfov_limit и put_inflight с последними значениями. Снимок появляется не чаще раза в секунду и при завершении TRACK. Камера, время начала и поколение команд идентифицируют эпизод. -В одном синтетическом пролёте за 3,6 с получено 7 принятых импульсов вместо 4. Пять повторов запущены до нового статуса после STOP. При исходном смещении 26% полуширины кандидат запустил первый импульс, а исходный контроллер за этот пролёт до зума не дошёл. Линза, кадры и валидация в этом сравнении моделировались. +Событие native10_pulse_calibration связывает окно команды, монотонные RTT PUT и STOP, статус до импульса, первый статус после STOP и установившийся HFOV. Поле post_stop_tail_deg показывает наблюдённое сужение между двумя статусами после STOP. Если известен свежий замер до STOP, stop_bracket_narrowing_deg дополнительно показывает изменение между ним и установившимся статусом. Оба измерения ограничены частотой опроса. Отменённые, пересекающиеся и незавершённые окна помечаются. Команда python fit_tele_profile.py episode.jsonl собирает предварительный профиль из допустимых окон для отдельной проверки. Автоматического применения нет. -Профили PTZ19 и PTZ21 для офлайн-повтора приведены в [field_replay_profiles.json](field_replay_profiles.json). Они получены по пяти связанным парам native10 и FOV. Стендовые проверки покрыли задержки видео 180–250 мс и статуса ISAPI 40–60 мс. Измеренного верхнего предела физического отклика на телеугле пока нет. Автоматической загрузки этих значений в конфигурацию камеры нет. +- Unit-тесты: python -m unittest discover -s integration/ptz_predictive_zoom -v +- Полный повтор TRACK и транспорта: run_local_replay.py принимает каталог снимков Codex, каталог пакета, корень Electro-pribory, вариант before или after и необязательный путь к материализованной серверной копии. +- Парный стенд: replay_broad_tele.py запускается с --camera 19 и --camera 21, дополнительно с --edge-stress, --put-rtt и --problem. Каталог с candidate и pins.json задаёт PTZ_BROAD_TELE_REPLAY_DIR. Нужны сохранённые записи AIM и стенд предыдущего PR в соседних каталогах снимков. +- График одного эпизода: python plot_episode.py episode.jsonl episode.svg diff --git a/integration/ptz_predictive_zoom/broad_tele_replay_results.json b/integration/ptz_predictive_zoom/broad_tele_replay_results.json new file mode 100644 index 0000000..1024862 --- /dev/null +++ b/integration/ptz_predictive_zoom/broad_tele_replay_results.json @@ -0,0 +1,105 @@ +{ + "paired_cases": 32, + "problem_cases": 3, + "runtime_after_sha": { + "postprocess.py": "dba9b3dc69768526777e166a8ef3270ee54a5d9ed7415a3f905b65e05a8534a5", + "ptz_io.py": "a469c6683c9afc93a1499dd45146e1198bbd752505ba9045704ce5fb8a5d378b", + "status.py": "0f0b91414e04eedc547a5e6931825974f9b5bc09d5795babe5c6ac6179f0882d", + "zoom_controller.py": "6bb81b19e7125daa4ab3ae387ac8ba963bde86f0a51a938d72684ec32d755be3" + }, + "minimum_margin_at_6_deg": { + "PR8": 0.1844, + "BT": 0.2508 + }, + "step_back_cases_at_6_deg": 10, + "joint_below_18_by_camera_starts_at_least_12_deg": { + "19": { + "PR8_pct": 81.54, + "BT_pct": 100.0, + "paired_samples": 661 + }, + "21": { + "PR8_pct": 81.45, + "BT_pct": 99.92, + "paired_samples": 1256 + } + }, + "joint_below_18_by_camera_wide_starts": { + "19": { + "PR8_pct": 100.0, + "BT_pct": 100.0, + "paired_samples": 379 + }, + "21": { + "PR8_pct": 86.45, + "BT_pct": 99.9, + "paired_samples": 974 + } + }, + "joint_11_to_18_by_camera": { + "19": { + "PR8_pct": 100.0, + "BT_pct": 100.0, + "paired_samples": 407 + }, + "21": { + "PR8_pct": 86.83, + "BT_pct": 99.9, + "paired_samples": 1002 + } + }, + "below_18_by_controller_at_own_exposure": { + "19:59.0": { + "PR8_pct": 100.0, + "BT_pct": 100.0, + "PR8_samples": 873, + "BT_samples": 383 + }, + "19:12.0": { + "PR8_pct": 56.74, + "BT_pct": 100.0, + "PR8_samples": 282, + "BT_samples": 282 + }, + "19:6.0": { + "PR8_pct": 50.44, + "BT_pct": 93.88, + "PR8_samples": 686, + "BT_samples": 686 + }, + "21:59.0": { + "PR8_pct": 85.56, + "BT_pct": 99.21, + "PR8_samples": 990, + "BT_samples": 1008 + }, + "21:12.0": { + "PR8_pct": 64.18, + "BT_pct": 100.0, + "PR8_samples": 282, + "BT_samples": 282 + }, + "21:6.0": { + "PR8_pct": 41.98, + "BT_pct": 90.23, + "PR8_samples": 686, + "BT_samples": 686 + } + }, + "problem_first_start_s": [ + { + "PR8": 2.18, + "BT": 1.68 + }, + { + "PR8": 2.22, + "BT": 1.72 + }, + { + "PR8": 2.25, + "BT": 1.75 + } + ], + "no_common_tele_exposure_pairs": 5, + "field_calibration_performed": false +} diff --git a/integration/ptz_predictive_zoom/fit_tele_profile.py b/integration/ptz_predictive_zoom/fit_tele_profile.py new file mode 100644 index 0000000..3a34873 --- /dev/null +++ b/integration/ptz_predictive_zoom/fit_tele_profile.py @@ -0,0 +1,68 @@ +"""Summarize qualified telephoto pulses from episode JSONL into review candidates.""" +import argparse +from collections import defaultdict +import json +import math +from pathlib import Path +from statistics import median + + +def focal(fov): + return -math.log(math.tan(math.radians(fov / 2))) + + +def summarize(files): + grouped = defaultdict(dict) + for file in files: + with file.open(encoding="utf-8") as rows: + for line in rows: + item = json.loads(line) + if item.get("event") != "native10_pulse_calibration": + continue + key = (item["track_start_utc"], item["generation"], item["request"]) + grouped[item["cam"]][key] = item + result = {} + for camera, records in sorted(grouped.items()): + accepted = [] + for item in records.values(): + rate = item.get("effective_tele_log_rate_s") + if (item.get("outcome") != "settled" or not isinstance(rate, (float, int)) + or not math.isfinite(rate) or rate <= 0): + continue + before, after = item["HFOV_before"], item["HFOV_after"] + if not 0 < after < before < 15: + continue + if item.get("put_rtt_ms") is None or item.get("stop_rtt_ms") is None: + continue + accepted.append(item) + result[str(camera)] = {"qualified": len(accepted), "seen": len(records), + "candidate_for_review": None} + # A single track may have a common framing or status-timing bias. + if len(accepted) < 3 or len({x["track_start_utc"] for x in accepted}) < 2: + continue + rates = [x["effective_tele_log_rate_s"] for x in accepted] + bracket = [ + max(0., focal(x["HFOV_after"]) - focal(x["HFOV_stop_sample"])) + for x in accepted if x.get("stop_status_at") is not None + and x["put_at"] <= x["stop_status_at"] <= x["stop_issued_at"] + and 0 <= x["stop_issued_at"] - x["stop_status_at"] <= .18 + ] + full_sweep_rate = math.log(252 / 6) / 5.5 + result[str(camera)]["candidate_for_review"] = { + "observed_hfov_before_deg": [min(x["HFOV_before"] for x in accepted), + max(x["HFOV_before"] for x in accepted)], + "median_effective_tele_log_rate_s": median(rates), + "stress_upper_gain": max(rates) * 1.25 / full_sweep_rate, + "qualified_stop_brackets": len(bracket), + "max_stop_bracket_log_narrowing": max(bracket, default=None), + "max_put_rtt_ms": max(x["put_rtt_ms"] for x in accepted), + "max_stop_rtt_ms": max(x["stop_rtt_ms"] for x in accepted), + } + return {"scope": "observed_telephoto_windows_only", "production_enable": False, "cameras": result} + + +if __name__ == "__main__": + parser = argparse.ArgumentParser(description=__doc__) + parser.add_argument("files", nargs="+", type=Path) + args = parser.parse_args() + print(json.dumps(summarize(args.files), indent=2, allow_nan=False)) diff --git a/integration/ptz_predictive_zoom/pins.json b/integration/ptz_predictive_zoom/pins.json index 59bd046..e55a174 100644 --- a/integration/ptz_predictive_zoom/pins.json +++ b/integration/ptz_predictive_zoom/pins.json @@ -1,17 +1,17 @@ { "postprocess.py": { "before": "776b5b337e5f898e69770fec517c4459560f4cc6adeb951af40ac8f0b2178534", - "after": "82ba632e7e2696e5b48daa99e77741482b98a02103eaf7fb416c58fd686e4327" + "after": "dba9b3dc69768526777e166a8ef3270ee54a5d9ed7415a3f905b65e05a8534a5" }, "ptz_io.py": { "before": "13d31539193b1a773a5d1db2187cc2a0c8db67d0aacaad89082face64df07499", - "after": "0b0651b0e0344e4f78aeaf01f76c969d0043148a4ac92e4b7b6fd4a5f0d6a2e7" + "after": "a469c6683c9afc93a1499dd45146e1198bbd752505ba9045704ce5fb8a5d378b" }, "status.py": { "before": "2fca9a2000c2cac9af42ad3f637416c66e1bb87f2f8166c7aefbbebbe5d07b06", - "after": "820db8615638c728d1017ea365f3d16b95edfe61dbb684ba0cda8de051ea8bd1" + "after": "0f0b91414e04eedc547a5e6931825974f9b5bc09d5795babe5c6ac6179f0882d" }, "zoom_controller.py": { - "after": "7197baba29976215b05f6f86ae1563e8b5c28b314a80894600ede517de00f91e" + "after": "6bb81b19e7125daa4ab3ae387ac8ba963bde86f0a51a938d72684ec32d755be3" } } diff --git a/integration/ptz_predictive_zoom/replay_broad_tele.py b/integration/ptz_predictive_zoom/replay_broad_tele.py new file mode 100644 index 0000000..b67dea0 --- /dev/null +++ b/integration/ptz_predictive_zoom/replay_broad_tele.py @@ -0,0 +1,484 @@ +"""Run PR7 and PR8 on identical trajectories reconstructed from logged AIM.""" +from bisect import bisect_right +from collections import Counter, deque +from contextlib import ExitStack +from hashlib import sha256 +from pathlib import Path +from statistics import median +from types import ModuleType, SimpleNamespace +from unittest.mock import patch +import argparse +import cv2 +import json +from inspect import unwrap +import logging +import math +import os +import socket +import sys + +OUT = Path(os.environ.get( + "PTZ_BROAD_TELE_REPLAY_DIR", + Path(__file__).resolve().parents[3] / "ptz_broad_tele_20261111", +)) +DATA = OUT.parent / "ptz_native10_profiles_20261111" +sys.path.insert(0, str(DATA)) +from measure_native10 import FIELD, PULSES, PR7, FULL_SWEEP_LOG_RATE, ROOT, read, utc + +LAB = ROOT / "ptz_virtual_lab_20261111" +sys.path.insert(0, str(LAB)) +import simulator as sim + +CANDIDATE = ROOT / "ptz_predictive_zoom_candidate_20261111" +BUNDLE = ROOT / "chameleon_predictive_zoom_git_20261111/integration/ptz_predictive_zoom" +BASE_STATUS = sim.SOURCE / "status.py" +NEW = OUT / "candidate" +NEW_PINS = OUT / "pins.json" +TRACE_KEYS = ("normal_zoom_quality_ok", "zoom_quality_ok", "zoom_rearm_ready", "zoom_safe", + "zoom_angular_safe", "stable_ok", "near_edge", "edge_recovery_active", + "followup_ready", "raw_zoom_needed", "normal_size_hold", "final_guard_active", + "visual_control_stale", "visual_control_coast", "zoom_zero_reason", + "zoom_cmd", "can_zoom_in", "predictive_decision") +FLIGHTS = { + "19": ("03:11:07", "04:54:08"), + "21": ("04:13:33", "04:37:45"), +} + + +def angle(error, fov): + return math.degrees(math.atan(2*error*math.tan(math.radians(fov/2)))) + + +def trajectory(camera, episode, source): + start, end = map(utc, (episode["started_at"], episode["ended_at"])) + candidates = [r for r in source["cameras"][camera]["samples"] + if start <= r["t"] <= min(end, start+25) + and r["err"] and r["fov"] and r["gimbal"] and r["optics_age"] is not None + and r["optics_age"] <= .65 and r["source"] in ("visual","detector","acquire")] + groups = [] + for r in candidates: + if not groups or r["t"] - groups[-1][-1]["t"] > 1.65: + groups.append([]) + groups[-1].append(r) + # Prefer the longest contiguous trace so neither controller bridges a gap. + rows = max(groups, key=lambda g: g[-1]["t"]-g[0]["t"]) + origin = rows[0]["t"] + pose = [0.0, 0.0] + samples = [] + for i,r in enumerate(rows): + if i: + previous = rows[i-1] + dt = r["t"] - previous["t"] + for axis, sign in enumerate((1,-1)): + pose[axis] += sign * dt * (r["gimbal"][axis]+previous["gimbal"][axis])/2 + bearing = tuple(pose[k]+angle(r["err"][k], r["fov"][k]) for k in range(2)) + samples.append((r["t"]-origin,*bearing)) + duration = math.floor(samples[-1][0]*100)/100 + if duration < 3: + raise ValueError(f"Insufficient contiguous AIM trace: camera {camera} {episode['started_at']}") + # Only a scalar filtered size is available. Use a projected square with that + # geometric mean at initial FOV and hold angular extent constant. + first_size = episode["size_first"] or .018 + fov_x, fov_y = rows[0]["fov"] + angular = tuple(2*angle(first_size/2, fov) for fov in (fov_x, fov_y)) + return dict(camera=int(camera), episode=episode["started_at"], aim_start=origin, + duration=duration, knots=samples, angular_extent=angular, + source_rows=len(samples), observed_hfov=rows[0]["fov"]) + + +class FlightCamera(sim.Camera): + def __init__(self, parameters, pan_sign, tilt_sign, trace, initial_fov): + super().__init__(parameters, pan_sign, tilt_sign) + self.trace = trace + self.times = [x[0] for x in trace["knots"]] + self.magnification = sim.OPTICAL_ZOOM ** ( + math.log(math.tan(math.radians(59/2))/math.tan(math.radians(initial_fov/2))) + / math.log(math.tan(math.radians(59/2))/math.tan(math.radians(2/2))) + ) + if initial_fov < 20: + _, az, el = trace["knots"][0] + # Keep this synthetic narrow-angle stress start inside the aperture. + self.pan, self.tilt = az-.10*initial_fov, el-.05*initial_fov + + def target(self, t): + i = min(len(self.times)-2, max(0, bisect_right(self.times,t)-1)) + a,b = self.trace["knots"][i:i+2] + frac = min(1,max(0,(t-a[0])/(b[0]-a[0]))) + az, el = (x+(y-x)*frac for x,y in zip(a[1:],b[1:])) + if self.trace.get("edge_stress"): + # Same finite lateral acceleration for both controllers. + push = min(1.1,max(0.0,t-.90)) + az += 2.8 * push*push / 1.21 + return az,el + + def project(self, t, *, clipped=True): + center = self.target(t) + bounds = [] + for bearing, pose, width, fov, pixels in zip( + center,(self.pan,self.tilt),self.trace["angular_extent"],(self.hfov,self.vfov),(1280,720) + ): + # stress small angles with a tiny object that still asks for zoom + if self.trace.get("stress"): + width *= .22 + deviation = bearing-pose + if abs(deviation)+width/2 >= 85: + return None + focal = (pixels/2)/math.tan(math.radians(fov/2)) + lo,hi = [round(pixels/2+focal*math.tan(math.radians(deviation+o*width/2))) for o in (-1,1)] + if clipped: + lo,hi = max(0,lo),min(pixels-1,hi) + if hi <= lo: + return None + bounds.append((lo,hi)) + return (bounds[0][0],bounds[1][0],bounds[0][1],bounds[1][1]) + + +def capture_control(pp, function): + row = {} + code = unwrap(pp.postprocess_control).__code__ + + def trace(frame,event,arg): + if event == "call" and frame.f_code is code: + frame.f_trace_lines = False + return trace + if event == "return" and frame.f_code is code: + row.update({k:frame.f_locals[k] for k in TRACE_KEYS if k in frame.f_locals}) + row["return_line"] = frame.f_lineno + return None + + original = sys.gettrace() + sys.settrace(trace) + try: + function() + finally: + sys.settrace(original) + return row + + +def blocker(row, st): + """Primary gate plus all simultaneous blockers. Locals come from live policy.""" + if "zoom_cmd" not in row: + return "track_or_frame_rejected", [] + if row["zoom_cmd"] > 0: + return None,[] + reasons = [ + (row.get("final_guard_active"), "gimbal_brake"), + (row.get("near_edge") or row.get("edge_recovery_active"), "edge_margin_low"), + (row.get("visual_control_stale") or row.get("visual_control_coast"), "stale_visual"), + (not row.get("zoom_rearm_ready",True), "geometry_rearm"), + (not row.get("raw_zoom_needed",True) or row.get("normal_size_hold"), "size_controller"), + (not row.get("zoom_quality_ok",True), "quality_or_validation"), + (not row.get("followup_ready",True), "fov_cooldown"), + (not row.get("zoom_safe",True), "framing"), + (not row.get("zoom_angular_safe",True), "angular_limit"), + (not row.get("stable_ok",True), "not_centered_or_not_stable"), + ] + flags = [why for enabled,why in reasons if enabled] + predictive = row.get("predictive_decision") + if predictive and predictive.stop: + return predictive.reason,flags + return (flags[0] if flags else row.get("zoom_zero_reason") or st.get("zoom_useful_pulse_reason") or "unknown"),flags + + +def run(trace, profile, steering, revision, video_ms, isapi_ms, initial_fov=59., + plant_stress=False, put_rtt=False): + import numpy + import torch + + cam = trace["camera"]-11 + cfg = sim.Parameters(camera=trace["camera"], seconds=trace["duration"], + video_ms=video_ms, command_ms=isapi_ms, optics_report_ms=isapi_ms, + frame_hz=10, status_hz=4, steering_exponent=2, + pan_at_10=steering["pan"]["native10_equivalent_deg_s"], + tilt_at_10=steering["tilt"]["native10_equivalent_deg_s"], + zoom_at_10=profile["upper_gain" if plant_stress else "nominal_gain"] * FULL_SWEEP_LOG_RATE + / math.log(math.tan(math.radians(59/2))/math.tan(math.radians(2/2))) * math.log(42)/math.log(2)) + with ExitStack() as stack: + for owner,name in ((socket.socket,"connect"),(socket.socket,"connect_ex"), + (socket,"create_connection"),(torch,"load"),(torch.cuda,"_lazy_init")): + stack.enter_context(patch.object(owner,name,sim.deny)) + stack.enter_context(patch.object(torch.cuda,"is_available",return_value=False)) + pkg = ModuleType(sim.PKG) + pkg.__path__ = [str(sim.BASE/"geometry_candidate/ptz_tracker_modular"),str(sim.ROOT)] + stale_modules = {k:m for k,m in sys.modules.items() if k.startswith(sim.PKG+".")} + for k in stale_modules: + sys.modules.pop(k) + stack.enter_context(patch.dict(sys.modules,{sim.PKG:pkg})) + model = ModuleType(sim.PKG+".model") + model.model_shared,model.yolo_forward=object(),sim.deny + sys.modules[model.__name__] = model + sim.load(sim.PKG+".visual_control_probe",sim.SOURCE/"visual_control_probe.py") + control = sim.load(sim.PKG+".visual_control",sim.SOURCE/"visual_control.py") + pins = read(NEW_PINS if revision=="BT" else BUNDLE/"pins.json") + bundle = NEW if revision=="BT" else BUNDLE + candidate = NEW if revision=="BT" else CANDIDATE + for filename, values in pins.items(): + path = (bundle if filename == "zoom_controller.py" else candidate) / filename + assert sha256(path.read_bytes()).hexdigest() == values["after"], filename + sim.load(sim.PKG+".zoom_controller",bundle/"zoom_controller.py") + io_path, pp_path, status_path = [candidate/f for f in ("ptz_io.py","postprocess.py","status.py")] + io = sim.load(sim.PKG+".ptz_io",io_path) + status = sim.load(sim.PKG+".status",status_path) + pp = sim.load(sim.PKG+".postprocess",pp_path) + from ptz_tracker_modular import config,state,visual_tracker + values = read(sim.BASE/"zoom_continuation_candidate/runtime_policy_snapshot.json")["values"] + for key,value in values.items(): + stack.enter_context(patch.object(config,key,value,create=True)) + stack.enter_context(patch.object(config,"PTZ_CAM_IDS",[cam,*[x for x in config.PTZ_CAM_IDS if x!=cam]])) + transport = sim.load("native10_mock_transport",sim.ROOT/"tests/test_zoom_useful_transport.py") + before = sim.load("native10_control_fixture",sim.REPLAY) + before.np,before.torch,before.config,before.state = numpy,torch,config,state + before.ptz_io,before.pp,before.tracker,before.control,before.transport = io,pp,visual_tracker,control,transport + case = before.Replay() + case.setUp() + stack.callback(case.doCleanups) + f,st = case.fixture,case.st + cam_config = config.CAMERA_CONFIG[cam] + settings = dict(ptz_hfov_wide_deg=59.,ptz_hfov_tele_deg=2.) + settings["predictive_zoom"] = dict(profile) + if revision=="BT": + settings["predictive_zoom"].update( + tele_pan_native10_deg_s=cfg.pan_at_10, tele_tilt_native10_deg_s=cfg.tilt_at_10, + latency_horizon_ms=video_ms+isapi_ms/2, + ) + settings["predictive_zoom"].update(trace.get("tuning",{})) + stack.enter_context(patch.object(config,"PREDICTIVE_ZOOM_TELEMETRY_DIR",None,create=True)) + stack.enter_context(patch.dict(cam_config,settings)) + for key in ("ZOOM_STEP_PULSE_SEC","ZOOM_STEP_HARD_STOP_SEC"): + stack.enter_context(patch.object(config,key,values[key])) + plant = FlightCamera(cfg,float(cam_config.get("pan_sign",1)),float(cam_config.get("tilt_sign",-1)),trace,initial_fov) + initial = plant.project(0) + if initial is None: + raise ValueError("Invalid initial viewport") + st.update(last_bbox=initial,prev_cx=(initial[0]+initial[2])/2560, + prev_cy=(initial[1]+initial[3])/1440,stable_frames=0, + track_started_at=100.,last_seen=100.,visual_tracker_last_detector_at=100., + visual_tracker_last_zoom_detector_at=100.,visual_tracker_last_detector_conf=.8) + status._update_runtime_state(cam,None,None,99.99,zoom_ratio=plant.magnification,hfov_deg=plant.hfov) + st["vfov_deg"]=plant.vfov + # Replace only the asynchronous STOP thread's scheduler so it runs at + # this virtual tick; the actual priority STOP path still runs. + if revision in ("PR8","BT"): + class InlineThread: + def __init__(self, *, target, **_): self.target=target + def start(self): self.target() + original_thread = io.threading.Thread + def choose_thread(*args, **kwargs): + if kwargs.get("name") == "ptz-predictive-stop": + return InlineThread(**kwargs) + return original_thread(*args, **kwargs) + stack.enter_context(patch.object(io.threading,"Thread",choose_thread)) + t = 0. + commands, rows, decisions = [], [], [] + statuses, frames = deque(),deque() + request_starts, request_ids = [],set() + prior_lens = 0 + busy_until = 0.0 + + def send(_url, *, data=b"", **_kw): + nonlocal prior_lens, busy_until + axes = tuple(transport.xml_axis(str(data),name) for name in ("pan","tilt","zoom")) + # Model physical propagation and GET response separately. PUT + # success and STOP timers use the existing deterministic fixture. + issue = max(t,f.wall-100) if put_rtt else t + response = issue+isapi_ms/1000 if put_rtt else issue + propagation = isapi_ms/(2000 if put_rtt else 1000) + plant.commands.append((issue+propagation,axes)) + commands.append((round(issue,4),axes)) + if put_rtt: + busy_until = max(busy_until,response) + f.wall,f.mono=100+response,1000+response + request = st.get("zoom_step_pending_id") + if axes[2]>0 and prior_lens<=0 and request and request not in request_ids: + request_starts.append(round(response,4)) + request_ids.add(request) + prior_lens=axes[2] + return True,200,"" + + stack.enter_context(patch.object(io,"_ptz_put",side_effect=send)) + stack.enter_context(patch.object(io,"_ptz_put_priority_stop",side_effect=send)) + logger=logging.getLogger("PTZTracker") + saved=logger.level + logger.setLevel(logging.ERROR) + stack.callback(logger.setLevel,saved) + interval = video_ms//10 + stop_step = round(trace["duration"]*100) + optics_count=0 + for step in range(stop_step+1): + t=step/100 + now = max(t,busy_until) + f.wall,f.mono=100+now,1000+now + if step: + plant.step(t,.01) + if step%25==0: + statuses.append((step+isapi_ms//10,step,plant.magnification,plant.hfov,plant.vfov, + plant.pan_rate*plant.pan_sign,plant.tilt_rate*plant.tilt_sign)) + while statuses and statuses[0][0]<=step and t+1e-8>=busy_until: + _,origin,mag,hfov,vfov,pan,tilt=statuses.popleft() + measured_at=100+origin/100 + st.update(status_rate_at=measured_at,pan_rate_at=measured_at,tilt_rate_at=measured_at, + pan_rate_deg_s=pan,tilt_rate_deg_s=tilt,vfov_deg=vfov) + st["status_rate_history"] = (st.get("status_rate_history", []) + + [(measured_at,pan,tilt)])[-20:] + status._update_runtime_state(cam,None,None,f.wall,zoom_ratio=mag,hfov_deg=hfov, + zoom_age_sec=(step-origin)/100) + timer=st.get("zoom_step_stop_timer") + if (timer and timer.is_alive() and f.mono+1e-8>=st["zoom_step_stop_started_mono"]+timer.delay): + timer.fire() + if step%10==0: + frames.append((step+interval,step,plant.project(t))) + while frames and frames[0][0]<=step and t+1e-8>=busy_until: + due,origin,box=frames.popleft() + case.seq+=1 + track=visual_tracker.VisualTrackResult(box is not None,box,.8 if box else 0.,0,.8, + case.seq,"telemetry projection",case.seq) + age=(step-origin)/100 + captured_at=f.wall-age + visual=control.VisualSample(control.owner(st),case.seq,captured_at,track) + if origin%40==0 and box is not None: + detection=SimpleNamespace(boxes=before.Boxes([(*box,.8,0)])) + pp.postprocess_control(cam,case.frame,detection,True,case.seq,visual_sample=visual) + frame=SimpleNamespace(boxes=before.Boxes([])) + row=capture_control(pp,lambda: pp.postprocess_control( + cam,case.frame,frame,True,case.seq,visual_sample=visual,visual_only=True)) + reason,flags=blocker(row,st) + decisions.append(dict(t=t, zoom_block_reason=reason, flags=flags, + requested=row.get("zoom_cmd"), source_seq=case.seq, + return_line=row.get("return_line"), + actual_hfov=plant.hfov, + forecast_reason=getattr(row.get("predictive_decision"),"reason",None))) + if not state.ptz_cmd_q.empty(): + state.ptz_cmd_q.put_nowait(None) + io._ptz_worker() + optics_count=max(optics_count,st.get("zoom_in_pulse_count",0)) + if step%5==0 or step==stop_step: + box=plant.project(t,clipped=False) + if box is None: + error,margin,px=None,-.5,None + else: + x0,y0,x1,y1=box + error=max(abs((x0+x1)/2560-.5),abs((y0+y1)/1440-.5)) + margin=min(x0/1280,1-x1/1280,y0/720,1-y1/720) + px=(x1-x0,y1-y0) + rows.append(dict(t=t,hfov=plant.hfov,margin=margin,center_err=error,bbox_px=px, + mode=st.get("mode"),zoom=plant.active[2], + tele_steering=st.get("tele_steering_active",False))) + first=request_starts[0] if request_starts else None + prefix=[row for row in decisions if first is None or row["t"]<=first] + errors=[r["center_err"] for r in rows if r["center_err"] is not None] + tele = [r for r in rows if r["hfov"] < 18] + telemetry=dict( + trace={k:val for k,val in trace.items() if k!="knots"}, revision=revision, + tested_after_sha={file:row["after"] for file,row in pins.items()}, + video_ms=video_ms,isapi_ms=isapi_ms,initial_fov=initial_fov,plant_stress=plant_stress, + put_rtt_ms=isapi_ms if put_rtt else 0, + observed_model=dict(profile=profile,pan_at_native10=cfg.pan_at_10,tilt_at_native10=cfg.tilt_at_10), + metrics=dict(first_zoom_s=first,positive_starts=len(request_starts),optics_confirmed_max=optics_count, + hfov_min=round(min(r["hfov"] for r in rows),3), + tele_samples=len(tele), + tele_center_pct=round(100*sum(r["center_err"] is not None and + r["center_err"]<=.1 for r in tele)/len(tele),1) if tele else None, + tele_steering_samples=sum(bool(r["tele_steering"]) for r in tele), + fully_in_frame_pct=round(100*sum(r["margin"]>=0 for r in rows)/len(rows),1), + inside_center_10pct=round(100*sum(r["center_err"] is not None and r["center_err"]<=.1 for r in rows)/len(rows),1), + min_edge_margin=round(min(r["margin"] for r in rows),4), + peak_center_error=round(max(errors),4) if errors else None, + narrow_lens_active_samples=sum(r["hfov"]<=15 and r["zoom"]>0 for r in rows), + narrowing_with_edge_clip_samples=sum(r["margin"]<0 and r["zoom"]>0 for r in rows), + first_edge_clip_s=next((r["t"] for r in rows if r["margin"]<0),None), + positive_native=dict(Counter(axes[2] for _,axes in commands if axes[2]>0)), + max_bbox_short_px=max((min(r["bbox_px"]) for r in rows if r["bbox_px"]),default=0)), + zoom_block_reason_before_first=dict(Counter(r["zoom_block_reason"] for r in prefix if r["zoom_block_reason"])), + zoom_block_reason=dict(Counter(r["zoom_block_reason"] for r in decisions if r["zoom_block_reason"])), + simultaneous_flags_before_first=dict(Counter(x for r in prefix for x in r["flags"])), + request_starts=request_starts,commands=commands,rows=rows,decisions=decisions) + return telemetry + + +def main(): + parser=argparse.ArgumentParser(description=__doc__) + parser.add_argument("--camera",choices=sorted(FLIGHTS)) + parser.add_argument("--tune",action="store_true") + parser.add_argument("--problem",action="store_true") + parser.add_argument("--tuning-json",default="{}") + parser.add_argument("--smoke",action="store_true") + parser.add_argument("--edge-stress",action="store_true") + parser.add_argument("--put-rtt",action="store_true") + parser.add_argument("--counters",action="store_true", + help="repeat the first six seconds with per-tick controller diagnostics") + args=parser.parse_args() + import torch + torch.set_num_threads(1) + dest=OUT/"final" + dest.mkdir(exist_ok=True) + profiles=read(DATA/"profiles.json")["cameras"] + field,pulses=read(FIELD),read(PULSES) + steering=read(LAB/"field-calibration-report.json")["cameras"] + jobs=[] + for camera, keys in FLIGHTS.items(): + if args.camera and args.camera!=camera: + continue + traces=[trajectory(camera,next(e for e in pulses["episodes"][camera] if e["started_at"][11:19]==key),field) + for key in keys] + if args.problem: + if camera=="19": + ep=next(e for e in read(PR7)["episodes"][camera] if "05:49:18" in e["started_at"] or "12:49:18" in e["started_at"]) + trace=trajectory(camera,ep,field) + for video,api in ((180,40),(220,50),(250,60)): + jobs.append((trace,video,api,59.,False)) + continue + if args.tune: + trace = {**traces[0],"stress":True,"edge_stress":True, + "duration":min(5.,traces[0]["duration"])} + jobs.extend(((traces[-1],220,50,59.,False),(trace,250,60,6.,True), + ({**traces[0],"stress":True,"duration":7.},250,60,12.,True))) + continue + if args.put_rtt: + for video,api in ((180,40),(250,60)): + jobs.append(({**traces[1],"duration":min(7.,traces[1]["duration"])},video,api,59.,False)) + jobs.append(({**traces[0],"stress":True,"edge_stress":True, + "duration":min(5.,traces[0]["duration"])},video,api,6.,True)) + continue + if args.edge_stress: + trace = {**traces[0],"stress":True,"edge_stress":True, + "duration":min(5.,traces[0]["duration"])} + for video,api in ((180,40),(250,60)): + jobs.append((trace,video,api,6.,True)) + continue + if args.counters: + for trace in traces: + jobs.append(({**trace,"duration":min(6.,trace["duration"])},220,50,59.,False)) + continue + if args.smoke: + jobs.append((traces[0],220,50,59.,False)) + continue + for trace in traces: + for video,api in ((180,40),(220,50),(250,60)): + jobs.append((trace,video,api,59.,False)) + for fov in (12.,6.): + for video,api in ((180,40),(250,60)): + stress = {**traces[0],"stress":True,"duration":min(traces[0]["duration"],7.)} + jobs.append((stress,video,api,fov,True)) + summaries=[] + for trace,video,api,fov,stress in jobs: + cam=str(trace["camera"]) + versions=[] + for revision in ("PR8","BT"): + configured_trace = {**trace,"tuning":json.loads(args.tuning_json)} + result=run(configured_trace,profiles[cam]["replay_settings"],steering[cam]["fit"], + revision,video,api,fov,stress,put_rtt=args.put_rtt) + name=f"{cam}-{trace['episode'][11:19].replace(':','')}-{video}-{api}-{int(fov)}-{revision}{'-counters' if args.counters else ''}{'-edge' if args.edge_stress else ''}{'-put' if args.put_rtt else '-tune' if args.tune else '-problem' if args.problem else ''}.json" + (dest/name).write_text(json.dumps(result,allow_nan=False,separators=(",",":"))+"\n",encoding="utf-8") + versions.append({k:result[k] for k in ("revision","metrics","zoom_block_reason_before_first","tested_after_sha")}|{"trace_file":name}) + summaries.append(dict(camera=int(cam),episode=trace["episode"],video_ms=video,isapi_ms=api, + initial_fov=fov,stress=stress,put_rtt_ms=api if args.put_rtt else 0,runs=versions)) + print(cam,trace["episode"][11:19],video,api,fov, + [(x["metrics"]["first_zoom_s"],x["metrics"]["hfov_min"],x["metrics"]["tele_center_pct"], + x["metrics"]["min_edge_margin"],x["metrics"]["tele_steering_samples"]) for x in versions], + flush=True) + name = f"comparison{('-'+args.camera) if args.camera else ''}{'-smoke' if args.smoke else ''}{'-counters' if args.counters else ''}{'-edge' if args.edge_stress else ''}{'-put' if args.put_rtt else '-tune' if args.tune else '-problem' if args.problem else ''}.json" + (dest/name).write_text(json.dumps(summaries,indent=2,allow_nan=False)+"\n",encoding="utf-8") + + +if __name__=="__main__": + main() diff --git a/integration/ptz_predictive_zoom/run_local_replay.py b/integration/ptz_predictive_zoom/run_local_replay.py index bdd981a..69f7d8a 100644 --- a/integration/ptz_predictive_zoom/run_local_replay.py +++ b/integration/ptz_predictive_zoom/run_local_replay.py @@ -1,11 +1,11 @@ from pathlib import Path import json, os, sys -v,z,root,variant=map(Path,sys.argv[1:]) -candidate=v/"ptz_predictive_zoom_candidate_20261111" +v,z,root,variant=map(Path,sys.argv[1:5]) +candidate=Path(sys.argv[5]) if len(sys.argv)>5 else v/"ptz_broad_tele_20261111/candidate" pr7=v/"ptz_zoom_threshold_diagnostics_20261111" pins=json.loads((z/"pins.json").read_text()) -dest=v/f"predictive_zoom_regression_{variant}.json" +dest=candidate.parent/f"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" @@ -24,6 +24,9 @@ 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(" values=json.loads", """ stack.enter_context(patch.object( + config, 'PREDICTIVE_ZOOM_TELEMETRY_DIR', None, create=True)) + values=json.loads""") 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", @@ -31,11 +34,12 @@ if not before: "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"): + "test_priority_zoom_stop_does_not_override_full_stop", + "test_transport_floor_preserves_gimbal_and_rejects_lens"): 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( + Path({str(candidate.parent/"regression_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") diff --git a/integration/ptz_predictive_zoom/server.patch b/integration/ptz_predictive_zoom/server.patch index 70265c4..b2bbe29 100644 --- a/integration/ptz_predictive_zoom/server.patch +++ b/integration/ptz_predictive_zoom/server.patch @@ -1,14 +1,16 @@ --- a/postprocess.py +++ b/postprocess.py -@@ -10,6 +10,7 @@ +@@ -10,6 +10,9 @@ import cv2 from . import config, state, visual_control -+from .zoom_controller import ZoomFrame, controller_for_track, telemetry_tick ++from .zoom_controller import ( ++ ZoomFrame, controller_for_track, expire_calibration, flush_zoom_blocks, record_zoom_blocks, telemetry_tick, ++) from . import motion_detector from .utils import ( clipf, -@@ -30,6 +31,7 @@ +@@ -30,6 +33,7 @@ _zoom_command_lock, zoom_step_optics_delta, _zoom_speed_to_int, @@ -16,7 +18,7 @@ ) from .geom import ang_diff_signed, sector_contains from .patrol import ( -@@ -4597,6 +4599,7 @@ +@@ -4597,6 +4601,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, @@ -24,7 +26,7 @@ ) -> 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 @@ +@@ -4706,18 +4711,23 @@ _finish_zoom_step(st, now, stable_key=stable_key) return 0.0 if pulse_until <= 0.0: @@ -53,7 +55,16 @@ if not feedback_ready: st["zoom_followup_reason"] = str(feedback_reason) return 0.0 -@@ -8778,6 +8786,74 @@ +@@ -6553,6 +6563,8 @@ + if (now - last) < TRACK_END_RETURN_COOLDOWN_SEC: + return False + st["last_track_end_return_at"] = now ++ flush_zoom_blocks(config, cam_idx, st, now, final=True) ++ expire_calibration(config, cam_idx, st, now, final=True) + + # СРАЗУ ставим "за нято goto", чтобы второй источник не успел добавить ещё один goto + settle = float(getattr(config, "GOTO_SETTLE_SEC", 2.5)) +@@ -8778,6 +8790,76 @@ react_margin = clipf(react_margin_base + react_margin_gain * vel, react_margin_base, react_margin_max) near_react_edge = min_margin < react_margin @@ -65,6 +76,7 @@ + predictive = controller_for_track(st, cfg_ptz) + if predictive is not None: + decision_wall = time.time() ++ expire_calibration(config, cam_idx, st, decision_wall) + sample_is_current = bool( + visual_sample is not None + and visual_control.current( @@ -88,13 +100,14 @@ + 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) + ), ++ rearm_ready=_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) ++ or hfov_deg < predictive.cfg.tele_steering_start_fov + ), + ) + skip_wait = predictive.repeat_after_stop(st, decision_wall) @@ -128,7 +141,7 @@ zoom_quality_ok = bool( normal_zoom_quality_ok or probe_allowed -@@ -8819,6 +8895,21 @@ +@@ -8819,6 +8901,21 @@ pan_rate_deg_s=float(los_pan_rate_deg_s), tilt_rate_deg_s=float(los_tilt_rate_deg_s), ) @@ -150,7 +163,7 @@ 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 @@ +@@ -8832,6 +8929,9 @@ ) if approach_margin is not None and followup_ready: zoom_angular_safe = True @@ -160,7 +173,7 @@ 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 @@ +@@ -8952,6 +9052,7 @@ or bounded_identification_request ) zoom_rearm_ready = _zoom_revalidation_ready(st) @@ -168,7 +181,7 @@ can_zoom_in = ( zoom_safe and zoom_angular_safe -@@ -9099,6 +9194,14 @@ +@@ -9099,6 +9200,14 @@ zoom_zero_reason = "deadband" zoom_cmd = 0.0 @@ -183,7 +196,7 @@ zoom_after_size_minimum = zoom_cmd cmd_mag_est = max( abs(float(pan_speed) + float(pan_rate_ff)), -@@ -9151,7 +9254,7 @@ +@@ -9151,7 +9260,7 @@ center_err=float(center_err), moving_out=bool(moving_out), recent_zoom=bool(recent_zoom_steering), @@ -192,7 +205,46 @@ ) if before_recenter > 0.0 and zoom_cmd <= 0.0: zoom_zero_reason = "recenter" -@@ -9272,6 +9375,25 @@ +@@ -9159,21 +9268,27 @@ + out_edge_effective + or float(min_margin) < float(getattr(config, "Z_SAFETY_EDGE", 0.05)) + ) +- zoom_cmd, routine_zoom_out_blocked = _gate_routine_zoom_out_hold( ++ predictive_step_back = bool( ++ predictive_decision and predictive_decision.step_back ++ and not zoom_out_emergency and not st.get("zoom_step_pending") ++ and not st.get("lens_move_put_inflight") and not st.get("zoom_stop_obligation_active") ++ and float(st.get("last_ptz_physical_cmd", (0., 0., 0.))[2]) <= 0 ++ ) ++ if predictive_step_back: ++ zoom_cmd = -predictive.cfg.step_back_cmd ++ routine_zoom_out_blocked = False ++ else: ++ zoom_cmd, routine_zoom_out_blocked = _gate_routine_zoom_out_hold( ++ st, now, zoom_cmd, emergency=zoom_out_emergency, ++ ) ++ zoom_cmd = _gate_zoom_out_pulse( + st, +- now, ++ time.time() if predictive_step_back else now, + zoom_cmd, + emergency=zoom_out_emergency, + ) +- zoom_cmd = _gate_zoom_out_pulse( +- st, +- now, +- zoom_cmd, +- emergency=zoom_out_emergency, +- ) +- if float(zoom_cmd) < 0.0 and float(prev_z) >= 0.0 and not bool( +- st.get("zoom_rearm_required", False) +- ): ++ if (not predictive_step_back and float(zoom_cmd) < 0.0 and float(prev_z) >= 0.0 ++ and not st.get("zoom_rearm_required", False)): + _arm_zoom_revalidation(st, now, "zoom_out", cam_idx=cam_idx) + pre_ramp_zoom_cmd = float(zoom_cmd) + if zoom_cmd > 0.0: +@@ -9272,6 +9387,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 @@ -218,7 +270,7 @@ before_shape = zoom_cmd zoom_cmd, useful_reason = _shape_useful_zoom_pulse( zoom_cmd, -@@ -9299,6 +9421,21 @@ +@@ -9299,6 +9433,23 @@ requested_native=timing_native, rejection=response_rejection, ) @@ -232,15 +284,17 @@ + float(st["zoom_step_pending_hard_sec"]), + ) + if predictive_decision and ( -+ predictive_decision.stop or (predictive_decision.reason == "size_reached" -+ and pulse_family == "normal") ++ predictive_decision.stop or predictive_decision.reason in ("min_hfov_limit", "tele_calibration_wait") ++ or (predictive_decision.reason == "edge_margin_low" ++ and hfov_deg <= predictive.cfg.tele_pulse_fov) ++ 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 @@ +@@ -9321,7 +9472,10 @@ optics_observed_now=zoom_gate_now, stop_trace=gate_stop, continuation_trace=continuation_trace, @@ -252,7 +306,7 @@ if gate_stop: zoom_zero_reason = gate_stop[-1] decision_generation = st.get("ptz_generation") -@@ -9380,6 +9520,12 @@ +@@ -9380,6 +9534,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) @@ -265,7 +319,30 @@ adj_pan, adj_tilt = _merge_gimbal_rate_feedforward( st, now, -@@ -9844,6 +9990,14 @@ +@@ -9398,6 +9558,22 @@ + precenter_optics_at = st.get("zoom_status_at") + precenter_measured_hfov = st.get("hfov_deg") + status_observed_now = time.time() ++ tele_rate_history = tuple(st.get("status_rate_history", ())) if predictive else () ++ if predictive and predictive_frame and predictive.optics.actual is not None: ++ st["tele_steering_active"] = False ++ aligned = next((x for x in reversed(tele_rate_history) ++ if 0 <= now - x[0] <= predictive.cfg.frame_age_s), None) ++ if aligned and status_observed_now - aligned[0] <= predictive.cfg.status_age_s: ++ steering = predictive.tele_aim( ++ predictive_frame, status_observed_now, ++ (float(aligned[1]) * _PAN_SIGN, float(aligned[2]) * _TILT_SIGN), ++ predictive_decision.ff_deg_s if predictive_decision else (0., 0.), ++ ) ++ if steering is not None: ++ (pan_tele, tilt_tele), blend = steering ++ adj_pan += blend * (pan_tele - adj_pan) ++ adj_tilt += blend * (tilt_tele - adj_tilt) ++ st["tele_steering_active"] = True + guard_status_fresh = bool( + guard_status_at > 0.0 + and 0.0 <= (status_observed_now - guard_status_at) +@@ -9844,12 +10020,53 @@ ("brake", final_guard_active), ) if blocked) zoom_decision = (source_seq, decision_request, zoom_zero_reason, "+".join(blockers) or "-") @@ -277,31 +354,83 @@ + ) + if predictive_decision.stop and predictive.request is not None: + stop_predictive_zoom_async(cam_idx, predictive, predictive_decision.reason) ++ if getattr(config, "PREDICTIVE_ZOOM_TELEMETRY_DIR", "logs/ptz_zoom"): ++ blocked = {} ++ if zoom_cmd <= 0.0 and raw_zoom_needed: ++ if not stable_ok: ++ blocked["centering"] = {"error": center_err, "limit": lock_gate, "stable_frames": st.get("stable_frames")} ++ if not zoom_safe: ++ blocked["bbox_check"] = {"margin": zoom_pred_margin, "limit": zoom_center_max, ++ "bbox_px": [x2-x1, y2-y1]} ++ if not zoom_angular_safe and followup_ready: ++ blocked["angle_tol"] = {"pan_deg": los_pan_angle_deg, "tilt_deg": los_tilt_angle_deg, ++ "limit_deg": angular_max} ++ if near_edge or edge_recovery_active or ( ++ predictive_decision and predictive_decision.reason in ("edge_margin_low", "edge_acceleration")): ++ blocked["edge_margin"] = {"measured": min_margin, ++ "predicted": predictive_decision.margin if predictive_decision else None, ++ "required": predictive_decision.edge_margin_required if predictive_decision else None} ++ if predictive_decision and predictive_decision.reason == "min_hfov_limit": ++ blocked["min_hfov_limit"] = {"hfov": hfov_deg, "minimum": predictive.cfg.min_allowed_hfov_deg} ++ if st.get("lens_move_put_inflight"): ++ blocked["put_inflight"] = {"kind": st.get("lens_move_put_kind"), "request": decision_request} ++ with _zoom_command_lock(cam_idx): ++ record_zoom_blocks(config, cam_idx, st, time.time(), blocked) dispatch_kwargs = {"force": force_track_ptz} if zoom_decision is not None: dispatch_kwargs["zoom_decision"] = zoom_decision +- _dispatch_owned_zoom_tracking( +- cam_idx, _PAN_SIGN * adj_pan, _TILT_SIGN * adj_tilt, zoom_cmd, **dispatch_kwargs +- ) ++ if predictive_step_back and zoom_cmd < 0.0: ++ with _zoom_command_lock(cam_idx): ++ if not visual_control.current(cam_idx, visual_sample, time.time(), max_age=.25): ++ return ++ if not st.get("zoom_rearm_required"): ++ _arm_zoom_revalidation_locked(st, time.time(), "tele_step_back") ++ dispatch_kwargs["force"] = True ++ _dispatch_owned_zoom_tracking( ++ cam_idx, _PAN_SIGN * adj_pan, _TILT_SIGN * adj_tilt, zoom_cmd, **dispatch_kwargs ++ ) ++ else: ++ _dispatch_owned_zoom_tracking( ++ cam_idx, _PAN_SIGN * adj_pan, _TILT_SIGN * adj_tilt, zoom_cmd, **dispatch_kwargs ++ ) + if visual_control.enabled(): + dispatch_at = time.monotonic() + samples = st.setdefault('control_dispatch_samples', []) --- a/ptz_io.py +++ b/ptz_io.py -@@ -14,6 +14,7 @@ +@@ -14,6 +14,9 @@ from requests.adapters import HTTPAdapter from . import config, state -+from .zoom_controller import transport_accepted, transport_confirmed, transport_stopped ++from .zoom_controller import ( ++ record_zoom_blocks, transport_accepted, transport_confirmed, transport_stopped, transport_zoom_block_reason, ++) 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 @@ +@@ -550,6 +553,7 @@ + cmd: PTZCommand, + now: float, + *, native_cmd: int = 0, ++ put_rtt_s: float | None = None, + ) -> bool: + """Anchor a pending +zoom window to the successful camera HTTP PUT.""" + if not _pending_zoom_request_matches(st, cmd): +@@ -582,6 +586,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, ++ deadline_origin + hard_sec, put_rtt_s, + ) logger.info( "[ZOOM DELIVERY] cam=%s request=%s kind=%s physical start seq=%s pulse=%.3fs hard=%.3fs", cam_idx, -@@ -744,6 +749,8 @@ +@@ -744,6 +752,8 @@ float(st.get("zoom_recenter_until", 0.0) or 0.0), float(now) + float(getattr(config, "ZOOM_STEP_RECENTER_SEC", 0.14)), ) @@ -310,7 +439,16 @@ logger.info( "[ZOOM DELIVERY] cam=%s request=%s optics confirmed source=%s zstep=%s", cam_idx, -@@ -922,6 +929,12 @@ +@@ -800,6 +810,8 @@ + reason: str, + stop_axes: Optional[tuple[int, int, int]] = None, + stop_command: Optional[MoveTuple] = None, ++ stop_issued_at: float | None = None, ++ stop_rtt_s: float | None = None, + ) -> bool: + """Finalize transport delivery; charge a pulse only with optical evidence.""" + if not bool(st.get("zoom_step_pending", False)): +@@ -922,6 +934,13 @@ + max(0.0, float(getattr(config, "ZOOM_STEP_RETRY_COOLDOWN_SEC", 0.12))), ) @@ -319,16 +457,39 @@ + 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), ++ duration_s=delivered, stop_issued_at=stop_issued_at, stop_rtt_s=stop_rtt_s, + ) 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 +@@ -1257,6 +1276,7 @@ + float(getattr(config, "PTZ_LENS_STOP_DIRECT_TIMEOUT_SEC", 0.75)), + ), + ) ++ stop_issued_at, stop_issued_mono = time.time(), time.monotonic() + ok, code, body = _ptz_put_priority_stop( + url, + data=xml, +@@ -1266,6 +1286,7 @@ + verify=verify, + ) + now = time.time() ++ stop_rtt_s = time.monotonic() - stop_issued_mono + st["zoom_stop_obligation_attempts"] = int( + st.get("zoom_stop_obligation_attempts", 0) or 0 + ) + 1 +@@ -1328,6 +1349,7 @@ + now, + reason=f"priority_stop:{reason}", + stop_axes=(pan_i, tilt_i, 0), ++ stop_issued_at=stop_issued_at, stop_rtt_s=stop_rtt_s, + ) + _cancel_zoom_step_watchdog(st) + if ordering_inflight: +@@ -2121,6 +2143,35 @@ 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): @@ -356,12 +517,82 @@ + 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: + """ + Жёсткий стоп PTZ. +@@ -3308,6 +3359,23 @@ + + reserve_wall = time.time() + reserve_mono = time.monotonic() ++ camera_profile = config.CAMERA_CONFIG.get(cam_idx, {}) ++ floor_block = (transport_zoom_block_reason( ++ st, camera_profile, reserve_wall, _zoom_step_timing(st)[1], ++ ) if int(zoom_i) > 0 else None) ++ if floor_block: ++ if pending_matches and not st.get("zoom_step_physical_started_at"): ++ _clear_pending_zoom_delivery(st, reserve_wall, reason=floor_block, force=True) ++ pending_matches = False ++ if st.get("mode") == "TRACK": ++ record_zoom_blocks(config, cam_idx, st, reserve_wall, { ++ floor_block: {"hfov": st.get("hfov_deg"), "stage": "pre_put", ++ "request": _cmd_move_seq(cmd)}, ++ }) ++ zoom_i = 0 ++ if bool(getattr(config, "PTZ_ZOOM_STOP_FORCE_AXES_ZERO", True)): ++ pan_i = tilt_i = 0 ++ xml = f"{pan_i}{tilt_i}0" + if int(zoom_i) > 0 and pending_matches and float( + st.get("zoom_step_physical_started_at", 0.0) or 0.0 + ) <= 0.0: +@@ -3481,6 +3549,7 @@ + sent_nonzero_move = bool( + int(pan_i) != 0 or int(tilt_i) != 0 or int(zoom_i) != 0 + ) ++ put_call_at, put_call_mono = time.time(), time.monotonic() + ok, code, body = _ptz_put( + url, + data=xml, +@@ -3489,6 +3558,7 @@ + timeout=timeout, + verify=verify, + ) ++ put_rtt_s = time.monotonic() - put_call_mono + if ok: + break + if attempt < retry_count: +@@ -3560,7 +3630,7 @@ + st, + cmd, + physical_now, +- native_cmd=int(zoom_i), ++ native_cmd=int(zoom_i), put_rtt_s=put_rtt_s, + ) + elif int(zoom_i) <= 0: + if float(previous_physical_zoom) > 0.0: +@@ -3571,6 +3641,8 @@ + reason="physical_stop", + stop_axes=(pan_i, tilt_i, zoom_i), + stop_command=cmd, ++ stop_issued_at=put_call_at if int(zoom_i) == 0 else None, ++ stop_rtt_s=put_rtt_s, + ) + elif ( + int(zoom_i) == 0 --- a/status.py +++ b/status.py -@@ -170,6 +170,13 @@ +@@ -14,6 +14,9 @@ + + + # ---------------- Публичные утилиты статуса ---------------- ++from .zoom_controller import observe_calibration ++ ++ + def get_cached_angles(cam_id: int) -> Tuple[Optional[float], Optional[float], Optional[float]]: + """ + Вернёт (pan_deg, tilt_deg, t_epoch) из кэша, если есть (иначе None). +@@ -170,6 +173,14 @@ 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 @@ -372,6 +603,7 @@ + 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)) ++ observe_calibration(config, cam_id, st, 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 "") diff --git a/integration/ptz_predictive_zoom/test_broad_tele.py b/integration/ptz_predictive_zoom/test_broad_tele.py new file mode 100644 index 0000000..49dc680 --- /dev/null +++ b/integration/ptz_predictive_zoom/test_broad_tele.py @@ -0,0 +1,120 @@ +import json +from pathlib import Path +from tempfile import TemporaryDirectory +import unittest +from types import SimpleNamespace +from unittest.mock import patch + +import zoom_controller as z +from fit_tele_profile import summarize +from test_zoom_controller import frame, tick + + +class BroadTeleTests(unittest.TestCase): + def make(self, fov, **settings): + c = z.ZoomController(z.ZoomConfig(.5, .9, **settings), (3, 100.)) + c.optics.observe(99.99, fov, 100.) + return c + + def test_broad_converging_entry_beyond_static_band(self): + ctrl = self.make(59) + for k, x in enumerate((.173, .167, .161)): + decision = tick(ctrl, frame(100 + .08*k, k+1, + offset=(x,0), detector_at=99.99)) + self.assertEqual(decision.reason, "fast_start") + self.assertEqual(decision.gate, "initial") + self.assertGreater(decision.margin, ctrl.cfg.edge_margin) + ctrl = self.make(59) + for k in range(3): + decision = tick(ctrl, frame(100+.08*k,k+1,offset=(.161,0),detector_at=99.99)) + self.assertEqual(decision.reason, "not_centered") + + def test_margin_curve_and_floor_keep_legacy_wire_from_bypassing(self): + ctrl = self.make(10.6) + self.assertAlmostEqual(ctrl.cfg.margin_for(59), .16) + self.assertAlmostEqual(ctrl.cfg.margin_for(12), .32) + self.assertGreater(ctrl.cfg.margin_for(15), ctrl.cfg.margin_for(20)) + decision = tick(ctrl, frame(100.08, 1, offset=(.2,0), detector_at=99.99)) + self.assertEqual(decision.reason, "min_hfov_limit") + self.assertIsNone(decision.window) + self.assertTrue(decision.step_back) + profile = {"predictive_zoom": {"nominal_gain": .5, "upper_gain": .9}} + self.assertEqual(z.transport_zoom_block_reason({"predictive_zoom_hfov":(99.9,10.6)}, + profile,100.,.12),"min_hfov_limit") + self.assertIsNone(z.transport_zoom_block_reason({"predictive_zoom_hfov":(99.9,59.)}, + profile,100.,.40)) + self.assertEqual(z.transport_zoom_block_reason({"predictive_zoom_hfov":(98.9,59.)}, + profile,100.,.40),"min_hfov_limit") + self.assertIsNone(z.transport_zoom_block_reason({}, {}, 100., .40)) + + def test_latency_horizon_moves_prediction_in_residual_direction(self): + def command(horizon): + ctrl = self.make(14, latency_horizon_ms=horizon) + f = frame(100.20,2,offset=(.025, -.03),detector_at=99.99) + ctrl.latest_pose = (2,(.34,-.20),(.5,-.2),True) + result=ctrl.tele_aim(f,100.25,(0.,0.)) + self.assertIsNotNone(result) + return result[0] + slow,fast = command(120),command(245) + self.assertGreater(fast[0],slow[0]) + self.assertLess(fast[1],slow[1]) + + def test_measured_pulse_reports_settled_delta_rtt_and_tail(self): + ctrl = self.make(14.5) + st=dict(predictive_zoom_controller=ctrl,mode="TRACK",tracking_active=True, + ptz_generation=3,track_started_at=100., + predictive_zoom_request=1,zoom_step_start_status_at=99.99, + zoom_step_start_hfov_deg=14.5,last_ptz_physical_cmd=(0.,0.,0.)) + with patch.object(z,"_episode_telemetry") as submit: + cfg=SimpleNamespace() + z.transport_accepted(cfg,8,st,1,10,100.03,100.08,100.21,.05) + ctrl.optics.observe(100.15,13.9,100.15) + z.transport_stopped(cfg,8,st,1,100.21,reason="priority_stop:watchdog", + confirmed=True,clean=True,duration_s=.18,stop_issued_at=100.16, + stop_rtt_s=.05) + self.assertTrue(ctrl.calibration_hold(100.22)) + for at,fov in ((100.25,13.6),(100.48,13.4),(100.63,13.4)): + z.observe_calibration(cfg,8,st,at,fov,at) + self.assertFalse(ctrl.calibration_hold(100.64)) + row=submit.call_args.kwargs + self.assertEqual(row["outcome"],"settled") + self.assertEqual(row["put_rtt_ms"],50.) + self.assertEqual(row["stop_rtt_ms"],50.) + self.assertAlmostEqual(row["delta_hfov_deg"],-1.1) + self.assertAlmostEqual(row["post_stop_tail_deg"],.2) + self.assertAlmostEqual(row["stop_bracket_narrowing_deg"],.5) + self.assertGreater(row["effective_tele_log_rate_s"],0.) + + def test_offline_fit_excludes_censored_rows_and_duplicate_requests(self): + base=dict(event="native10_pulse_calibration", cam=8, generation=3, + outcome="settled", effective_tele_log_rate_s=.4, HFOV_before=14., + HFOV_after=13., put_rtt_ms=50., stop_rtt_ms=55., put_at=101., + stop_status_at=101.11, stop_issued_at=101.16, HFOV_stop_sample=13.2) + records = [dict(base, track_start_utc=100.+i//2, request=i+1) for i in range(3)] + records += [records[0], dict(base, track_start_utc=102., request=5, outcome="next_pulse")] + with TemporaryDirectory() as tmp: + path=Path(tmp)/"episode.jsonl" + path.write_text("".join(json.dumps(x)+"\n" for x in records),encoding="utf-8") + result=summarize([path]) + self.assertFalse(result["production_enable"]) + camera=result["cameras"]["8"] + self.assertEqual(camera["qualified"],3) + self.assertEqual(camera["candidate_for_review"]["qualified_stop_brackets"],3) + + def test_episode_counts_and_snapshot_values(self): + cfg=SimpleNamespace() + st=dict(ptz_generation=2,track_started_at=100.) + with patch.object(z,"_episode_telemetry") as emit: + z.record_zoom_blocks(cfg,8,st,100.1,{"angle_tol":{"pan_deg":4.}}) + z.record_zoom_blocks(cfg,8,st,100.2,{"angle_tol":{"pan_deg":4.2},"put_inflight":{"request":2}}) + self.assertEqual(emit.call_count,1) + z.flush_zoom_blocks(cfg,8,st,100.3,final=True) + last=emit.call_args.kwargs + self.assertEqual(last["zoom_block_reason"],{"angle_tol":2,"put_inflight":1}) + self.assertEqual(last["last_values"]["angle_tol"]["pan_deg"],4.2) + self.assertTrue(last["final"]) + self.assertNotIn("episode_zoom_blocks",st) + + +if __name__=="__main__": + unittest.main() diff --git a/integration/ptz_predictive_zoom/test_zoom_controller.py b/integration/ptz_predictive_zoom/test_zoom_controller.py index 0dd3488..d78a1b4 100644 --- a/integration/ptz_predictive_zoom/test_zoom_controller.py +++ b/integration/ptz_predictive_zoom/test_zoom_controller.py @@ -79,7 +79,7 @@ class PredictiveZoomTests(unittest.TestCase): pulses.append(t) else: if decision.stop: - self.assertEqual(decision.reason, "size_reached") + self.assertIn(decision.reason, ("size_reached", "edge_margin_low", "min_hfov_limit")) c.stopped(active[0], now, clean=False, confirmed=False) last_stop = now stops.append(now) diff --git a/integration/ptz_predictive_zoom/track_integration.py b/integration/ptz_predictive_zoom/track_integration.py index 437cfa0..e3549dd 100644 --- a/integration/ptz_predictive_zoom/track_integration.py +++ b/integration/ptz_predictive_zoom/track_integration.py @@ -153,4 +153,20 @@ def build_track_cases(fixture, pp, ptz_io, config, state, status): def test_priority_zoom_stop_does_not_override_full_stop(self): self.assert_early_stop_axes(full_stop=True) + def test_transport_floor_preserves_gimbal_and_rejects_lens(self): + f, st = self.fixture, self.st + 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)) + status._update_runtime_state(self.cam, None, None, f.wall-.05, zoom_ratio=6., hfov_deg=10.6) + command = pp._gate_zoom_in_pulse(st, f.wall, .10, True, pulse_floor=.10, pulse_timing=(.12, .18)) + self.assertGreater(command, 0.) + f._enqueue(command, pan=.12, tilt=-.08) + f._flush() + self.assertFalse(st.get("zoom_step_pending")) + axes = tuple(fixture.transport.xml_axis(f.sent[-1], axis) for axis in ("pan", "tilt", "zoom")) + self.assertEqual(axes, (12, -8, 0)) + return PredictiveTrack diff --git a/integration/ptz_predictive_zoom/zoom_controller.py b/integration/ptz_predictive_zoom/zoom_controller.py index a602736..57ff4ee 100644 --- a/integration/ptz_predictive_zoom/zoom_controller.py +++ b/integration/ptz_predictive_zoom/zoom_controller.py @@ -19,6 +19,22 @@ class ZoomConfig: 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 @@ -48,6 +64,8 @@ class ZoomConfig: 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 ( @@ -60,6 +78,17 @@ class ZoomConfig: 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 @@ -74,6 +103,14 @@ class ZoomConfig: 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))) @@ -182,6 +219,7 @@ class ZoomFrame: detector_conf: float | None geometry_ok: bool los_valid: bool + rearm_ready: bool = True @dataclass(frozen=True) @@ -191,6 +229,9 @@ class ZoomDecision: 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: @@ -206,6 +247,8 @@ class ZoomController: 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: @@ -275,8 +318,42 @@ class ZoomController: 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 @@ -289,17 +366,17 @@ class ZoomController: math.isfinite(frame.tracker_score) and frame.tracker_score >= cfg.tracker_score ): self.history.clear() - return ZoomDecision("low_tracker_score", stop=moving) + 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) + return ZoomDecision("stale_or_geometry", stop=moving, gate=gate) if not self.optics.fresh(now): - return ZoomDecision("fov_cooldown", stop=moving) + 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) + 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): @@ -315,15 +392,19 @@ class ZoomController: 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) + 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 = cfg.steering_delay_s + (now - frame.at) + remaining + 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), @@ -334,48 +415,80 @@ class ZoomController: 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) + 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 < cfg.edge_watch_margin and outward_rate > 0 and outward_accel > cfg.edge_accel: - return ZoomDecision("edge_acceleration", stop=True, margin=margin) + 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 + 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) + 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) + 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) + return ZoomDecision("size_reached", stop=True, margin=margin, gate=gate) if now >= self.optics.active[1]: - return ZoomDecision("fov_cooldown", stop=True, margin=margin) + 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(cfg.ff_gain * cfg.log_rate * math.sin(math.radians(a)) + 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) + 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) + 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) + return ZoomDecision("fov_cooldown", margin=margin, gate=gate, + edge_margin_required=required) if not detector_fresh: - return ZoomDecision("waiting_yolo", margin=margin) + 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) - 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) + 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: @@ -439,10 +552,21 @@ def controller_for_track(st, camera_config): return current -def transport_accepted(config, camera, st, request, native, put_at, ack_at, hard_end): +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, @@ -481,9 +605,24 @@ def transport_confirmed(config, st, request, at): controller.stop_cooldowns = _cooldowns(st) -def transport_stopped(config, camera, st, request, at, *, reason, confirmed, clean): +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) or controller.request != request: + 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) @@ -498,15 +637,19 @@ _telemetry_lock = Lock() def _telemetry(config, camera, controller, event, at, **fields): - directory = getattr(config, "PREDICTIVE_ZOOM_TELEMETRY_DIR", None) - if directory is None: + _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, controller.owner, event, at, **fields, dropped_before=log.dropped) + log.submit(camera, owner, event, at, **fields, dropped_before=log.dropped) def telemetry_tick(config, camera, controller, frame, decision, zoom, legacy_reason, at): @@ -521,4 +664,130 @@ def telemetry_tick(config, camera, controller, frame, decision, zoom, legacy_rea 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) + 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)