commit b5372e74ea4a8f419392bc56abc4e8d4b5bf2226 Author: Ярослав Карташов Date: Mon Feb 16 11:35:43 2026 +0700 Actual versions for electro-pribory diff --git a/.idea/.gitignore b/.idea/.gitignore new file mode 100644 index 0000000..13566b8 --- /dev/null +++ b/.idea/.gitignore @@ -0,0 +1,8 @@ +# Default ignored files +/shelf/ +/workspace.xml +# Editor-based HTTP Client requests +/httpRequests/ +# Datasource local storage ignored files +/dataSources/ +/dataSources.local.xml diff --git a/.idea/CHAMELEON.iml b/.idea/CHAMELEON.iml new file mode 100644 index 0000000..a1d6e1f --- /dev/null +++ b/.idea/CHAMELEON.iml @@ -0,0 +1,12 @@ + + + + + + + + + + \ No newline at end of file diff --git a/.idea/inspectionProfiles/Project_Default.xml b/.idea/inspectionProfiles/Project_Default.xml new file mode 100644 index 0000000..9351f02 --- /dev/null +++ b/.idea/inspectionProfiles/Project_Default.xml @@ -0,0 +1,47 @@ + + + + \ No newline at end of file diff --git a/.idea/inspectionProfiles/profiles_settings.xml b/.idea/inspectionProfiles/profiles_settings.xml new file mode 100644 index 0000000..105ce2d --- /dev/null +++ b/.idea/inspectionProfiles/profiles_settings.xml @@ -0,0 +1,6 @@ + + + + \ No newline at end of file diff --git a/.idea/misc.xml b/.idea/misc.xml new file mode 100644 index 0000000..f9fcfbc --- /dev/null +++ b/.idea/misc.xml @@ -0,0 +1,4 @@ + + + + \ No newline at end of file diff --git a/.idea/modules.xml b/.idea/modules.xml new file mode 100644 index 0000000..adc3842 --- /dev/null +++ b/.idea/modules.xml @@ -0,0 +1,8 @@ + + + + + + + + \ No newline at end of file diff --git a/.idea/vcs.xml b/.idea/vcs.xml new file mode 100644 index 0000000..94a25f7 --- /dev/null +++ b/.idea/vcs.xml @@ -0,0 +1,6 @@ + + + + + + \ No newline at end of file diff --git a/ptz_tracker_modular/README.md b/ptz_tracker_modular/README.md new file mode 100644 index 0000000..0312eef --- /dev/null +++ b/ptz_tracker_modular/README.md @@ -0,0 +1,33 @@ +# PTZ Tracker — Modular Version + +## Structure +``` +ptz_tracker_modular/ +├── app.py # entrypoint (async main) +├── calibration.py # preset/sector and pan-sign calibration +├── config.py # constants + camera config +├── events.py # alarms, detection event plumbing +├── geom.py # angular math and sector helpers +├── heartbeat.py # UDP heartbeat +├── model.py # YOLO loading + forward +├── patrol.py # patrol logic (bounded/smooth + sync) +├── postprocess.py # PTZ control + zoom + notify_detected +├── preview.py # JPEG encode and preview publishing +├── ptz_io.py # PTZ HTTP session + worker + bounded move +├── sector.py # sector init and autocalculation +├── state.py # runtime shared state +├── status.py # PTZ status poller +├── streaming.py # WS servers, MicroBatcher, CPP bridges +└── __init__.py +``` + +## Run +```bash +python -m ptz_tracker_modular.app +``` + +Optional env: +- `YOLO_WEIGHTS=/path/to/weights.pt` +- `CAM__PASSWORD=...` for every camera id + +Dependencies: `opencv-python`, `torch`, `ultralytics`, `websockets`, `requests`, `numpy`. diff --git a/ptz_tracker_modular/__init__.py b/ptz_tracker_modular/__init__.py new file mode 100644 index 0000000..5bb534f --- /dev/null +++ b/ptz_tracker_modular/__init__.py @@ -0,0 +1 @@ +# package diff --git a/ptz_tracker_modular/alarms.py b/ptz_tracker_modular/alarms.py new file mode 100644 index 0000000..87cbecd --- /dev/null +++ b/ptz_tracker_modular/alarms.py @@ -0,0 +1,588 @@ +# alarms.py + +from __future__ import annotations + +import asyncio +import json +import logging +from datetime import datetime, timezone +from typing import Any, Dict, Optional, List, Tuple + +# ====== Импорты локальных модулей с fallback (пакет/скрипт) ====== +try: + from . import config as _config + from . import state as _state # ВАЖНО: модуль, а не значения + # для «живого» чтения камер из cameras.toml + try: + from . import cameras_io # type: ignore + except Exception: + cameras_io = None # type: ignore +except Exception: # запуск из корня проекта + import config as _config # type: ignore + import state as _state # type: ignore + try: + import cameras_io # type: ignore + except Exception: + cameras_io = None # type: ignore + +config = _config +logger = logging.getLogger("PTZTracker") + +# ===================================================================== +# Параметры (из config.py, с безопасными дефолтами) +# ===================================================================== + +# Пороги гистерезиса по кадрам +ALARM_COUNT_ON: int = int(getattr(config, "ALARM_COUNT_ON", 20)) +ALARM_COUNT_OFF: int = int(getattr(config, "ALARM_COUNT_OFF", 80)) + +# WS для одно-битного флага video_alarm +ALARM_WS_URL: str = getattr(config, "ALARM_WS_URL", "ws://127.0.0.1/api/core/ws") + +# HTTP bulk (всегда http/https, без /ws в конце) +ALARM_HTTP_ENABLE: bool = bool(getattr(config, "ALARM_HTTP_ENABLE", True)) +ALARM_HTTP_BASE: str = getattr(config, "ALARM_HTTP_BASE", "http://127.0.0.1/api/core") +# bulk путь БЕЗ {mac} +ALARM_HTTP_PATH: str = getattr(config, "ALARM_HTTP_PATH", "/videodata") + +# минутный heartbeat (тот же bulk формат) +ALARM_HEARTBEAT_SEND_EMPTY: bool = bool(getattr(config, "ALARM_HEARTBEAT_SEND_EMPTY", True)) +ALARM_HEARTBEAT_SEC: int = int(getattr(config, "ALARM_HEARTBEAT_SEC", 60)) + +# Микробатчинг очередей +HTTP_BATCH_MAX: int = int(getattr(config, "ALARM_HTTP_BATCH_MAX", 16)) +HTTP_BATCH_MS: int = int(getattr(config, "ALARM_HTTP_BATCH_MS", 120)) +WS_BATCH_MAX: int = int(getattr(config, "ALARM_WS_BATCH_MAX", 16)) +WS_BATCH_MS: int = int(getattr(config, "ALARM_WS_BATCH_MS", 120)) + +# ===================================================================== +# Очереди +# ===================================================================== + +def _ensure_ws_queue() -> asyncio.Queue: + if _state.alarm_queue is None: + _state.alarm_queue = asyncio.Queue() + logger.debug("[ALARM] created alarm_queue") + return _state.alarm_queue + +_http_queue: Optional[asyncio.Queue] = None +def _ensure_http_queue() -> asyncio.Queue: + global _http_queue + if _http_queue is None: + _http_queue = asyncio.Queue() + logger.debug("[ALARM][HTTP] created _http_queue") + return _http_queue + +# ===================================================================== +# Утилиты +# ===================================================================== + +def _now_iso_ms_utc() -> str: + return ( + datetime.now(timezone.utc) + .isoformat(timespec="milliseconds") + .replace("+00:00", "Z") + ) + +def _norm_mac(s: str) -> str: + s = (s or "").strip().upper().replace("-", ":") + parts = s.split(":") + if len(parts) != 6 or any(len(p) != 2 for p in parts): + raise ValueError(f"Bad MAC: {s!r}") + return s + +def _get_cam_mac(cam_idx: int) -> Optional[str]: + # 1) предпочтительно читаем cameras_io (актуально по файлу) + if cameras_io is not None: + try: + rec = cameras_io.get_all().get(int(cam_idx)) # type: ignore[attr-defined] + mac = (rec or {}).get("mac") + if isinstance(mac, str) and mac: + return _norm_mac(mac) + except Exception: + pass + # 2) фоллбэк — config (если кто-то синхронизирует его отдельно) + try: + mac = (_config.CAMERA_CONFIG[int(cam_idx)]).get("mac") # type: ignore[attr-defined] + except Exception: + mac = None + if not isinstance(mac, str) or not mac: + return None + try: + return _norm_mac(mac) + except Exception: + return None + +def _build_swagger_camera(mac: str, alarm: bool, status: bool = True) -> Dict[str, Any]: + return {"mac": _norm_mac(mac), "status": bool(status), "alarm": bool(alarm)} + +def _build_swagger_payload(cameras: List[Dict[str, Any]]) -> Dict[str, Any]: + return {"registeredAt": _now_iso_ms_utc(), "cameras": cameras} + +def _sanitize_http_base(v: str) -> str: + """Лечим ошибки вида ws://... или хвост /ws у базы HTTP.""" + v = (v or "").strip() + if v.startswith("ws://"): + v = "http://" + v[5:] + if v.startswith("wss://"): + v = "https://" + v[6:] + if v.endswith("/ws"): + v = v[:-3] + return v.rstrip("/") + +def _bulk_snapshot_for_all_cams() -> Tuple[str, Dict[str, Any]]: + """ + Собираем bulk-payload по всем камерам (mac/status/alarm). + Возвращает (url, json_body). + """ + # первоисточник камер + if cameras_io is not None: + try: + src = cameras_io.get_all() # type: ignore[attr-defined] + except Exception: + src = None + else: + src = None + if src is None: + src = getattr(config, "CAMERA_CONFIG", {}) + + detected = getattr(_state, "detected_cameras", set()) or set() + cameras_payload: List[Dict[str, Any]] = [] + + try: + items = sorted(src.items(), key=lambda kv: int(kv[0])) + except Exception: + items = list((src or {}).items()) + + for cid, cfg in items: + try: + cam_id = int(cid) + except Exception: + continue + mac = (cfg or {}).get("mac") + if not isinstance(mac, str) or not mac: + continue + try: + mac_n = _norm_mac(mac) + except Exception: + continue + online = bool(cam_id in detected) + armed = bool((_H.get(cam_id) or {}).get("armed", False)) + cameras_payload.append(_build_swagger_camera(mac_n, alarm=armed, status=online)) + + base = _sanitize_http_base(ALARM_HTTP_BASE) + path = str(ALARM_HTTP_PATH) + # heartbeat/bulk путь обязан быть БЕЗ {mac} + if "{mac}" in path: + path = path.replace("{mac}", "") + url = f"{base}/{path.lstrip('/')}" + return url, _build_swagger_payload(cameras_payload) + +def _dispatch_ws(minimal: Dict[str, Any]) -> None: + q = _ensure_ws_queue() + loop = _state.MAIN_LOOP + if loop is not None: + try: + loop.call_soon_threadsafe(q.put_nowait, minimal) + return + except Exception: + pass + q.put_nowait(minimal) + +def _dispatch_http(url: str, body: Dict[str, Any]) -> None: + q = _ensure_http_queue() + loop = _state.MAIN_LOOP + item = {"url": url, "json": body} + if loop is not None: + try: + loop.call_soon_threadsafe(q.put_nowait, item) + return + except Exception: + pass + q.put_nowait(item) + +# ===================================================================== +# Гистерезис по кадрам (состояние «armed» на камеру) +# ===================================================================== + +_H: Dict[int, Dict[str, Any]] = {} # cam_idx -> {"on":int, "off":int, "armed":bool} + +def _hyst_state(cam_idx: int) -> Dict[str, Any]: + st = _H.get(cam_idx) + if st is None: + st = {"on": 0, "off": 0, "armed": False} + _H[cam_idx] = st + return st + +def _prune_hysteresis(valid_ids: set[int]) -> None: + """Удалить состояния для камер, которых больше нет в конфиге.""" + for cid in list(_H.keys()): + if cid not in valid_ids: + _H.pop(cid, None) + +def ingest_detection(cam_idx: int, detected: bool) -> None: + """ + True -> есть детект на кадре + False -> нет детекта + При изменении «armed»: + - шлём WS одно событие {"type":"video_alarm","data": } (для совместимости) + - параллельно шлём HTTP bulk со всеми камерами (mac/status/alarm) + """ + st = _hyst_state(cam_idx) + if detected: + st["on"] += 1 + st["off"] = 0 + if (not st["armed"]) and st["on"] >= ALARM_COUNT_ON: + st["armed"] = True + _emit_alarm_change(True) + else: + st["off"] += 1 + st["on"] = 0 + if st["armed"] and st["off"] >= ALARM_COUNT_OFF: + st["armed"] = False + _emit_alarm_change(False) + +# ===================================================================== +# Эмиттер: WS + HTTP bulk +# ===================================================================== + +def _emit_alarm_change(alarm_state: bool) -> None: + # 1) WS минимальный флаг (оставляем — «как раньше») + minimal = {"type": "video_alarm", "data": bool(alarm_state)} + logger.info("[ALARM][WS][ENQ] data=%s", minimal["data"]) + _dispatch_ws(minimal) + + # 2) HTTP bulk по всем камерам + if ALARM_HTTP_ENABLE: + url, body = _bulk_snapshot_for_all_cams() + cams_cnt = len(body.get("cameras") or []) + logger.info("[ALARM][HTTP-BULK][ENQ] url=%s cameras=%d", url, cams_cnt) + _dispatch_http(url, body) + +# ===================================================================== +# Дополнительно: прямой smoke-тест по конкретному MAC (bulk + опц. WS) +# ===================================================================== + +def queue_alarm_mac(mac: str, alarm: bool, also_ws: bool = True) -> None: + """ + Вручную положить тревогу: на WS один бит, а на HTTP — bulk со всеми камерами, + где у камеры с данным MAC выставлен alarm, остальные — по текущему _H/online. + """ + try: + mac_n = _norm_mac(mac) + except Exception: + logger.error("[ALARM][SMOKE] bad MAC: %r", mac) + return + + # WS (минимальный формат) — опционально, чтобы сразу засветить on/off + if also_ws: + minimal = {"type": "video_alarm", "data": bool(alarm)} + logger.info("[ALARM][WS][ENQ][SMOKE] data=%s", minimal["data"]) + _dispatch_ws(minimal) + + # HTTP bulk — строим полный срез и принудительно меняем alarm у данного MAC + if not ALARM_HTTP_ENABLE: + return + url, body = _bulk_snapshot_for_all_cams() + # подменим/добавим запись с mac_n + cams = body.get("cameras") or [] + found = False + for c in cams: + if str(c.get("mac", "")).upper() == mac_n: + c["alarm"] = bool(alarm) + found = True + break + if not found: + # если в конфиге нет — добавим как online с заданной тревогой + cams.append(_build_swagger_camera(mac_n, alarm=bool(alarm), status=True)) + body["cameras"] = cams + logger.info("[ALARM][HTTP-BULK][ENQ][SMOKE] url=%s cameras=%d", url, len(cams)) + _dispatch_http(url, body) + +# ===================================================================== +# Отправители: WS и HTTP +# ===================================================================== + +async def _wait_state_ready(kind: str = "WS") -> None: + t0 = asyncio.get_running_loop().time() + while _state.MAIN_LOOP is None or (kind == "WS" and _state.alarm_queue is None): + await asyncio.sleep(0.05) + if asyncio.get_running_loop().time() - t0 > 5.0: + if _state.MAIN_LOOP is None: + _state.MAIN_LOOP = asyncio.get_running_loop() + if kind == "WS" and _state.alarm_queue is None: + _ensure_ws_queue() + break + +async def alarm_sender() -> None: + """ + Отправка video_alarm по WS c микробатчем (сливаем X сообщений за WS_BATCH_MS и отправляем последнее значение). + """ + await _wait_state_ready("WS") + import websockets + q = _ensure_ws_queue() + backoff = 1.0 + + while True: + try: + logger.info("[ALARM][WS] Connecting to %s ...", ALARM_WS_URL) + async with websockets.connect( + ALARM_WS_URL, + ping_interval=20, + close_timeout=1.0, + compression=None, + ) as ws: + logger.info("[ALARM][WS] Connected") + backoff = 1.0 + while True: + item = await q.get() + try: + # коалесцируем и берём последнее значение + last = item + t0 = asyncio.get_running_loop().time() + bucket = 1 + while bucket < WS_BATCH_MAX and (asyncio.get_running_loop().time() - t0) < (WS_BATCH_MS / 1000.0): + try: + nxt = q.get_nowait() + last = nxt + bucket += 1 + except asyncio.QueueEmpty: + await asyncio.sleep(0.01) + + data_bit = bool(last.get("data", False)) if isinstance(last, dict) else bool(last) + to_send = {"type": "video_alarm", "data": data_bit} + await ws.send(json.dumps(to_send, separators=(",", ":"))) + logger.info("[ALARM][WS][SENT] data=%s (batched=%d)", data_bit, bucket) + except Exception as exc: + logger.error("[ALARM][WS] send failed: %s", exc) + try: + q.put_nowait(item) + except Exception: + pass + break + finally: + try: + q.task_done() + except Exception: + pass + except Exception as exc: + logger.error("[ALARM][WS] connection error: %s", exc) + + await asyncio.sleep(backoff) + logger.info("[ALARM][WS] reconnect in %.1fs", backoff) + backoff = min(backoff * 2, 10.0) + +async def alarm_http_sender() -> None: + """ + POST bulk-payload на HTTP с коалесцированием по URL (оставляем последний body для данного URL). + """ + if not ALARM_HTTP_ENABLE: + logger.info("[ALARM][HTTP] disabled") + return + + import requests + from requests.adapters import HTTPAdapter + try: + from urllib3.util.retry import Retry # type: ignore + except Exception: + Retry = None # type: ignore + + sess = requests.Session() + sess.trust_env = False + if Retry is not None: + adapter = HTTPAdapter( + pool_connections=16, + pool_maxsize=16, + max_retries=Retry( + total=2, + backoff_factor=0.3, + status_forcelist=[500, 502, 503, 504], + raise_on_status=False, + ), + ) + else: + adapter = HTTPAdapter(pool_connections=16, pool_maxsize=16, max_retries=0) + sess.mount("http://", adapter) + sess.mount("https://", adapter) + headers = {"Content-Type": "application/json", "Connection": "keep-alive"} + + q = _ensure_http_queue() + loop = asyncio.get_running_loop() + + def _merge_http_bucket(bucket: List[Dict[str, Any]]) -> Tuple[str, Dict[str, Any]]: + # Все элементы в bucket — с одинаковым URL. Берём ПОСЛЕДНИЙ body. + url = bucket[0].get("url", "") + last_body = bucket[-1].get("json") or {} + return url, last_body + + while True: + item = await q.get() + try: + bucket = [item] + t0 = asyncio.get_running_loop().time() + while len(bucket) < HTTP_BATCH_MAX and (asyncio.get_running_loop().time() - t0) < (HTTP_BATCH_MS / 1000.0): + try: + nxt = q.get_nowait() + if nxt.get("url") != bucket[0].get("url"): + # другой URL — вернём назад + q.put_nowait(nxt) + break + bucket.append(nxt) + except asyncio.QueueEmpty: + await asyncio.sleep(0.01) + + url_raw, body = _merge_http_bucket(bucket) + base = _sanitize_http_base(ALARM_HTTP_BASE) + # если кто-то положил в элемент абсолютный url — используем его; иначе соберём из base + if url_raw.startswith("http://") or url_raw.startswith("https://"): + url = url_raw + else: + path = str(ALARM_HTTP_PATH).replace("{mac}", "") + url = f"{base}/{path.lstrip('/')}" + + cams_cnt = len(body.get("cameras") or []) + logger.info("[ALARM][HTTP-BULK][SEND] url=%s cameras=%d", url, cams_cnt) + + def _post(): + return sess.post(url, json=body, headers=headers, timeout=3.0) + + resp = await loop.run_in_executor(None, _post) + if 200 <= resp.status_code < 300: + logger.info("[ALARM][HTTP-BULK][OK] %s (%s) cameras=%d", url, resp.status_code, cams_cnt) + else: + logger.warning("[ALARM][HTTP-BULK][FAIL] %s HTTP %s", url, resp.status_code) + logger.debug("[ALARM][HTTP-BULK][RESP] %s", (resp.text or "")[:200]) + # вернуть последний элемент на ретрай + try: + q.put_nowait(bucket[-1]) + except Exception: + pass + except Exception as exc: + logger.error("[ALARM][HTTP-BULK] error: %s", exc) + try: + q.put_nowait(item) + except Exception: + pass + finally: + # аккуратно закрываем все task_done + for _ in range(len(bucket)): + try: + q.task_done() + except Exception: + pass + +# ===================================================================== +# Периодический heartbeat HTTP со всеми камерами (alarm=false) +# ===================================================================== + +async def periodic_status_sender() -> None: + """ + Раз в ALARM_HEARTBEAT_SEC отправляет массив камер на HTTP-адрес БЕЗ {mac}. + Формат: + { + "registeredAt": "...Z", + "cameras": [{"mac": "...", "status": true|false, "alarm": false}, ...] + } + """ + if not ALARM_HTTP_ENABLE: + logger.info("[ALARM][HEARTBEAT] disabled because HTTP disabled") + return + + import requests + from requests.adapters import HTTPAdapter + try: + from urllib3.util.retry import Retry # type: ignore + except Exception: + Retry = None # type: ignore + + period = int(ALARM_HEARTBEAT_SEC) + send_empty = bool(ALARM_HEARTBEAT_SEND_EMPTY) + + base = _sanitize_http_base(ALARM_HTTP_BASE) + hb_path = str(ALARM_HTTP_PATH) + if "{mac}" in hb_path: + logger.warning("[ALARM][HEARTBEAT] ALARM_HTTP_PATH содержит {mac}; это путь для bulk. Уберу {mac}.") + hb_path = hb_path.replace("{mac}", "") + url = f"{base}/{hb_path.lstrip('/')}" + + # подготовим HTTP клиент + sess = requests.Session() + sess.trust_env = False + if Retry is not None: + adapter = HTTPAdapter( + pool_connections=8, + pool_maxsize=8, + max_retries=Retry( + total=2, + backoff_factor=0.3, + status_forcelist=[500, 502, 503, 504], + raise_on_status=False, + ), + ) + else: + adapter = HTTPAdapter(pool_connections=8, pool_maxsize=8, max_retries=0) + sess.mount("http://", adapter) + sess.mount("https://", adapter) + headers = {"Content-Type": "application/json", "Connection": "keep-alive"} + + loop = asyncio.get_running_loop() + + def _snapshot_cameras() -> Tuple[List[Dict[str, Any]], set[int]]: + """Вернёт (список-камер-для-отправки, множество валидных id)""" + cams: List[Dict[str, Any]] = [] + valid_ids: set[int] = set() + # предпочтительно читаем из cameras_io, иначе — из config + src = None + if cameras_io is not None: + try: + src = cameras_io.get_all() # type: ignore[attr-defined] + except Exception: + src = None + if src is None: + src = getattr(config, "CAMERA_CONFIG", {}) + try: + items = sorted(src.items(), key=lambda kv: int(kv[0])) + except Exception: + items = list((src or {}).items()) + detected = getattr(_state, "detected_cameras", set()) or set() + for cid, cfg in items: + try: + cid_i = int(cid) + valid_ids.add(cid_i) + except Exception: + continue + mac = (cfg or {}).get("mac") + if isinstance(mac, str): + try: + mac_n = _norm_mac(mac) + cams.append(_build_swagger_camera(mac_n, alarm=False, status=(cid_i in detected))) + except Exception: + # плохой MAC — пропускаем + pass + return cams, valid_ids + + while True: + try: + cameras, valid_ids = _snapshot_cameras() + # Чистим гистерезис от удалённых камер + _prune_hysteresis(valid_ids) + if cameras or send_empty: + body = _build_swagger_payload(cameras) + cams_cnt = len(cameras) + logger.info("[ALARM][HEARTBEAT][SEND] url=%s cameras=%d", url, cams_cnt) + + def _post(): + return sess.post(url, json=body, headers=headers, timeout=3.0) + + resp = await loop.run_in_executor(None, _post) + if 200 <= resp.status_code < 300: + logger.info("[ALARM][HEARTBEAT][OK] %s (%s) cameras=%d", url, resp.status_code, cams_cnt) + else: + logger.warning("[ALARM][HEARTBEAT][FAIL] %s HTTP %s", url, resp.status_code) + logger.debug("[ALARM][HEARTBEAT][RESP] %s", (resp.text or "")[:200]) + else: + logger.info("[ALARM][HEARTBEAT] skip: no cameras (send_empty=False)") + except Exception as exc: + logger.error("[ALARM][HEARTBEAT] error: %s", exc) + finally: + await asyncio.sleep(max(5, period)) diff --git a/ptz_tracker_modular/app.py b/ptz_tracker_modular/app.py new file mode 100644 index 0000000..eb5b0f9 --- /dev/null +++ b/ptz_tracker_modular/app.py @@ -0,0 +1,149 @@ +from __future__ import annotations +import asyncio, logging, os, threading + +# UVLoop (optional) +try: + import uvloop # type: ignore + asyncio.set_event_loop_policy(uvloop.EventLoopPolicy()) +except Exception: + pass + +# Logging setup +def setup_logging() -> logging.Logger: + os.makedirs("logs", exist_ok=True) + logging.basicConfig( + level=logging.INFO, + format="%(asctime)s - %(levelname)s - %(message)s", + handlers=[ + logging.FileHandler("logs/ptz_tracker.log", encoding="utf-8"), + logging.StreamHandler(), + ], + ) + return logging.getLogger("PTZTracker") +logger = setup_logging() + +from . import config, state +from .streaming import video_broadcaster, cpp_ws_loop, cpp_detection_loop, MicroBatcher, build_servers +from .events import alarm_sender # WS-отправитель тревог (минимальный payload) +from .alarms import alarm_http_sender # HTTP-отправитель тревог (маршрут с MAC) +from .status import ptz_status_poller +from .heartbeat import heartbeat_pinger +from .sector import sector_autocal_from_presets, sector_init_on_startup +from .calibration import calibrate_presets, autocal_pan_sign, park_to_patrol_start +from .patrol import patrol_init_on_startup, patrol_supervisor_tick_bounded, patrol_supervisor_tick, sync_patrol_coordinator_loop +from . import ptz_io + +async def main() -> None: + state.MAIN_LOOP = asyncio.get_running_loop() + state.alarm_queue = asyncio.Queue() + state.detection_queue = asyncio.Queue() + state.batcher = MicroBatcher(max_batch=config.BATCH_MIN, max_wait_ms=2.0, + adaptive=config.ADAPTIVE_BATCH_ENABLE, bmin=config.BATCH_MIN, + bmax=config.BATCH_MAX, target_ms=config.BATCH_TARGET_MS) + + # Start PTZ worker thread + from queue import Queue + state.ptz_cmd_q = Queue(maxsize=2048) + state._ptz_thread = threading.Thread(target=ptz_io._ptz_worker, name="PTZWorker", daemon=True) + state._ptz_thread.start() + + # Calibrations + for cam_id in config.PTZ_CAM_IDS: + cfg = config.CAMERA_CONFIG[cam_id] + need = ( + cfg.get("preset1_deg") is None or + cfg.get("preset2_deg") is None or + cfg.get("sector_min_deg") is None or + cfg.get("sector_max_deg") is None + ) + if need: + ok = await calibrate_presets(cam_id) + if not ok: + logger.warning("Failed to calibrate camera %s, using fallback for sector clamp.", cam_id) + + if config.USE_PRESET_EDGES_FOR_SECTOR: + need_sector = any(state.ptz_states[c].get("sector_left_deg") is None for c in config.PTZ_CAM_IDS) + if need_sector: + await sector_autocal_from_presets() + sector_init_on_startup() + + for cam in config.PTZ_CAM_IDS: + try: + await autocal_pan_sign(cam) + except Exception as e: + logger.debug("autocal pan_sign cam %s: %s", cam, e) + + await park_to_patrol_start() + + if config.PTZ_ORDER and config.PATROL_ENABLE_ON_START: + if config.SYNC_PATROL_ENABLE and config.SYNC_COHORTS: + logger.info("SYNC PTZ patrol enabled (cohorts): %s", ", ".join(config.SYNC_COHORTS.keys())) + else: + patrol_init_on_startup() + logger.info("PTZ patrol started (standalone)") + else: + logger.info("PTZ patrol is DISABLED on start") + + logger.info("PTZ tracking + HARD sector clamp + bounded patrol (resync enabled)") + + ctrl_server, video_server = await build_servers() + logger.info("[CTRL WS] listening on 0.0.0.0:%s", config.VUE_CONTROL_WS_PORT) + logger.info("[VIDEO WS] listening on 0.0.0.0:%s", config.VUE_VIDEO_WS_PORT) + + async def patrol_supervisor_loop(): + while True: + try: + patrol_supervisor_tick_bounded() + patrol_supervisor_tick() + except Exception as e: + logger.error("patrol_supervisor error: %s", e) + await asyncio.sleep(0.10) + + tasks: list[asyncio.Task | asyncio.Future] = [ + asyncio.create_task(video_broadcaster()), + asyncio.create_task(cpp_ws_loop()), + asyncio.create_task(cpp_detection_loop()), + asyncio.create_task(alarm_sender()), # WS тревоги ({"type":"freq_alarm","data":...}) + asyncio.create_task(alarm_http_sender()), # HTTP тревоги (включается через config.ALARM_HTTP_ENABLE) + asyncio.create_task(ptz_status_poller()), + asyncio.create_task(heartbeat_pinger()), + asyncio.create_task(ctrl_server.wait_closed()), + asyncio.create_task(video_server.wait_closed()), + ] + if config.PATROL_ENABLE_ON_START: + tasks.insert(6, asyncio.create_task(patrol_supervisor_loop())) + if config.SYNC_PATROL_ENABLE and config.SYNC_COHORTS: + tasks.insert(6, asyncio.create_task(sync_patrol_coordinator_loop())) + + try: + await asyncio.gather(*tasks) + finally: + ctrl_server.close() + video_server.close() + await ctrl_server.wait_closed() + await video_server.wait_closed() + +def run(): + try: + asyncio.run(main()) + except KeyboardInterrupt: + logger.info("Stopped by user") + finally: + try: + if state.ptz_cmd_q is not None: + state.ptz_cmd_q.put_nowait(None) # sentinel for PTZ worker + if state._ptz_thread is not None: + state._ptz_thread.join(timeout=0.5) + except Exception: + pass + for _cid, _st in state.ptz_states.items(): + if _st.get("zoom_reset_timer"): + try: _st["zoom_reset_timer"].cancel() + except Exception: pass + if _st.get("preset_timer"): + try: _st["preset_timer"].cancel() + except Exception: pass + logger.info("Shutdown complete") + +if __name__ == "__main__": + run() diff --git a/ptz_tracker_modular/calibration.py b/ptz_tracker_modular/calibration.py new file mode 100644 index 0000000..09f4324 --- /dev/null +++ b/ptz_tracker_modular/calibration.py @@ -0,0 +1,480 @@ +# calibration.py +from __future__ import annotations + +import asyncio +import logging +import math +from typing import Optional, Tuple + +from . import config, state +from .geom import ang_diff_signed, normalize360 +from .ptz_io import read_ptz_azimuth_deg, goto_preset_token, get_ptz_status + +# Опционально: запись в cameras.toml +try: + from .toml_persist import ( + persist_north_offset, # (cam_id, north_deg, source) -> bool + persist_geo_fields, # (cam_id, fields: dict[str, float], source) -> bool + ) +except Exception: + persist_north_offset = None # type: ignore[assignment] + persist_geo_fields = None # type: ignore[assignment] + +logger = logging.getLogger("PTZTracker") + + +# ============================== +# Внутренние утилиты +# ============================== + +async def _wait_az_stable(cam: int, timeout: float = 2.5, tol_deg: float = 0.3) -> Optional[float]: + """ + Ждём стабилизации pan: возвращает угол, когда изменения <= tol_deg. + """ + loop = asyncio.get_event_loop() + t0 = loop.time() + last = None + stable = 0 + while loop.time() - t0 < timeout: + a = read_ptz_azimuth_deg(cam) + if a is None: + await asyncio.sleep(0.12) + continue + a = normalize360(a) + if last is None or abs(ang_diff_signed(a, last)) > tol_deg: + stable = 0 + else: + stable += 1 + if stable >= 1: + return a + last = a + await asyncio.sleep(0.12) + return last + + +async def get_absolute_ptz_position(cam_idx: int) -> Optional[Tuple[float, float]]: + """ + Читает абсолютные оценки pan/tilt (deg), pan нормализует в 0..360. + + FIX: tilt у части Hikvision может не возвращаться/не парситься. + Для калибровки сектора критичен pan, поэтому: + - если pan есть, а tilt нет -> tilt=0.0 + """ + pan, tilt = get_ptz_status(cam_idx) + if pan is None: + return None + pan_n = normalize360(float(pan)) + tilt_f = float(tilt) if tilt is not None else 0.0 + return pan_n, tilt_f + + +def to_geo_bearing(pan_abs_cam_deg: float, north_offset_deg: float | None) -> float: + """geo = pan_abs + north_offset (в 0..360).""" + return normalize360(pan_abs_cam_deg + (north_offset_deg or 0.0)) + + +def shortest_signed(a: float, b: float) -> float: + """Подписанное отклонение угла a -> b в (-180; 180].""" + return ang_diff_signed(b, a) + + +async def compute_north_offset_on_preset1(cam_idx: int, ref_bearing_geo: float) -> Optional[float]: + """ + Находясь на preset1: north_offset = ref_bearing_geo - pan_abs_cam. + """ + a = await _wait_az_stable(cam_idx) + if a is None: + return None + return normalize360(ref_bearing_geo - a) + + +async def get_bearing_curr_geo(cam_idx: int) -> Optional[float]: + """ + Текущий гео-азимут взгляда (0..360), если есть north_offset_deg. + """ + cfg = config.CAMERA_CONFIG[cam_idx] + north = cfg.get("north_offset_deg") + if north is None: + return None + a = await _wait_az_stable(cam_idx) + if a is None: + return None + return to_geo_bearing(a, float(north)) + + +async def get_delta_from_preset1_geo(cam_idx: int) -> Optional[float]: + """ + Подписанная дельта от preset1 в гео-СК. Положит. — вправо. + """ + cfg = config.CAMERA_CONFIG[cam_idx] + p1_geo = cfg.get("preset1_deg_geo") + if p1_geo is None: + return None + b = await get_bearing_curr_geo(cam_idx) + if b is None: + return None + return shortest_signed(p1_geo, b) + + +async def calibrate_presets(cam_idx: int) -> bool: + """ + Снимаем pan/tilt на preset1/preset2 и выставляем сектор во «внутренних» углах. + + FIX: + - не требуем tilt (если его нет — tilt=0.0) + - сохраняем preset*_deg / sector_* в cameras.toml через persist_geo_fields (если доступно) + - НЕ запускаем патруль здесь (иначе легко получить "дерганье" и спам пресетов) + """ + cfg = config.CAMERA_CONFIG[cam_idx] + p1, p2 = cfg.get("preset1"), cfg.get("preset2") + if not (p1 and p2): + return False + + start_az = read_ptz_azimuth_deg(cam_idx) + logger.info("Calibrating presets for camera %s", cam_idx) + + # preset1 + goto_preset_token(cam_idx, p1) + await asyncio.sleep(0.6) + _ = await _wait_az_stable(cam_idx) + pos1 = await get_absolute_ptz_position(cam_idx) + if pos1 is None: + logger.error("Failed to get pan for preset1 on camera %s", cam_idx) + return False + + # preset2 + goto_preset_token(cam_idx, p2) + await asyncio.sleep(0.6) + _ = await _wait_az_stable(cam_idx) + pos2 = await get_absolute_ptz_position(cam_idx) + if pos2 is None: + logger.error("Failed to get pan for preset2 on camera %s", cam_idx) + return False + + az1, tilt1 = pos1 + az2, tilt2 = pos2 + + cfg["preset1_deg"] = float(az1) + cfg["preset2_deg"] = float(az2) + cfg["preset1_tilt_deg"] = float(tilt1) + cfg["preset2_tilt_deg"] = float(tilt2) + + span = ang_diff_signed(az2, az1) + if span >= 0: + sector_min = az1 - config.SECTOR_MARGIN_DEG + sector_max = az2 + config.SECTOR_MARGIN_DEG + else: + sector_min = az2 - config.SECTOR_MARGIN_DEG + sector_max = az1 + config.SECTOR_MARGIN_DEG + + cfg["sector_min_deg"] = float(normalize360(sector_min)) + cfg["sector_max_deg"] = float(normalize360(sector_max)) + + logger.info( + "Camera %s sector calibrated: %.2f°..%.2f° (p1=%.2f°, p2=%.2f°)", + cam_idx, cfg["sector_min_deg"], cfg["sector_max_deg"], az1, az2 + ) + + # Persist в cameras.toml (если доступно) + if callable(persist_geo_fields): + try: + persist_geo_fields( + cam_idx, + { + "preset1_deg": float(az1), + "preset2_deg": float(az2), + "preset1_tilt_deg": float(tilt1), + "preset2_tilt_deg": float(tilt2), + "sector_min_deg": float(cfg["sector_min_deg"]), + "sector_max_deg": float(cfg["sector_max_deg"]), + }, + source="calibrate_presets", + ) + except Exception as e: + logger.warning("[CAL] persist_geo_fields failed for cam %s: %s", cam_idx, e) + + # вернуть ближе к исходному положению + if start_az is not None: + token = p1 if abs(ang_diff_signed(start_az, az1)) <= abs(ang_diff_signed(start_az, az2)) else p2 + else: + token = p1 + goto_preset_token(cam_idx, token) + + return True + + +async def calibrate_presets_geo(cam_idx: int, ref_bearing_geo: float) -> bool: + """ + 1) preset1 → pan/tilt → preset1_deg/preset1_tilt_deg + 2) north_offset = ref_bearing_geo - pan_abs(preset1) → сохранить + 3) preset2 → pan/tilt + 4) preset*_deg_geo, sector_*_geo → сохранить + """ + cfg = config.CAMERA_CONFIG[cam_idx] + p1, p2 = cfg.get("preset1"), cfg.get("preset2") + if not (p1 and p2): + logger.error("[GEO] Geo-calibration requires preset1 and preset2 on camera %s", cam_idx) + return False + + start_az = read_ptz_azimuth_deg(cam_idx) + logger.info("[GEO] Calibrating presets (with north) for camera %s", cam_idx) + + # --- preset1 + goto_preset_token(cam_idx, p1) + await asyncio.sleep(0.6) + _ = await _wait_az_stable(cam_idx) + pos1 = await get_absolute_ptz_position(cam_idx) + if pos1 is None: + logger.error("[GEO] Failed to get pan for preset1 on camera %s", cam_idx) + return False + az1, tilt1 = pos1 + + # --- north offset + ref_bearing_geo = normalize360(ref_bearing_geo) + north_offset = await compute_north_offset_on_preset1(cam_idx, ref_bearing_geo) + if north_offset is None: + logger.error("[GEO] Failed to compute north_offset for camera %s", cam_idx) + return False + north_offset = normalize360(north_offset) + + # записать в TOML (если доступно) + if callable(persist_north_offset): + try: + changed = persist_north_offset(cam_idx, north_offset, source="calibrate_presets_geo") + if changed: + logger.info("[GEO] cameras.toml updated with north_offset for cam %s (%.2f°)", cam_idx, north_offset) + else: + logger.info("[GEO] north_offset for cam %s already up-to-date (%.2f°)", cam_idx, north_offset) + except Exception as e: + logger.warning("[GEO] persist_north_offset failed for cam %s: %s", cam_idx, e) + else: + logger.warning("[GEO] persist_north_offset is unavailable; north won't be written to cameras.toml") + + # --- preset2 + goto_preset_token(cam_idx, p2) + await asyncio.sleep(0.6) + _ = await _wait_az_stable(cam_idx) + pos2 = await get_absolute_ptz_position(cam_idx) + if pos2 is None: + logger.error("[GEO] Failed to get pan for preset2 on camera %s", cam_idx) + return False + az2, tilt2 = pos2 + + # --- внутренние углы + cfg["preset1_deg"] = float(az1) + cfg["preset2_deg"] = float(az2) + cfg["preset1_tilt_deg"] = float(tilt1) + cfg["preset2_tilt_deg"] = float(tilt2) + + # --- гео-углы пресетов + cfg["north_offset_deg"] = float(north_offset) + cfg["preset1_deg_geo"] = to_geo_bearing(az1, north_offset) + cfg["preset2_deg_geo"] = to_geo_bearing(az2, north_offset) + + # --- сектор в гео + span_geo = ang_diff_signed(cfg["preset2_deg_geo"], cfg["preset1_deg_geo"]) + if span_geo >= 0: + sector_min_geo = cfg["preset1_deg_geo"] - config.SECTOR_MARGIN_DEG + sector_max_geo = cfg["preset2_deg_geo"] + config.SECTOR_MARGIN_DEG + else: + sector_min_geo = cfg["preset2_deg_geo"] - config.SECTOR_MARGIN_DEG + sector_max_geo = cfg["preset1_deg_geo"] + config.SECTOR_MARGIN_DEG + + cfg["sector_min_deg_geo"] = normalize360(sector_min_geo) + cfg["sector_max_deg_geo"] = normalize360(sector_max_geo) + + # сохранить полезные geo-поля в TOML (опц.) + if callable(persist_geo_fields): + try: + persist_geo_fields( + cam_idx, + { + "preset1_deg": float(az1), + "preset2_deg": float(az2), + "preset1_tilt_deg": float(tilt1), + "preset2_tilt_deg": float(tilt2), + "north_offset_deg": float(north_offset), + "preset1_deg_geo": float(cfg["preset1_deg_geo"]), + "preset2_deg_geo": float(cfg["preset2_deg_geo"]), + "sector_min_deg_geo": float(cfg["sector_min_deg_geo"]), + "sector_max_deg_geo": float(cfg["sector_max_deg_geo"]), + }, + source="calibrate_presets_geo", + ) + except Exception: + pass + + logger.info( + "[GEO] Camera %s north_offset=%.2f°, p1_geo=%.2f°, p2_geo=%.2f°, sector_geo=%.1f°..%.1f°", + cam_idx, + cfg["north_offset_deg"], cfg["preset1_deg_geo"], cfg["preset2_deg_geo"], + cfg["sector_min_deg_geo"], cfg["sector_max_deg_geo"] + ) + + # вернуть ближе к исходному + token = p1 + if start_az is not None: + token = p1 if abs(ang_diff_signed(start_az, az1)) <= abs(ang_diff_signed(start_az, az2)) else p2 + goto_preset_token(cam_idx, token) + return True + + +async def autocal_pan_sign(cam: int): + """ + Определение знака панорамирования через небольшой толчок вправо. + """ + a0 = read_ptz_azimuth_deg(cam) + if a0 is None: + return + from .ptz_io import move_ptz, stop_ptz + move_ptz(cam, +0.12, 0.0, 0.0) + await asyncio.sleep(0.25) + stop_ptz(cam) + await asyncio.sleep(0.20) + a1 = read_ptz_azimuth_deg(cam) + if a1 is None: + return + delta = ang_diff_signed(a1, a0) + if abs(delta) < 0.05: + logger.info( + "[PAN_SIGN] cam %s: no movement (near wall?) — keep pan_sign=%s", + cam, config.CAMERA_CONFIG[cam].get("pan_sign") + ) + return + config.CAMERA_CONFIG[cam]["pan_sign"] = +1 if delta > 0 else -1 + logger.info("[PAN_SIGN] cam %s set to %s (delta=%.2f°)", cam, config.CAMERA_CONFIG[cam]["pan_sign"], delta) + + +async def park_to_patrol_start(): + """ + Поставить камеры на старт патруля согласно PATROL_START_DIR: + если +1 — preset1, если -1 — preset2. + """ + from .ptz_io import stop_ptz, goto_preset_token + for cam in config.PTZ_ORDER: + cfg = config.CAMERA_CONFIG[cam] + dir_sign = config.PATROL_START_DIR.get(cam, +1) + start_token = cfg.get("preset1") if dir_sign >= 0 else cfg.get("preset2") + if start_token: + stop_ptz(cam) + goto_preset_token(cam, start_token) + await asyncio.sleep(1.2) + + +def _pano_center_geo_from_state(pano_id: int) -> Optional[float]: + """ + Достаём последний абсолютный азимут центра панорамы из state. + Поддерживаем несколько возможных ключей. + """ + s = getattr(state, "pano_states", {}).get(pano_id) or {} + for k in ("center_bearing_deg", "bearing_deg", "geo_bearing_deg", "attack_bearing_deg"): + v = s.get(k) + if isinstance(v, (int, float)): + return normalize360(float(v)) + # fallback: иногда кладут в ptz_states[pano_id]["pan_deg_geo"] + ss = getattr(state, "ptz_states", {}).get(pano_id) or {} + v = ss.get("pan_deg_geo") + if isinstance(v, (int, float)): + return normalize360(float(v)) + return None + + +async def auto_calibrate_north_from_pano_edges(ptz_id: int) -> Optional[float]: + """ + Вычисляет и сохраняет north_offset_deg для PTZ, используя спаренную панораму: + - pano_id = config.PTZ_TO_PAN[ptz_id] + - center_geo = центр панорамы из state (0..360) + - hfov = ширина обзора панорамы в градусах (из cameras.toml) + - preset1/preset2 считаем левой/правой границей (или наоборот при preset1_is_left=false) + - измеряем внутренние pan на обоих пресетах (a1, a2) + - считаем два независимых north: n1=left_geo-a1, n2=right_geo-a2 + - берём среднее на окружности и пишем в TOML + Вернёт north_offset_deg или None. + """ + # 0) привязка PTZ↔PANO + pano_id = config.PTZ_TO_PAN.get(ptz_id) + if pano_id is None: + logger.warning("[AUTO-NORTH] PTZ %s: no paired pano in PTZ_TO_PAN", ptz_id) + return None + + # 1) центр панорамы + center_geo = _pano_center_geo_from_state(pano_id) + if center_geo is None: + logger.warning("[AUTO-NORTH] PTZ %s: pano %s center bearing is unknown", ptz_id, pano_id) + return None + + # 2) HFOV панорамы + hfov = config.get_cam_hfov_deg(pano_id) + if not isinstance(hfov, (int, float)) or hfov <= 0: + logger.warning("[AUTO-NORTH] PTZ %s: pano %s HFOV unknown", ptz_id, pano_id) + return None + + left_geo = normalize360(center_geo - hfov * 0.5) + right_geo = normalize360(center_geo + hfov * 0.5) + + cfg = config.CAMERA_CONFIG.get(ptz_id, {}) + p1, p2 = cfg.get("preset1"), cfg.get("preset2") + if not (p1 and p2): + logger.warning("[AUTO-NORTH] PTZ %s: presets 1/2 are required", ptz_id) + return None + + # какие пресеты — левый/правый край? + p1_is_left = cfg.get("preset1_is_left") + if p1_is_left is None: + # если явно не задано — ориентируемся по sweep_sign (или pan_sign) + sweep = int(cfg.get("sweep_sign", cfg.get("pan_sign", 1)) or 1) + p1_is_left = True if sweep >= 0 else False + + target_geo_for_p1 = left_geo if p1_is_left else right_geo + target_geo_for_p2 = right_geo if p1_is_left else left_geo + + # helper: стабилизация pan + async def _stable_pan(cam: int) -> Optional[float]: + return await _wait_az_stable(cam, timeout=2.5, tol_deg=0.3) + + # 3) измеряем пресеты + goto_preset_token(ptz_id, p1); await asyncio.sleep(0.6) + a1 = await _stable_pan(ptz_id) + if a1 is None: + logger.warning("[AUTO-NORTH] PTZ %s: cannot read pan on preset1", ptz_id) + return None + + goto_preset_token(ptz_id, p2); await asyncio.sleep(0.6) + a2 = await _stable_pan(ptz_id) + if a2 is None: + logger.warning("[AUTO-NORTH] PTZ %s: cannot read pan on preset2", ptz_id) + return None + + # 4) два независимых north + среднее на окружности + n1 = normalize360(target_geo_for_p1 - a1) + n2 = normalize360(target_geo_for_p2 - a2) + + sx = math.cos(math.radians(n1)) + math.cos(math.radians(n2)) + sy = math.sin(math.radians(n1)) + math.sin(math.radians(n2)) + north = normalize360(math.degrees(math.atan2(sy, sx))) if (sx != 0 or sy != 0) else n1 + + # 5) сохраняем в TOML и рантайм + if callable(persist_north_offset): + try: + persist_north_offset(ptz_id, north, source="auto_from_pano_edges") + except Exception: + pass + cfg["north_offset_deg"] = north + + # опционально сохраним справочно preset1_ref_geo_deg + if callable(persist_geo_fields): + try: + persist_geo_fields(ptz_id, {"preset1_ref_geo_deg": target_geo_for_p1}, source="auto_from_pano_edges") + except Exception: + pass + + logger.info( + "[AUTO-NORTH] PTZ %s: north_offset=%.2f°, p1->%.2f°, p2->%.2f° (center=%.2f°, hfov=%.1f°)", + ptz_id, north, target_geo_for_p1, target_geo_for_p2, center_geo, hfov + ) + return north + + +async def autocal_geo_from_panorama(cam_id: int) -> bool: + north = await auto_calibrate_north_from_pano_edges(cam_id) + return north is not None diff --git a/ptz_tracker_modular/cameras.toml b/ptz_tracker_modular/cameras.toml new file mode 100644 index 0000000..d0e358a --- /dev/null +++ b/ptz_tracker_modular/cameras.toml @@ -0,0 +1,179 @@ +[[camera]] +id = 0 +ip = "192.168.54.8" +username = "admin" +password = "Vika_1975" +ptz_channel = 1 +wiper_channel = 1 +preset1 = "1" +preset2 = "2" +sweep_sign = 1 +is_ptz = true +mac = "80:BE:AF:E5:48:01" +preset1_deg = 343.000000 +preset2_deg = 66.000000 +preset1_tilt_deg = -138.000000 +preset2_tilt_deg = -138.000000 +sector_min_deg = 345.000000 +sector_max_deg = 64.000000 +geo_last_source = "sector_autocal" +geo_last_at = 1770695205 + +[[camera]] +id = 1 +ip = "192.168.54.8" +username = "admin" +password = "Vika_1975" +ptz_channel = 2 +wiper_channel = 1 +is_ptz = false + +[[camera]] +id = 2 +ip = "192.168.54.9" +username = "admin" +password = "Vika_1975" +ptz_channel = 1 +wiper_channel = 1 +preset1 = "1" +preset2 = "2" +sweep_sign = 1 +is_ptz = true +mac = "80:BE:AF:E5:48:09" +preset1_deg = 342.000000 +preset2_deg = 134.000000 +preset1_tilt_deg = -140.000000 +preset2_tilt_deg = -140.000000 +sector_min_deg = 344.000000 +sector_max_deg = 132.000000 +geo_last_source = "sector_autocal" +geo_last_at = 1770695207 + +[[camera]] +id = 3 +ip = "192.168.54.9" +username = "admin" +password = "Vika_1975" +ptz_channel = 2 +wiper_channel = 1 +is_ptz = false + +[[camera]] +id = 4 +ip = "192.168.54.10" +username = "admin" +password = "Vika_1975" +ptz_channel = 1 +wiper_channel = 1 +preset1 = "1" +preset2 = "2" +sweep_sign = 1 +is_ptz = true +mac = "80:BE:AF:E5:47:DB" +preset1_deg = 343.600000 +preset2_deg = 343.600000 +preset1_tilt_deg = -12.000000 +preset2_tilt_deg = -12.000000 +sector_min_deg = 343.600000 +sector_max_deg = 343.600000 +geo_last_source = "sector_autocal" +geo_last_at = 1770695209 + +[[camera]] +id = 5 +ip = "192.168.54.10" +username = "admin" +password = "Vika_1975" +ptz_channel = 2 +wiper_channel = 1 +is_ptz = false + +[[camera]] +id = 6 +ip = "192.168.54.11" +username = "admin" +password = "Vika_1975" +ptz_channel = 1 +wiper_channel = 1 +preset1 = "1" +preset2 = "2" +sweep_sign = 1 +is_ptz = true +mac = "80:BE:AF:E5:47:D5" +preset1_deg = 340.200000 +preset2_deg = 129.000000 +preset1_tilt_deg = -150.000000 +preset2_tilt_deg = -150.000000 +sector_min_deg = 342.200000 +sector_max_deg = 127.000000 +geo_last_source = "sector_autocal" +geo_last_at = 1770695210 + +[[camera]] +id = 7 +ip = "192.168.54.11" +username = "admin" +password = "Vika_1975" +ptz_channel = 2 +wiper_channel = 1 +is_ptz = false + +[[camera]] +id = 8 +ip = "192.168.54.12" +username = "admin" +password = "Vika_1975" +ptz_channel = 1 +wiper_channel = 1 +preset1 = "1" +preset2 = "2" +sweep_sign = 1 +is_ptz = true +mac = "80:BE:AF:E5:48:07" +preset1_deg = 346.300000 +preset2_deg = 33.000000 +preset1_tilt_deg = -118.000000 +preset2_tilt_deg = -118.000000 +sector_min_deg = 348.300000 +sector_max_deg = 31.000000 +geo_last_source = "sector_autocal" +geo_last_at = 1770695212 + +[[camera]] +id = 9 +ip = "192.168.54.12" +username = "admin" +password = "Vika_1975" +ptz_channel = 2 +wiper_channel = 1 +is_ptz = false + +[[camera]] +id = 10 +ip = "192.168.54.13" +username = "admin" +password = "Vika_1975" +ptz_channel = 1 +wiper_channel = 1 +preset1 = "1" +preset2 = "2" +sweep_sign = 1 +is_ptz = true +mac = "80:BE:AF:E5:47:C1" +preset1_deg = 340.700000 +preset2_deg = 41.000000 +preset1_tilt_deg = -120.000000 +preset2_tilt_deg = -120.000000 +sector_min_deg = 342.700000 +sector_max_deg = 39.000000 +geo_last_source = "sector_autocal" +geo_last_at = 1770695214 + +[[camera]] +id = 11 +ip = "192.168.54.13" +username = "admin" +password = "Vika_1975" +ptz_channel = 2 +wiper_channel = 1 +is_ptz = false diff --git a/ptz_tracker_modular/cameras_io.py b/ptz_tracker_modular/cameras_io.py new file mode 100644 index 0000000..ad6924b --- /dev/null +++ b/ptz_tracker_modular/cameras_io.py @@ -0,0 +1,312 @@ +from __future__ import annotations +from pathlib import Path +from typing import Any, Dict, List, Tuple, Optional +import os +import tomllib +import logging +logger = logging.getLogger("PTZTracker") + + +try: + from . import config # type: ignore +except Exception: + config = None # type: ignore + + +# ------------------------------ Порядок ключей ------------------------------ + +CAM_KEYS_ORDER = [ + "ip", "username", "password", "ptz_channel", + "preset1", "preset2", "sweep_sign", + "preset1_deg", "preset2_deg", + "preset1_tilt_deg", "preset2_tilt_deg", + "sector_min_deg", "sector_max_deg", + "pan_sign", "tilt_sign", "pan_offset_deg", "tilt_offset_deg", + "mac", + + "north_offset_deg", + "preset1_deg_geo", "preset2_deg_geo", + "sector_min_deg_geo", "sector_max_deg_geo", +] + +# Мягкие дефолты для новых полей (и некоторых старых) +DEFAULTS: Dict[str, Any] = { + "ptz_channel": 1, + "preset1": None, + "preset2": None, + "sweep_sign": 1, + "preset1_deg": None, + "preset2_deg": None, + "preset1_tilt_deg": None, + "preset2_tilt_deg": None, + "sector_min_deg": None, + "sector_max_deg": None, + "pan_sign": None, + "tilt_sign": None, + "pan_offset_deg": 0.0, + "tilt_offset_deg": 0.0, + "mac": None, + "north_offset_deg": None, + "preset1_deg_geo": None, + "preset2_deg_geo": None, + "sector_min_deg_geo": None, + "sector_max_deg_geo": None, +} + + +# ------------------------------ IO helpers ------------------------------ + +def _cameras_file_path() -> Path: + env = os.getenv("CAMERAS_FILE") + if env: + return Path(env).resolve() + + here = Path(__file__).resolve() + for parent in [here.parent, *here.parents]: + if parent.name == "ptz_tracker_modular": + return (parent / "cameras.toml").resolve() + + # fallback — рядом с текущим модулем + return (here.parent / "cameras.toml").resolve() + + +def _apply_defaults(cfg: Dict[str, Any]) -> Dict[str, Any]: + """Мягко подставляет дефолты; в файл None не пишем.""" + for k, v in DEFAULTS.items(): + cfg.setdefault(k, v) + return cfg + + +def _load_all() -> Dict[int, Dict[str, Any]]: + path = _cameras_file_path() + if not path.exists(): + return {} + if path.suffix.lower() != ".toml": + raise RuntimeError("Для веб-редактирования ожидается TOML (cameras.toml)") + with open(path, "rb") as f: + data = tomllib.load(f) + arr = data.get("camera") or data.get("cameras") or [] + res: Dict[int, Dict[str, Any]] = {} + for item in arr: + cid = int(item["id"]) + res[cid] = _apply_defaults(dict(item)) + return res + + +def _dump_toml(cams: Dict[int, Dict[str, Any]]) -> str: + lines: List[str] = [] + for cid in sorted(cams.keys()): + cfg = dict(cams[cid]) + lines.append("[[camera]]") + lines.append(f"id = {cid}") + + # Сначала печатаем ключи в фиксированном порядке + for k in CAM_KEYS_ORDER: + if k not in cfg or cfg[k] is None: + continue + v = cfg[k] + if isinstance(v, str): + v = v.replace("\\", "\\\\").replace('"', '\\"') + lines.append(f'{k} = "{v}"') + elif isinstance(v, bool): + lines.append(f'{k} = {"true" if v else "false"}') + else: + lines.append(f"{k} = {v}") + + # Затем доп. ключи, которых нет в CAM_KEYS_ORDER + for k, v in cfg.items(): + if k in CAM_KEYS_ORDER or k == "id": + continue + if v is None: + continue + if isinstance(v, str): + v = v.replace("\\", "\\\\").replace('"', '\\"') + lines.append(f'{k} = "{v}"') + elif isinstance(v, bool): + lines.append(f'{k} = {"true" if v else "false"}') + else: + lines.append(f"{k} = {v}") + + lines.append("") # пустая строка между камерами + return "\n".join(lines).rstrip() + "\n" + + +def _write_all(cams: Dict[int, Dict[str, Any]]) -> None: + path = _cameras_file_path() + txt = _dump_toml(cams) + path.write_text(txt, encoding="utf-8") + + +# ------------------------------ Публичные утилиты ------------------------------ + +def get_all() -> Dict[int, Dict[str, Any]]: + """Вернёт все камеры с подставленными дефолтами (в памяти).""" + return _load_all() + + +def get(cid: int) -> Optional[Dict[str, Any]]: + """Вернёт конфиг одной камеры или None.""" + return _load_all().get(int(cid)) + + +def update(cid: int, patch: Dict[str, Any]) -> Dict[int, Dict[str, Any]]: + """ + Патчит одну камеру по id и сохраняет файл. + Возвращает обновлённый словарь всех камер. + """ + cid = int(cid) + cams = _load_all() + if cid not in cams: + raise KeyError(f"Camera id {cid} not found") + base = cams[cid] + + # нормализация типов + if "ptz_channel" in patch and patch["ptz_channel"] is not None: + patch["ptz_channel"] = int(patch["ptz_channel"]) + if "sweep_sign" in patch and patch["sweep_sign"] is not None: + patch["sweep_sign"] = int(patch["sweep_sign"]) + + base.update(patch) + cams[cid] = _apply_defaults(base) + _write_all(cams) + return cams + + +# ------------------------------ CRUD (твоя логика) ------------------------------ + +def add_or_update_camera(cam: Dict[str, Any]) -> Tuple[int, Dict[int, Dict[str, Any]]]: + """ + Одиночная запись (как у тебя было). + cam: {id? , ip, username, password, ptz_channel, preset1, preset2, sweep_sign, ...} + Если id не задан – берём max+1. Возвращает (real_id, updated_dict). + """ + cams = _load_all() + + # Вычисляем id + real_id = int(cam["id"]) if ("id" in cam and cam["id"] is not None) else ( + (max(cams.keys()) + 1) if cams else 0 + ) + + # Нормализация/дефолты + rec = dict(cam) + rec["id"] = real_id + if "ptz_channel" in rec and rec["ptz_channel"] is not None: + rec["ptz_channel"] = int(rec["ptz_channel"]) + if "sweep_sign" in rec and rec["sweep_sign"] is not None: + rec["sweep_sign"] = int(rec["sweep_sign"]) + + cams[real_id] = _apply_defaults(rec) + _write_all(cams) + + # Аккуратный лог: покажем, какие ID по этому IP уже есть (ch=1/ch=2) + ip = rec.get("ip") + ptz_id = None + pan_id = None + try: + for cid, c in cams.items(): + if c.get("ip") == ip: + ch = int(c.get("ptz_channel", 1)) + if ch == 1: + ptz_id = cid + elif ch == 2: + pan_id = cid + except Exception: + pass + + logger.info( + f"[CFG] add_or_update_camera ip={ip} -> saved id={real_id} ch={rec.get('ptz_channel', 1)}; " + f"present: PTZ id={ptz_id}, PAN id={pan_id} (total={len(cams)})" + ) + + return real_id, cams + + +def delete_camera(cid: int) -> Dict[int, Dict[str, Any]]: + cams = _load_all() + if cid in cams: + del cams[cid] + _write_all(cams) + return cams + + +# ------------------------------ ПАРНАЯ запись PTZ+PAN ------------------------------ + +def add_or_update_camera_pair(cam: Dict[str, Any]) -> Tuple[int, Dict[int, Dict[str, Any]]]: + """ + Создаёт/обновляет ДВЕ записи на один IP: + - PTZ (канал 1) с пресетами preset1/preset2 + - PAN (канал 2) без пресетов + Если одна из записей существует — аккуратно обновит её и досоздаст вторую. + Возвращает (ptz_id, весь словарь камер). + """ + cams = _load_all() + + # Входные данные + ip = cam["ip"] + username = cam.get("username") + password = cam.get("password") + sweep_sign = int(cam.get("sweep_sign", 1)) + preset1 = cam.get("preset1") + preset2 = cam.get("preset2") + mac = cam.get("mac") + + have = {cid: c for cid, c in cams.items() if c.get("ip") == ip} + by_ch: Dict[int, Tuple[int, Dict[str, Any]]] = {} + for cid, c in have.items(): + try: + ch = int(c.get("ptz_channel", 1)) + except Exception: + ch = 1 + by_ch[ch] = (cid, c) + + # --- PTZ (ch = 1) --- + ptz_patch = { + "ip": ip, + "username": username, + "password": password, + "ptz_channel": 1, + "preset1": preset1, + "preset2": preset2, + "sweep_sign": sweep_sign, + "mac": mac, + } + if 1 in by_ch: + ptz_id, base = by_ch[1] + # не затираем None-ами + for k, v in ptz_patch.items(): + if v is not None: + base[k] = v + cams[ptz_id] = _apply_defaults(base) + else: + ptz_id = (max(cams.keys()) + 1) if cams else 0 + rec = {"id": ptz_id, **ptz_patch} + cams[ptz_id] = _apply_defaults(rec) + + # --- PAN (ch = 2) --- + pan_patch = { + "ip": ip, + "username": username, + "password": password, + "ptz_channel": 2, + "preset1": None, + "preset2": None, + "sweep_sign": sweep_sign, + "mac": mac, + } + if 2 in by_ch: + pan_id, base2 = by_ch[2] + # не затираем None-ами + for k, v in pan_patch.items(): + if v is not None: + base2[k] = v + cams[pan_id] = _apply_defaults(base2) + else: + # подберём свободный id, не наступая на ptz_id + new_id = (max(cams.keys()) + 1) if cams else (1 if ptz_id == 0 else 0) + if new_id == ptz_id: + new_id += 1 + rec2 = {"id": new_id, **pan_patch} + cams[new_id] = _apply_defaults(rec2) + + _write_all(cams) + return ptz_id, cams diff --git a/ptz_tracker_modular/config(main).py b/ptz_tracker_modular/config(main).py new file mode 100644 index 0000000..c003312 --- /dev/null +++ b/ptz_tracker_modular/config(main).py @@ -0,0 +1,486 @@ +from __future__ import annotations +import os +import json +from pathlib import Path +from typing import Any, Dict, List, Tuple + +# ================== CONSTANTS / SETTINGS ================== +# Inference / batching +INFER_IMGSZ = 768 +DRAW_ALL_ON_PANO = True +ADAPTIVE_BATCH_ENABLE = False +BATCH_MIN = 1 #4 +BATCH_MAX = 4 #16 +BATCH_TARGET_MS = 8.0 +BATCH_UP_SCALE = 1.05 +BATCH_DOWN_SCALE = 1.15 + +# ==== Logging toggles ==== +LOG_GEO_CHANGES = True # писать в лог при заметном изменении bearing/attack +LOG_GEO_DEG_STEP = 2.0 # шаг (°) для логирования bearing +LOG_ATTACK_LOG_STEP = 5.0 # шаг (°) для логирования attack_bearing +LOG_STATUS_BATCH = False # если True — broadcaster пишет сводку каждые CONTROL_STATUS_INTERVAL + +# Preview / video publishing +DROP_DECODE_WHEN_BUSY = True +PUBLISH_ONLY_IF_CLIENTS = True +PREVIEW_JPEG_QUALITY = 80 #60 +PREVIEW_TARGET_W = 1280 #896 +DEFAULT_CLIENT_FPS = 25 +# HUD на превью (азимут/дельта/атака) +PREVIEW_DRAW_HUD = False + +VIDEO_MIN_INTERVAL_SLOW = 1.0 / 20.0 +VIDEO_MIN_INTERVAL_FAST = max(1.0 / max(1, DEFAULT_CLIENT_FPS), 1.0 / 30.0) + +# Detection thresholds +MODEL_CONF = 0.65 +DETECT_CONF = 0.55 +ALARM_CONF = 0.55 + +# бэк из твоего примера ждёт массив cameras[], так что лучше БЕЗ {mac} +ALARM_HTTP_ENABLE = True +ALARM_HTTP_MIRROR = True +ALARM_HTTP_BASE = "http://192.168.54.100:3000" +ALARM_HTTP_PATH = "/videodata/10:ff:e0:4f:09:4b" #mac 00%3A11%3A22%3A33%3A44%3A51 +ALARM_HTTP_PATH_TMPL = "/videodata/" +ALARM_MAC_ONLY = False +ALARM_WS_FORMAT = "freq" +ALARM_COUNT_ON = 20 +ALARM_COUNT_OFF = 80 +ALARM_WS_SEND_LEGACY = True +# Компас (направления в логах/OSD) +COMPASS_LANG = "ru" # пока не используется, но оставим на будущее +COMPASS_POINTS = 8 # 8 румбов: С, СВ, В, ... +# Авто-вычисление ref-азимута preset1 от границ панорамы +AUTO_NORTH_FROM_PANORAMA = True + +# Debounce for recording +REC_ON_CONFIRM_SEC = 0.1 +REC_OFF_GRACE_SEC = 1.2 +REC_MIN_ON_SEC = 4.0 +REC_MIN_OFF_SEC = 1.0 +REC_MAX_ON_SEC = 45.0 + + +# Record gate +REC_DET_CONF = 0.45 +REC_MIN_MARGIN = 0.08 +REC_CENTER_MAX = 0.20 +REC_W_MIN, REC_W_MAX = 0.04, 0.55 + +# Recording permissions +RECORD_FROM_PANORAMAS = True +ALLOW_PANORAMA_RECORD = True + +# WebSocket / events +CPP_WS_URI = "ws://192.168.54.122:8767" # incoming video (C++) +CPP_CTRL_WS_URI = "ws://192.168.54.122:8767" # detection events → C++ +VUE_CONTROL_WS_PORT = 8765 +VUE_VIDEO_WS_PORT = 8766 +ALARM_WS_URL = "ws://192.168.54.100:3000/ws" +CONTROL_STATUS_INTERVAL = 0.25 # сек + +# Heartbeat +HEARTBEAT_HOST = "127.0.0.1" +HEARTBEAT_PORT = 55555 +HEARTBEAT_INTERVAL = 0.5 + +# Patrol +PATROL_LEG_SEC = 45.0 #25 +HANDOFF_NOTIFY = True +PATROL_ENABLE_ON_START = True +PUBLISH_RAW_BEFORE_INFER = False + +# Sweep (smooth pass) +SMOOTH_SWEEP_ENABLE = True +SWEEP_PAN_SPEED = 0.04 #0.08 +SWEEP_SETTLE_SEC = 0.20 #0.20 +SWEEP_RAMP_SEC = 0.80 #0.60 +PATROL_SNAP_TO_END = False +END_DWELL_SEC = 0 +SHOT_PAUSE_ENABLE = False +SHOT_INTERVAL_SEC = 0 +SHOT_PAUSE_SEC = 0 + +# Bounded patrol dynamics +PATROL_PAN_GAIN_DEG = 120.0 #30 +PATROL_SPEED_SCALE = 0.70 #0.90 +PATROL_OFFSECTOR_GRACE_SEC = 2.0 +# Starting direction per camera (PTZ only) +PATROL_START_DIR: Dict[int, int] = {0: +1, 2: +1, 4: +1, 6: -1, 8: -1, 10: -1} + +# Sector +SECTOR_PATROL_ENABLE = False +USE_PRESET_EDGES_FOR_SECTOR = True +SECTOR_MARGIN_DEG = 2.0 # было 0.0 — маленький запас вокруг сектора +EDGE_EPS_HARD = 0.35 # было 0.0 — жёсткий край +EDGE_EPS_SOFT = 1 # было 0.0 — мягкая зона замедления у края + +# Classes +CLASS_NAMES = {0: "Fighter", 1: "drone"} +TARGET_CLASSES = [0, 1] + +# Auto strategy +AUTO_STRATEGY = True +SHORT_LOSS_SEC = 5 #1.2 +LONG_LOSS_SEC = 4.0 +ABS_MIN_FALLBACK = 25.0 +ABS_MAX_FALLBACK = 40.0 +FALLBACK_PRESET_CAP: float | None = None + +# Loss/Return +TRACK_END_LOSS_SEC = 0.6 +LOSS_TO_PRESET_SEC = 6.0 +EDGE_LOSS_MARGIN = 0.015 +VERT_LOSS_MARGIN = 0.050 +PRESET_AFTER_LOSS = True +EDGE_LOSS_GRACE_SEC = 1.0 + +# Возврат к пресету, если камера ушла вручную/дрейфует без детекции +IDLE_DRIFT_RETURN_SEC = 4.0 # сколько секунд можно быть вне пресета по панораме +IDLE_TILT_RETURN_SEC = 2.0 # сколько секунд можно быть вне допустимого диапазона по наклону +IDLE_NEAR_TOL_PAN_DEG = 1.2 # допуск «рядом с пресетом» по pan +IDLE_NEAR_TOL_TILT_DEG = 2.5 # допуск «рядом с пресетом» по tilt + +# PID / Zoom / Track +DEAD_ZONE = 0.012 +SMOOTHING_FACTOR = 0.35 +MOVE_GAIN = 1.85 +KI = 0.06 +INTEGRAL_CLAMP = 0.20 +KD = 0.14 +VEL_SMOOTH = 0.75 +BASE_LEAD = 0.14 +LEAD_GAIN = 0.38 +LEAD_MAX = 0.36 +CENTER_TOL = 0.040 +DECAY_FACTOR = 0.85 +MIN_FRAMES_TO_TRACK = 3 + +TRACK_P = 1.25 +TRACK_I = 0.03 +TRACK_D = 0.08 +TRACK_SCALE = 0.75 +TRACK_DEAD_ZONE = 0.022 + +TRACK_EDGE_SLOWDOWN_DEG = 8.0 +TRACK_EDGE_MIN_SCALE = 0.18 +TRACK_OFFSECTOR_GRACE_SEC = 4.0 +IDLE_OFFPRESET_GRACE_SEC = 7.0 + +Z_TARGET_SLOW = 0.16 +Z_TARGET_FAST = 0.12 +Z_VEL_REF = 0.45 +Z_DELTA_IN = 0.008 +Z_DELTA_OUT = 0.060 +Z_DEADBAND = 0.012 #0.010 +Z_P = 2.4 +Z_I = 0.16 +Z_INT_CLAMP = 0.15 +Z_FF_GAIN = 0.55 +Z_SPEED_LIMIT_IN = 0.45 +Z_SPEED_LIMIT_OUT = -0.45 #-0.65 +Z_MIN_CMD = 0.05 +Z_RAMP_TIME = 0.45 +Z_PAN_BOOST = 1.18 +Z_CENTER_GATE_BASE = 0.06 +Z_CENTER_GATE_SCALE = 0.20 +Z_CENTER_STRICT = 0.030 +Z_VELOCITY_GATE = 0.45 +Z_OUT_EDGE = 0.07 +Z_SAFETY_EDGE = 0.05 +Z_SAFETY_TOP_BONUS = 0.50 +Z_MIN_SWITCH_INTERVAL = 0.80 #0.50 +Z_MOTION_THR = 0.015 +Z_HOVER_THR = 0.030 +Z_FREEZE_GROWTH_THR = 0.005 #0.004 +Z_LOCK_ENABLE = True +Z_LOCK_ARM_SECS = 1.2 #0.8 +Z_UNLOCK_VEL_THR = 0.070 #0.060 +Z_UNLOCK_GROWTH_THR = 0.018 #0.015 +Z_UNLOCK_CENTER_THR = 0.12 #0.10 +Z_UNLOCK_EDGE = 0.050 + +SCAN_AMPL = 0.12 +SCAN_STEP = 0.08 +RECOVER_ZOOM_OUT = -0.35 + +PTZ_EPS = 0.008 +PTZ_QUANT = 0.005 + +# Faster aim / zoom +AIM_SMOOTH = 0.40 +AIM_P = 2.30 +AIM_I = 0.06 +AIM_D = 0.22 +RECOVER_ZOOM_OUT_SHORT = 0.0 + +W_EMA_ALPHA = 0.30 +CENTER_GATE_IN = 0.06 +Z_PAN_BOOST_WHEN_IN = 1.35 +Z_LEAD_DURING_IN = 0.65 + +# Video broadcast +SEND_DUPLICATES_WHEN_IDLE = False +ANN_MIN_INTERVAL = 0.0 + +# Sync patrol +SYNC_PATROL_ENABLE = False +SYNC_COHORTS = {"A": {"plus": [0, 2, 4], "minus": [6, 8, 10]}} +SYNC_USE_BOUNDED = False +SYNC_BARRIER_SEC = 0.8 +SYNC_DWELL_SEC = END_DWELL_SEC + +#====================# +PTZ_MIN_INTERVAL_SEC = 0.04 +PTZ_MAX_PAN_I = 8 +PTZ_MAX_TILT_I = 10 +PTZ_MAX_ZOOM_I = 8 +GOTO_SETTLE_SEC = 2.5 +PATROL_MAX_SPEED = 0.14 + +# ================== CAMERA LOADER (TOML/JSON) ================== + +DEFAULT_SECOND_PRESET = "2" + +def _load_cameras_from_file() -> Dict[int, Dict[str, Any]]: + """ + Загружает конфиг камер из TOML/JSON. + - Путь можно задать переменной окружения CAMERAS_FILE. + - По умолчанию ищется 'cameras.toml' рядом с этим модулем. + Ожидается массив объектов с полем `id`. + """ + here = Path(__file__).resolve().parent + path = Path(os.getenv("CAMERAS_FILE", here / "cameras.toml")) + + if not path.exists(): + # fallback: попробуем cameras.json + json_path = here / "cameras.json" + if json_path.exists(): + path = json_path + else: + return {} + + data: Any + if path.suffix.lower() == ".toml": + try: + import tomllib # py311+ + except Exception as e: # pragma: no cover + raise RuntimeError("Для TOML нужен Python 3.11+ (модуль tomllib)") from e + with open(path, "rb") as f: + data = tomllib.load(f) + arr = data.get("camera") or data.get("cameras") or [] + elif path.suffix.lower() == ".json": + with open(path, "r", encoding="utf-8") as f: + data = json.load(f) + arr = data.get("camera") or data.get("cameras") or data + if isinstance(arr, dict): + arr = [{"id": int(k), **v} for k, v in arr.items()] + else: + raise RuntimeError(f"Неподдерживаемое расширение файла камер: {path.suffix}") + + if not isinstance(arr, list): + raise RuntimeError("Файл камер должен содержать список объектов") + + cfg_by_id: Dict[int, Dict[str, Any]] = {} + for item in arr: + if not isinstance(item, dict): + continue + if "id" not in item: + raise RuntimeError("Каждая камера должна иметь поле 'id'") + cid = int(item["id"]) + cfg = dict(item) # copy + + # Back-compat: если указали единый preset — дополнить вторым + if "preset" in cfg and cfg.get("preset") not in (None, "None"): + cfg.setdefault("preset1", cfg["preset"]) + cfg.setdefault("preset2", DEFAULT_SECOND_PRESET) + + # Значения по умолчанию + cfg.setdefault("ptz_channel", 1) + cfg.setdefault("preset1", None) + cfg.setdefault("preset2", None) + + cfg_by_id[cid] = cfg + + return cfg_by_id + +# === Загрузка и пост-обработка === +CAMERA_CONFIG: Dict[int, Dict[str, Any]] = _load_cameras_from_file() + +# Перекрытие паролей через переменные окружения CAM__PASSWORD +for cid, cfg in CAMERA_CONFIG.items(): + env_pw = os.getenv(f"CAM_{cid}_PASSWORD") + if env_pw: + cfg["password"] = env_pw + +# Список PTZ-камер (есть preset1 и preset2) +PTZ_CAM_IDS: List[int] = [cid for cid, cfg in CAMERA_CONFIG.items() if cfg.get("preset1") and cfg.get("preset2")] +PTZ_CAM_IDS.sort() + +# Значения по умолчанию для PTZ +for cam_id in PTZ_CAM_IDS: + cfg = CAMERA_CONFIG[cam_id] + cfg.setdefault("preset1_deg", None) + cfg.setdefault("preset2_deg", None) + cfg.setdefault("preset1_tilt_deg", None) + cfg.setdefault("preset2_tilt_deg", None) + cfg.setdefault("sector_min_deg", None) + cfg.setdefault("sector_max_deg", None) + cfg.setdefault("pan_sign", cfg.get("sweep_sign", 1)) + cfg.setdefault("tilt_sign", -1) + cfg.setdefault("pan_offset_deg", 0.0) + cfg.setdefault("tilt_offset_deg", 0.0) + # Гео-поля для привязки к северу и UI + cfg.setdefault("north_offset_deg", None) + cfg.setdefault("preset1_deg_geo", None) + cfg.setdefault("preset2_deg_geo", None) + cfg.setdefault("sector_min_deg_geo", None) + cfg.setdefault("sector_max_deg_geo", None) + +# Значения по умолчанию для «пуль»/панорам (без PTZ-пресетов) +for cid, cfg in CAMERA_CONFIG.items(): + if not (cfg.get("preset1") and cfg.get("preset2")): + # Для DS-2SF8C442MXS-DL(14F1)(P3) панорама (канал 2) ~= 85–86°. + # Явно прописывай в cameras.toml: bullet_hfov_deg = 85.5 + cfg.setdefault("bullet_hfov_deg", 90.0) + cfg.setdefault("bullet_vfov_deg", 65.0) + +# Panorama ↔ PTZ pairing по IP и каналам +PAN_TO_PTZ: Dict[int, int] = {} +PTZ_TO_PAN: Dict[int, int] = {} +_by_ip: Dict[str, Dict[int, int]] = {} +for cam_id, cfg in CAMERA_CONFIG.items(): + ip = cfg["ip"]; ch = int(cfg.get("ptz_channel", 1)) + _by_ip.setdefault(ip, {})[ch] = cam_id +for ip, chmap in _by_ip.items(): + ptz_id = chmap.get(1) + pano_id = chmap.get(2) + if ptz_id is not None and pano_id is not None: + PAN_TO_PTZ[pano_id] = ptz_id + PTZ_TO_PAN[ptz_id] = pano_id + +# PTZ order (circular) +PTZ_ORDER: List[int] = PTZ_CAM_IDS[:] +NEXT_FWD: Dict[int, int] = {} +NEXT_REV: Dict[int, int] = {} +if PTZ_ORDER: + n = len(PTZ_ORDER) + for i, cam in enumerate(PTZ_ORDER): + NEXT_FWD[cam] = PTZ_ORDER[(i + 1) % n] + NEXT_REV[cam] = PTZ_ORDER[(i - 1) % n] + +# Cohorts config → static maps (runtime sync_state lives in state.py) +sync_group_by_cam: Dict[int, str] = {} +cohort_sign_by_cam: Dict[int, int] = {} +for gname, coh in (SYNC_COHORTS or {}).items(): + for cid in coh.get("plus", []): + sync_group_by_cam[cid] = gname + cohort_sign_by_cam[cid] = +1 + for cid in coh.get("minus", []): + sync_group_by_cam[cid] = gname + cohort_sign_by_cam[cid] = -1 + +# ========= Утилита для hot-reload ========= +def reload_cameras() -> None: + """Перечитать cameras.toml/JSON и обновить производные структуры.""" + global CAMERA_CONFIG, PTZ_CAM_IDS, PAN_TO_PTZ, PTZ_TO_PAN, PTZ_ORDER, NEXT_FWD, NEXT_REV + global sync_group_by_cam, cohort_sign_by_cam + + cfg = _load_cameras_from_file() + + # env override паролей + for cid, c in cfg.items(): + env_pw = os.getenv(f"CAM_{cid}_PASSWORD") + if env_pw: + c["password"] = env_pw + + CAMERA_CONFIG = cfg + + PTZ_CAM_IDS = [cid for cid, c in CAMERA_CONFIG.items() if c.get("preset1") and c.get("preset2")] + PTZ_CAM_IDS.sort() + + for cam_id in PTZ_CAM_IDS: + c = CAMERA_CONFIG[cam_id] + c.setdefault("preset1_deg", None) + c.setdefault("preset2_deg", None) + c.setdefault("preset1_tilt_deg", None) + c.setdefault("preset2_tilt_deg", None) + c.setdefault("sector_min_deg", None) + c.setdefault("sector_max_deg", None) + c.setdefault("pan_sign", c.get("sweep_sign", 1)) + c.setdefault("tilt_sign", 1) + c.setdefault("pan_offset_deg", 0.0) + c.setdefault("tilt_offset_deg", 0.0) + c.setdefault("north_offset_deg", None) + c.setdefault("preset1_deg_geo", None) + c.setdefault("preset2_deg_geo", None) + c.setdefault("sector_min_deg_geo", None) + c.setdefault("sector_max_deg_geo", None) + + for cid, c in CAMERA_CONFIG.items(): + if not (c.get("preset1") and c.get("preset2")): + c.setdefault("bullet_hfov_deg", 90.0) + c.setdefault("bullet_vfov_deg", 65.0) + + PAN_TO_PTZ = {} + PTZ_TO_PAN = {} + by_ip: Dict[str, Dict[int, int]] = {} + for cam_id, c in CAMERA_CONFIG.items(): + ip = c["ip"]; ch = int(c.get("ptz_channel", 1)) + by_ip.setdefault(ip, {})[ch] = cam_id + for ip, chmap in by_ip.items(): + ptz_id = chmap.get(1) + pano_id = chmap.get(2) + if ptz_id is not None and pano_id is not None: + PAN_TO_PTZ[pano_id] = ptz_id + PTZ_TO_PAN[ptz_id] = pano_id + + PTZ_ORDER = PTZ_CAM_IDS[:] + NEXT_FWD = {} + NEXT_REV = {} + if PTZ_ORDER: + n = len(PTZ_ORDER) + for i, cam in enumerate(PTZ_ORDER): + NEXT_FWD[cam] = PTZ_ORDER[(i + 1) % n] + NEXT_REV[cam] = PTZ_ORDER[(i - 1) % n] + +# ===== Helpers for compass-bearing (used by postprocess.py) ===== + +def get_cam_base_azimuth_deg(cam_id: int) -> float: + """ + Базовый азимут камеры (0°=С, 90°=В). Это направление центрального пикселя + (для PTZ — при текущем pan=az_deg, для панорамы — просто «куда смотрит» середина кадра). + Берётся из cameras.toml: north_offset_deg. + """ + cfg = CAMERA_CONFIG.get(cam_id, {}) + north = cfg.get("north_offset_deg") + if not isinstance(north, (int, float)): + return 0.0 + + is_ptz = bool(cfg.get("preset1")) and cfg.get("preset2") + if not is_ptz: + return float(north) + + try: + from . import state + st = state.ptz_states.get(cam_id, {}) + pan = st.get("az_deg") + if isinstance(pan, (int, float)): + return (float(north) + float(pan)) % 360.0 + except Exception: + pass + return float(north) + +def get_cam_hfov_deg(cam_id: int) -> float: + """ + Горизонтальный FOV камеры. Для панорамы/«пули» — bullet_hfov_deg (или hfov_deg). + Для PTZ тоже можно прописать hfov_deg, если хотим считать bearing по x. + """ + cfg = CAMERA_CONFIG.get(cam_id, {}) + for key in ("bullet_hfov_deg", "hfov_deg"): + v = cfg.get(key) + if isinstance(v, (int, float)) and v > 0: + return float(v) + return 90.0 diff --git a/ptz_tracker_modular/config.py b/ptz_tracker_modular/config.py new file mode 100644 index 0000000..3b2a924 --- /dev/null +++ b/ptz_tracker_modular/config.py @@ -0,0 +1,497 @@ +from __future__ import annotations +import os +import json +from pathlib import Path +from typing import Any, Dict, List, Tuple + +# ================== CONSTANTS / SETTINGS ================== +# Inference / batching +INFER_IMGSZ = 960 +DRAW_ALL_ON_PANO = True +ADAPTIVE_BATCH_ENABLE = False +BATCH_MIN = 1 #4 +BATCH_MAX = 4 #16 +BATCH_TARGET_MS = 8.0 +BATCH_UP_SCALE = 1.05 +BATCH_DOWN_SCALE = 1.15 + +# ==== Logging toggles ==== +LOG_GEO_CHANGES = True # писать в лог при заметном изменении bearing/attack +LOG_GEO_DEG_STEP = 2.0 # шаг (°) для логирования bearing +LOG_ATTACK_LOG_STEP = 5.0 # шаг (°) для логирования attack_bearing +LOG_STATUS_BATCH = False # если True — broadcaster пишет сводку каждые CONTROL_STATUS_INTERVAL + +# Preview / video publishing +DROP_DECODE_WHEN_BUSY = True +PUBLISH_ONLY_IF_CLIENTS = True +PREVIEW_JPEG_QUALITY = 60 #60 +PREVIEW_TARGET_W = 896 #896 +DEFAULT_CLIENT_FPS = 25 +# HUD на превью (азимут/дельта/атака) +PREVIEW_DRAW_HUD = False + +VIDEO_MIN_INTERVAL_SLOW = 1.0 / 20.0 +VIDEO_MIN_INTERVAL_FAST = max(1.0 / max(1, DEFAULT_CLIENT_FPS), 1.0 / 30.0) + +# Detection thresholds +MODEL_CONF = 0.65 +DETECT_CONF = 0.55 +ALARM_CONF = 0.65 + +# бэк из твоего примера ждёт массив cameras[], так что лучше БЕЗ {mac} +ALARM_HTTP_ENABLE = True +ALARM_HTTP_MIRROR = True +ALARM_HTTP_BASE = "http://192.168.54.100/api/core" +ALARM_HTTP_PATH = "/videodata/10:ff:e0:4f:09:4b" #mac 00%3A11%3A22%3A33%3A44%3A51 +ALARM_HTTP_PATH_TMPL = "/videodata" +ALARM_MAC_ONLY = False +ALARM_WS_FORMAT = "freq" +ALARM_COUNT_ON = 20 +ALARM_COUNT_OFF = 80 +ALARM_WS_SEND_LEGACY = True +# Компас (направления в логах/OSD) +COMPASS_LANG = "ru" # пока не используется, но оставим на будущее +COMPASS_POINTS = 8 # 8 румбов: С, СВ, В, ... +# Авто-вычисление ref-азимута preset1 от границ панорамы +AUTO_NORTH_FROM_PANORAMA = True + +# Debounce for recording +REC_ON_CONFIRM_SEC = 0.1 +REC_OFF_GRACE_SEC = 1.2 +REC_MIN_ON_SEC = 4.0 +REC_MIN_OFF_SEC = 1.0 +REC_MAX_ON_SEC = 45.0 + + +# Record gate +REC_DET_CONF = 0.45 +REC_MIN_MARGIN = 0.08 +REC_CENTER_MAX = 0.20 +REC_W_MIN, REC_W_MAX = 0.04, 0.55 + +# Recording permissions +RECORD_FROM_PANORAMAS = True +ALLOW_PANORAMA_RECORD = True + +# WebSocket / events +CPP_WS_URI = "ws://192.168.54.122:8767" # incoming video (C++) +CPP_CTRL_WS_URI = "ws://192.168.54.122:8767" # detection events → C++ +VUE_CONTROL_WS_PORT = 8765 +VUE_VIDEO_WS_PORT = 8766 +ALARM_WS_URL = "ws://192.168.54.100/api/core/ws" +CONTROL_STATUS_INTERVAL = 0.25 # сек + +# Heartbeat +HEARTBEAT_HOST = "127.0.0.1" +HEARTBEAT_PORT = 55555 +HEARTBEAT_INTERVAL = 0.5 + +# Patrol +PATROL_LEG_SEC = 45.0 #25 +HANDOFF_NOTIFY = True +PATROL_ENABLE_ON_START = True +PUBLISH_RAW_BEFORE_INFER = False + +# Sweep (smooth pass) +SMOOTH_SWEEP_ENABLE = True +SWEEP_PAN_SPEED = 0.04 #0.08 +SWEEP_SETTLE_SEC = 0.20 #0.20 +SWEEP_RAMP_SEC = 0.80 #0.60 +PATROL_SNAP_TO_END = False +END_DWELL_SEC = 0 +SHOT_PAUSE_ENABLE = False +SHOT_INTERVAL_SEC = 0 +SHOT_PAUSE_SEC = 0 + +# Bounded patrol dynamics +PATROL_PAN_GAIN_DEG = 120.0 #30 +PATROL_SPEED_SCALE = 0.70 #0.90 +PATROL_OFFSECTOR_GRACE_SEC = 2.0 +# Starting direction per camera (PTZ only) +PATROL_START_DIR: Dict[int, int] = {0: +1, 2: +1, 4: +1, 6: -1, 8: -1, 10: -1} + +# Sector +SECTOR_PATROL_ENABLE = False +USE_PRESET_EDGES_FOR_SECTOR = True +SECTOR_MARGIN_DEG = 2.0 # было 0.0 — маленький запас вокруг сектора +EDGE_EPS_HARD = 0.35 # было 0.0 — жёсткий край +EDGE_EPS_SOFT = 1 # было 0.0 — мягкая зона замедления у края + +# Classes +CLASS_NAMES = {0: "Fighter", 1: "drone"} +TARGET_CLASSES = [0, 1] + +# Auto strategy +AUTO_STRATEGY = True +SHORT_LOSS_SEC = 0.25 #1.2 +LONG_LOSS_SEC = 1.20 #4/0 +ABS_MIN_FALLBACK = 25.0 +ABS_MAX_FALLBACK = 40.0 +FALLBACK_PRESET_CAP: float | None = None + +# Loss/Return +TRACK_END_LOSS_SEC = 2.5 +LOSS_TO_PRESET_SEC = 8.0 +EDGE_LOSS_MARGIN = 0.015 +VERT_LOSS_MARGIN = 0.050 +PRESET_AFTER_LOSS = True +EDGE_LOSS_GRACE_SEC = 1.2 + +# Возврат к пресету, если камера ушла вручную/дрейфует без детекции +IDLE_DRIFT_RETURN_SEC = 4.0 # сколько секунд можно быть вне пресета по панораме +IDLE_TILT_RETURN_SEC = 2.0 # сколько секунд можно быть вне допустимого диапазона по наклону +IDLE_NEAR_TOL_PAN_DEG = 1.2 # допуск «рядом с пресетом» по pan +IDLE_NEAR_TOL_TILT_DEG = 2.5 # допуск «рядом с пресетом» по tilt + +# PID / Zoom / Track +DEAD_ZONE = 0.010 +SMOOTHING_FACTOR = 0.35 +MOVE_GAIN = 1.85 +KI = 0.06 +INTEGRAL_CLAMP = 0.20 +KD = 0.14 +VEL_SMOOTH = 0.60 +BASE_LEAD = 0.14 +LEAD_GAIN = 0.55 +LEAD_MAX = 0.55 +CENTER_TOL = 0.040 +DECAY_FACTOR = 0.85 +MIN_FRAMES_TO_TRACK = 3 +LOSS_DECAY_TAU = 0.35 + +#НОВЫЕ +LEAD_OFF_ERR = 0.18 +Z_ERR_GATE = 0.18 +PRED_CLAMP_MIN = 0.05 +PRED_CLAMP_MAX = 0.95 +PRED_DX_CLAMP = 0.35 +W_GROWTH_CLAMP = 2.0 + + +TRACK_P = 1.25 +TRACK_I = 0.03 +TRACK_D = 0.08 +TRACK_SCALE = 0.75 +TRACK_DEAD_ZONE = 0.022 + +TRACK_EDGE_SLOWDOWN_DEG = 8.0 +TRACK_EDGE_MIN_SCALE = 0.18 +TRACK_OFFSECTOR_GRACE_SEC = 4.0 +IDLE_OFFPRESET_GRACE_SEC = 7.0 + +Z_TARGET_SLOW = 0.16 +Z_TARGET_FAST = 0.12 +Z_VEL_REF = 0.45 +Z_DELTA_IN = 0.008 +Z_DELTA_OUT = 0.060 +Z_DEADBAND = 0.012 #0.010 +Z_P = 2.4 +Z_I = 0.16 +Z_INT_CLAMP = 0.15 +Z_FF_GAIN = 0.55 +Z_SPEED_LIMIT_IN = 0.45 +Z_SPEED_LIMIT_OUT = -0.65 #-0.45 +Z_MIN_CMD = 0.05 +Z_RAMP_TIME = 0.25 #0.45 +Z_PAN_BOOST = 1.18 +Z_CENTER_GATE_BASE = 0.06 +Z_CENTER_GATE_SCALE = 0.20 +Z_CENTER_STRICT = 0.030 +Z_VELOCITY_GATE = 0.30 +Z_OUT_EDGE = 0.07 +Z_SAFETY_EDGE = 0.05 +Z_SAFETY_TOP_BONUS = 0.50 +Z_MIN_SWITCH_INTERVAL = 0.50 #0.80 +Z_MOTION_THR = 0.015 +Z_HOVER_THR = 0.030 +Z_FREEZE_GROWTH_THR = 0.005 #0.004 +Z_LOCK_ENABLE = True +Z_LOCK_ARM_SECS = 1.2 #0.8 +Z_UNLOCK_VEL_THR = 0.070 #0.060 +Z_UNLOCK_GROWTH_THR = 0.018 #0.015 +W_GROWTH_CLAMP = 2.0 # clamp for w_growth derivative (stability) +Z_UNLOCK_CENTER_THR = 0.12 #0.10 +Z_UNLOCK_EDGE = 0.050 + +SCAN_AMPL = 0.12 +SCAN_STEP = 0.08 +RECOVER_ZOOM_OUT = -0.55 #-0.35 + +PTZ_EPS = 0.008 +PTZ_QUANT = 0.005 + +# Faster aim / zoom +AIM_SMOOTH = 0.38 +AIM_P = 2.40 +AIM_I = 0.06 +AIM_D = 0.30 +RECOVER_ZOOM_OUT_SHORT = 0.0 + +W_EMA_ALPHA = 0.30 +CENTER_GATE_IN = 0.06 +Z_PAN_BOOST_WHEN_IN = 1.35 +Z_LEAD_DURING_IN = 0.65 + +# Video broadcast +SEND_DUPLICATES_WHEN_IDLE = False +ANN_MIN_INTERVAL = 0.0 + +# Sync patrol +SYNC_PATROL_ENABLE = False +SYNC_COHORTS = {"A": {"plus": [0, 2, 4], "minus": [6, 8, 10]}} +SYNC_USE_BOUNDED = False +SYNC_BARRIER_SEC = 0.8 +SYNC_DWELL_SEC = END_DWELL_SEC + +#====================# +PTZ_MIN_INTERVAL_SEC = 0.04 +PTZ_MAX_PAN_I = 8 +PTZ_MAX_TILT_I = 10 +PTZ_MAX_ZOOM_I = 8 +GOTO_SETTLE_SEC = 2.5 +PATROL_MAX_SPEED = 0.14 + +# ================== CAMERA LOADER (TOML/JSON) ================== + +DEFAULT_SECOND_PRESET = "2" + +def _load_cameras_from_file() -> Dict[int, Dict[str, Any]]: + """ + Загружает конфиг камер из TOML/JSON. + - Путь можно задать переменной окружения CAMERAS_FILE. + - По умолчанию ищется 'cameras.toml' рядом с этим модулем. + Ожидается массив объектов с полем `id`. + """ + here = Path(__file__).resolve().parent + path = Path(os.getenv("CAMERAS_FILE", here / "cameras.toml")) + + if not path.exists(): + # fallback: попробуем cameras.json + json_path = here / "cameras.json" + if json_path.exists(): + path = json_path + else: + return {} + + data: Any + if path.suffix.lower() == ".toml": + try: + import tomllib # py311+ + except Exception as e: # pragma: no cover + raise RuntimeError("Для TOML нужен Python 3.11+ (модуль tomllib)") from e + with open(path, "rb") as f: + data = tomllib.load(f) + arr = data.get("camera") or data.get("cameras") or [] + elif path.suffix.lower() == ".json": + with open(path, "r", encoding="utf-8") as f: + data = json.load(f) + arr = data.get("camera") or data.get("cameras") or data + if isinstance(arr, dict): + arr = [{"id": int(k), **v} for k, v in arr.items()] + else: + raise RuntimeError(f"Неподдерживаемое расширение файла камер: {path.suffix}") + + if not isinstance(arr, list): + raise RuntimeError("Файл камер должен содержать список объектов") + + cfg_by_id: Dict[int, Dict[str, Any]] = {} + for item in arr: + if not isinstance(item, dict): + continue + if "id" not in item: + raise RuntimeError("Каждая камера должна иметь поле 'id'") + cid = int(item["id"]) + cfg = dict(item) # copy + + # Back-compat: если указали единый preset — дополнить вторым + if "preset" in cfg and cfg.get("preset") not in (None, "None"): + cfg.setdefault("preset1", cfg["preset"]) + cfg.setdefault("preset2", DEFAULT_SECOND_PRESET) + + # Значения по умолчанию + cfg.setdefault("ptz_channel", 1) + cfg.setdefault("preset1", None) + cfg.setdefault("preset2", None) + + cfg_by_id[cid] = cfg + + return cfg_by_id + +# === Загрузка и пост-обработка === +CAMERA_CONFIG: Dict[int, Dict[str, Any]] = _load_cameras_from_file() + +# Перекрытие паролей через переменные окружения CAM__PASSWORD +for cid, cfg in CAMERA_CONFIG.items(): + env_pw = os.getenv(f"CAM_{cid}_PASSWORD") + if env_pw: + cfg["password"] = env_pw + +# Список PTZ-камер (есть preset1 и preset2) +PTZ_CAM_IDS: List[int] = [cid for cid, cfg in CAMERA_CONFIG.items() if cfg.get("preset1") and cfg.get("preset2")] +PTZ_CAM_IDS.sort() + +# Значения по умолчанию для PTZ +for cam_id in PTZ_CAM_IDS: + cfg = CAMERA_CONFIG[cam_id] + cfg.setdefault("preset1_deg", None) + cfg.setdefault("preset2_deg", None) + cfg.setdefault("preset1_tilt_deg", None) + cfg.setdefault("preset2_tilt_deg", None) + cfg.setdefault("sector_min_deg", None) + cfg.setdefault("sector_max_deg", None) + cfg.setdefault("pan_sign", cfg.get("sweep_sign", 1)) + cfg.setdefault("tilt_sign", -1) + cfg.setdefault("pan_offset_deg", 0.0) + cfg.setdefault("tilt_offset_deg", 0.0) + # Гео-поля для привязки к северу и UI + cfg.setdefault("north_offset_deg", None) + cfg.setdefault("preset1_deg_geo", None) + cfg.setdefault("preset2_deg_geo", None) + cfg.setdefault("sector_min_deg_geo", None) + cfg.setdefault("sector_max_deg_geo", None) + +# Значения по умолчанию для «пуль»/панорам (без PTZ-пресетов) +for cid, cfg in CAMERA_CONFIG.items(): + if not (cfg.get("preset1") and cfg.get("preset2")): + # Для DS-2SF8C442MXS-DL(14F1)(P3) панорама (канал 2) ~= 85–86°. + # Явно прописывай в cameras.toml: bullet_hfov_deg = 85.5 + cfg.setdefault("bullet_hfov_deg", 90.0) + cfg.setdefault("bullet_vfov_deg", 65.0) + +# Panorama ↔ PTZ pairing по IP и каналам +PAN_TO_PTZ: Dict[int, int] = {} +PTZ_TO_PAN: Dict[int, int] = {} +_by_ip: Dict[str, Dict[int, int]] = {} +for cam_id, cfg in CAMERA_CONFIG.items(): + ip = cfg["ip"]; ch = int(cfg.get("ptz_channel", 1)) + _by_ip.setdefault(ip, {})[ch] = cam_id +for ip, chmap in _by_ip.items(): + ptz_id = chmap.get(1) + pano_id = chmap.get(2) + if ptz_id is not None and pano_id is not None: + PAN_TO_PTZ[pano_id] = ptz_id + PTZ_TO_PAN[ptz_id] = pano_id + +# PTZ order (circular) +PTZ_ORDER: List[int] = PTZ_CAM_IDS[:] +NEXT_FWD: Dict[int, int] = {} +NEXT_REV: Dict[int, int] = {} +if PTZ_ORDER: + n = len(PTZ_ORDER) + for i, cam in enumerate(PTZ_ORDER): + NEXT_FWD[cam] = PTZ_ORDER[(i + 1) % n] + NEXT_REV[cam] = PTZ_ORDER[(i - 1) % n] + +# Cohorts config → static maps (runtime sync_state lives in state.py) +sync_group_by_cam: Dict[int, str] = {} +cohort_sign_by_cam: Dict[int, int] = {} +for gname, coh in (SYNC_COHORTS or {}).items(): + for cid in coh.get("plus", []): + sync_group_by_cam[cid] = gname + cohort_sign_by_cam[cid] = +1 + for cid in coh.get("minus", []): + sync_group_by_cam[cid] = gname + cohort_sign_by_cam[cid] = -1 + +# ========= Утилита для hot-reload ========= +def reload_cameras() -> None: + """Перечитать cameras.toml/JSON и обновить производные структуры.""" + global CAMERA_CONFIG, PTZ_CAM_IDS, PAN_TO_PTZ, PTZ_TO_PAN, PTZ_ORDER, NEXT_FWD, NEXT_REV + global sync_group_by_cam, cohort_sign_by_cam + + cfg = _load_cameras_from_file() + + # env override паролей + for cid, c in cfg.items(): + env_pw = os.getenv(f"CAM_{cid}_PASSWORD") + if env_pw: + c["password"] = env_pw + + CAMERA_CONFIG = cfg + + PTZ_CAM_IDS = [cid for cid, c in CAMERA_CONFIG.items() if c.get("preset1") and c.get("preset2")] + PTZ_CAM_IDS.sort() + + for cam_id in PTZ_CAM_IDS: + c = CAMERA_CONFIG[cam_id] + c.setdefault("preset1_deg", None) + c.setdefault("preset2_deg", None) + c.setdefault("preset1_tilt_deg", None) + c.setdefault("preset2_tilt_deg", None) + c.setdefault("sector_min_deg", None) + c.setdefault("sector_max_deg", None) + c.setdefault("pan_sign", c.get("sweep_sign", 1)) + c.setdefault("tilt_sign", 1) + c.setdefault("pan_offset_deg", 0.0) + c.setdefault("tilt_offset_deg", 0.0) + c.setdefault("north_offset_deg", None) + c.setdefault("preset1_deg_geo", None) + c.setdefault("preset2_deg_geo", None) + c.setdefault("sector_min_deg_geo", None) + c.setdefault("sector_max_deg_geo", None) + + for cid, c in CAMERA_CONFIG.items(): + if not (c.get("preset1") and c.get("preset2")): + c.setdefault("bullet_hfov_deg", 90.0) + c.setdefault("bullet_vfov_deg", 65.0) + + PAN_TO_PTZ = {} + PTZ_TO_PAN = {} + by_ip: Dict[str, Dict[int, int]] = {} + for cam_id, c in CAMERA_CONFIG.items(): + ip = c["ip"]; ch = int(c.get("ptz_channel", 1)) + by_ip.setdefault(ip, {})[ch] = cam_id + for ip, chmap in by_ip.items(): + ptz_id = chmap.get(1) + pano_id = chmap.get(2) + if ptz_id is not None and pano_id is not None: + PAN_TO_PTZ[pano_id] = ptz_id + PTZ_TO_PAN[ptz_id] = pano_id + + PTZ_ORDER = PTZ_CAM_IDS[:] + NEXT_FWD = {} + NEXT_REV = {} + if PTZ_ORDER: + n = len(PTZ_ORDER) + for i, cam in enumerate(PTZ_ORDER): + NEXT_FWD[cam] = PTZ_ORDER[(i + 1) % n] + NEXT_REV[cam] = PTZ_ORDER[(i - 1) % n] + +# ===== Helpers for compass-bearing (used by postprocess.py) ===== + +def get_cam_base_azimuth_deg(cam_id: int) -> float: + """ + Базовый азимут камеры (0°=С, 90°=В). Это направление центрального пикселя + (для PTZ — при текущем pan=az_deg, для панорамы — просто «куда смотрит» середина кадра). + Берётся из cameras.toml: north_offset_deg. + """ + cfg = CAMERA_CONFIG.get(cam_id, {}) + north = cfg.get("north_offset_deg") + if not isinstance(north, (int, float)): + return 0.0 + + is_ptz = bool(cfg.get("preset1")) and cfg.get("preset2") + if not is_ptz: + return float(north) + + try: + from . import state + st = state.ptz_states.get(cam_id, {}) + pan = st.get("az_deg") + if isinstance(pan, (int, float)): + return (float(north) + float(pan)) % 360.0 + except Exception: + pass + return float(north) + +def get_cam_hfov_deg(cam_id: int) -> float: + """ + Горизонтальный FOV камеры. Для панорамы/«пули» — bullet_hfov_deg (или hfov_deg). + Для PTZ тоже можно прописать hfov_deg, если хотим считать bearing по x. + """ + cfg = CAMERA_CONFIG.get(cam_id, {}) + for key in ("bullet_hfov_deg", "hfov_deg"): + v = cfg.get(key) + if isinstance(v, (int, float)) and v > 0: + return float(v) + return 90.0 diff --git a/ptz_tracker_modular/cpp_bridge.py b/ptz_tracker_modular/cpp_bridge.py new file mode 100644 index 0000000..55eae73 --- /dev/null +++ b/ptz_tracker_modular/cpp_bridge.py @@ -0,0 +1,165 @@ +from __future__ import annotations +import asyncio +import base64 +import logging +import time +from typing import List, Tuple + +import numpy as np +import cv2 +import websockets + +from config import CPP_WS_URI, CPP_CTRL_WS_URI, DROP_DECODE_WHEN_BUSY, PUBLISH_RAW_BEFORE_INFER, PREVIEW_TARGET_W +from utils import json_loads, json_dumps +from state import detected_cameras, video_clients, ptz_states, detection_queue +from media import publish_preview, maybe_downscale +from detection import batcher + +logger = logging.getLogger("PTZTracker") + +async def cpp_ws_loop() -> None: + frames_total = 0 + skipped_total = 0 + last_log = time.time() + + while True: + try: + logger.info("[CPP WS] connecting to %s ...", CPP_WS_URI) + async with websockets.connect( + CPP_WS_URI, max_size=None, compression=None, ping_interval=None, close_timeout=1.0 + ) as ws: + logger.info("[CPP WS] connected") + async for raw in ws: + if isinstance(raw, (bytes, bytearray)): + buf = memoryview(raw) + if len(buf) < 3 or buf[0] != 1: + continue + idx = int.from_bytes(buf[1:3], "big") + from config import CAMERA_CONFIG + if idx not in CAMERA_CONFIG: + continue + if DROP_DECODE_WHEN_BUSY and ptz_states[idx]["proc_busy"] and (not video_clients): + skipped_total += 1 + continue + arr = np.frombuffer(buf[3:], dtype=np.uint8) + img = cv2.imdecode(arr, cv2.IMREAD_COLOR) + if img is None: + continue + else: + try: + msg = json_loads(raw) + except Exception: + continue + if msg.get("type") != "image": + continue + idx_str, b64 = msg["data"].split("|", 1) + idx = int(idx_str) + from config import CAMERA_CONFIG + if idx not in CAMERA_CONFIG: + continue + if DROP_DECODE_WHEN_BUSY and ptz_states[idx]["proc_busy"] and (not video_clients): + skipped_total += 1 + continue + arr = np.frombuffer(base64.b64decode(b64), dtype=np.uint8) + img = cv2.imdecode(arr, cv2.IMREAD_COLOR) + if img is None: + continue + + frames_total += 1 + h, w = img.shape[:2] + now = time.time() + if now - last_log >= 1.0: + logger.info("[CPP WS] recv: %d fps, skipped=%d | last cam=%s | %dx%d | clients=%d", + frames_total, skipped_total, idx, w, h, len(video_clients)) + frames_total = 0; skipped_total = 0; last_log = now + + detected_cameras.add(idx) + + if PUBLISH_RAW_BEFORE_INFER and video_clients: + down, _, _ = maybe_downscale(img, PREVIEW_TARGET_W) + publish_preview(idx, down) + + pst = ptz_states[idx] + if pst["proc_busy"]: + continue + pst["proc_busy"] = True + + async def process_frame(cam_idx: int, frame): + try: + assert batcher is not None, "batcher is not initialized" + await batcher.submit(cam_idx, frame, cam_idx in from_state_ptz_ids()) + except Exception as exc: + logger.error("Error processing frame for camera %s: %s", cam_idx, exc) + finally: + ptz_states[cam_idx]["proc_busy"] = False + + # helper to avoid circular import at top-level + def from_state_ptz_ids(): + from state import PTZ_CAM_IDS + return PTZ_CAM_IDS + + asyncio.create_task(process_frame(idx, img)) + except Exception as exc: + logger.error("[CPP WS] error: %s", exc) + await asyncio.sleep(0.5) + +async def cpp_detection_loop() -> None: + assert detection_queue is not None + RESYNC_EVERY_SEC = 10.0 + + def _snapshot_states(): + from config import CAMERA_CONFIG + snap = [] + for cid in CAMERA_CONFIG.keys(): + st = ptz_states.get(cid, {}) + snap.append({"IdCamera": int(cid), "detection": bool(st.get("rec_active", False))}) + return snap + + while True: + try: + logger.info("[CPP CTRL] connecting to %s ...", CPP_CTRL_WS_URI) + async with websockets.connect( + CPP_CTRL_WS_URI, max_size=None, compression=None, ping_interval=None, close_timeout=1.0 + ) as ws: + logger.info("[CPP CTRL] connected") + try: + for item in _snapshot_states(): + await ws.send(json_dumps({"type": "detection", "data": item})) + except Exception as e: + logger.error("[CPP CTRL] initial resync failed: %s", e) + raise + + last_resync = time.time() + + while True: + now = time.time() + if now - last_resync >= RESYNC_EVERY_SEC and detection_queue.empty(): + try: + for item in _snapshot_states(): + await ws.send(json_dumps({"type": "detection", "data": item})) + except Exception as e: + logger.error("[CPP CTRL] periodic resync failed: %s", e) + break + last_resync = now + + try: + payload = await asyncio.wait_for(detection_queue.get(), timeout=1.0) + except asyncio.TimeoutError: + continue + + try: + await ws.send(json_dumps(payload)) + last_resync = time.time() + except Exception as exc: + logger.error("[CPP CTRL] send failed: %s", exc) + try: + detection_queue.put_nowait(payload) + except Exception: + pass + break + finally: + detection_queue.task_done() + + except Exception as exc: + logger.error("[CPP CTRL] error: %s", exc) + await asyncio.sleep(1.0) diff --git a/ptz_tracker_modular/detection.py b/ptz_tracker_modular/detection.py new file mode 100644 index 0000000..1712a64 --- /dev/null +++ b/ptz_tracker_modular/detection.py @@ -0,0 +1,42 @@ +# ptz_tracker_modular/detection.py +# -*- coding: utf-8 -*- +from __future__ import annotations +import os as _os + +from .config import ( + INFER_IMGSZ, MODEL_CONF, TARGET_CLASSES, + W_EMA_ALPHA, DEFAULT_CLIENT_FPS, DRAW_ALL_ON_PANO, + DETECT_CONF, ALARM_CONF, RECOVER_ZOOM_OUT, + CENTER_GATE_IN, AIM_SMOOTH, AIM_P, AIM_I, AIM_D, + AUTO_STRATEGY, SHORT_LOSS_SEC, LONG_LOSS_SEC, + ABS_MIN_FALLBACK, ABS_MAX_FALLBACK, +) +# Сохранение старого имени для веса модели +YOLO_WEIGHTS = _os.getenv("YOLO_WEIGHTS", "C:/Users/Legion/Desktop/weight/electro-pribory-new.pt") + +from .utils import clipf +from .state import ptz_states, detect_stats_global, detect_stats_cam, video_clients, batcher as _batcher +from .preview import publish_preview, maybe_downscale +from .postprocess import notify_detected, postprocess_control +from .events import queue_alarm +from .ptz_io import move_ptz_bounded, stop_ptz, call_autofocus +from .patrol import return_to_preset + +from .streaming import MicroBatcher +from .model import yolo_forward + +# Старый код иногда ждал detection.batcher — даём псевдоним на state.batcher +batcher = _batcher + +__all__ = [ + 'INFER_IMGSZ', 'MODEL_CONF', 'TARGET_CLASSES', + 'W_EMA_ALPHA', 'DEFAULT_CLIENT_FPS', 'DRAW_ALL_ON_PANO', + 'DETECT_CONF','ALARM_CONF','RECOVER_ZOOM_OUT', + 'CENTER_GATE_IN','AIM_SMOOTH','AIM_P','AIM_I','AIM_D', + 'AUTO_STRATEGY','SHORT_LOSS_SEC','LONG_LOSS_SEC','ABS_MIN_FALLBACK','ABS_MAX_FALLBACK', + 'YOLO_WEIGHTS', + 'clipf', 'ptz_states','detect_stats_global','detect_stats_cam','video_clients', + 'publish_preview','maybe_downscale','notify_detected','postprocess_control','queue_alarm', + 'move_ptz_bounded','stop_ptz','call_autofocus','return_to_preset', + 'MicroBatcher','yolo_forward','batcher' +] diff --git a/ptz_tracker_modular/events.py b/ptz_tracker_modular/events.py new file mode 100644 index 0000000..ac179de --- /dev/null +++ b/ptz_tracker_modular/events.py @@ -0,0 +1,112 @@ +from __future__ import annotations + +import asyncio +import json +import logging +import websockets +from typing import Dict, Any + +from . import config, state + +logger = logging.getLogger("PTZTracker") + + +def _dispatch_alarm(payload: Dict[str, Any]) -> None: + """ + Кладём событие тревоги в очередь event-loop из любого потока. + """ + if state.alarm_queue is not None and state.MAIN_LOOP is not None: + try: + state.MAIN_LOOP.call_soon_threadsafe(state.alarm_queue.put_nowait, payload) + except Exception: + # не роняем приложение из-за уведомлений + pass + + +async def alarm_sender() -> None: + """ + Фоновая задача: отправляет тревоги по WebSocket. + Требование: отправлять *только* минимальный JSON: + {"type":"freq_alarm","data":true|false} + """ + assert state.alarm_queue is not None + backoff = 1.0 + + while True: + try: + logger.info("[ALARM] Connecting to %s ...", config.ALARM_WS_URL) + async with websockets.connect( + config.ALARM_WS_URL, + ping_interval=20, + close_timeout=1.0, + compression=None, + ) as ws: + logger.info("[ALARM] Connected") + backoff = 1.0 + + while True: + payload = await state.alarm_queue.get() + try: + # Строго минимальный словарь только с нужными ключами + if isinstance(payload, dict): + minimal = { + "type": payload.get("type"), + "data": bool(payload.get("data")), + } + else: + # на случай если кто-то положил уже готовую строку/другую структуру + minimal = payload + + # Без лишних пробелов/форматирования + await ws.send(json.dumps(minimal, separators=(",", ":"))) + except Exception as exc: + logger.error("[ALARM] Send failed: %s", exc) + try: + # возвращаем событие в очередь, чтобы не потерять + state.alarm_queue.put_nowait(payload) + except Exception: + pass + break + finally: + state.alarm_queue.task_done() + + except Exception as exc: + logger.error("[ALARM] Connection error: %s", exc) + + await asyncio.sleep(backoff) + backoff = min(backoff * 2, 10.0) + + +def queue_alarm(cam_idx: int, state_on: bool = True) -> None: + """ + Поставить тревогу в очередь для WS-отправки. + + ВАЖНО: WS-полезная нагрузка должна быть строго минимальной, + без дополнительных полей вроде IdCamera/mac. + """ + _dispatch_alarm({"type": "freq_alarm", "data": bool(state_on)}) + + +def queue_alarm_clear(cam_idx: int) -> None: + """ + Снять тревогу (data=false). + """ + queue_alarm(cam_idx, False) + + +def _send_record_event(cam_idx: int, flag: bool) -> None: + """ + Вспомогательное событие детекции (если используется где-то ещё в коде). + Оставлено без изменений: это не WS-Alarm, а внутренняя очередь. + """ + if state.detection_queue is not None and state.MAIN_LOOP is not None: + try: + state.MAIN_LOOP.call_soon_threadsafe( + state.detection_queue.put_nowait, + { + "type": "detection", + "data": {"detection": bool(flag), "IdCamera": int(cam_idx)}, + }, + ) + except Exception: + pass diff --git a/ptz_tracker_modular/geo-autocal.py b/ptz_tracker_modular/geo-autocal.py new file mode 100644 index 0000000..b53ab45 --- /dev/null +++ b/ptz_tracker_modular/geo-autocal.py @@ -0,0 +1,249 @@ +from __future__ import annotations +import time +import logging +from collections import deque +from typing import Deque, Tuple, Optional, Dict, List + +import numpy as np +from pathlib import Path +import os + +from . import config, state +from .utils import wrap_deg + +logger = logging.getLogger("PTZTracker.GEO") + +# Скользящее окно сэмплов: (cxn, pan_deg, t) +_samples: Dict[int, Deque[Tuple[float, float, float]]] = {} +# Последняя корректная оценка (north, hfov, rms) +_estimate: Dict[int, Tuple[float, float, float]] = {} + +# Параметры автокалибровки +MIN_SAMPLES = 30 # минимум наблюдений +MAX_SAMPLES = 400 # размер окна +MIN_SPREAD = 0.20 # cxn должен «расходиться» как минимум на 0.20 +RMS_OK = 4.0 # приемлемое СКО по азимуту (в градусах) +REESTIMATE_EVERY = 10.0 # не переобучать чаще, чем раз в N секунд на камеру + +# Условия и дебаунс автосохранения +SAVE_DEBOUNCE_SEC = 60.0 # писать файл не чаще раз в минуту на камеру +MIN_CHANGE_NORTH_DEG = 1.0 # насколько должен отличаться north, чтобы писать +MIN_CHANGE_HFOV_DEG = 1.0 # насколько должен отличаться hfov, чтобы писать + +_last_fit_at: Dict[int, float] = {} +_last_saved_at: Dict[int, float] = {} +_last_saved_vals: Dict[int, Tuple[float, float]] = {} # pano_id -> (north, hfov) + + +def _unwrap_deg_seq(degs: np.ndarray) -> np.ndarray: + """Разворачивает углы из [-180,180) в непрерывную последовательность для ЛСК.""" + r = np.deg2rad(degs) + r_unw = np.unwrap(r) + return np.rad2deg(r_unw) + + +def _fit_linear(cxn: np.ndarray, pan_deg: np.ndarray) -> Tuple[float, float, float]: + """ + ЛСК: pan_deg ~ north + hfov*(cxn-0.5) + Возвращает (north_deg_wrapped[-180,180), hfov_deg[>0], rms_err_deg). + """ + x = cxn - 0.5 + y = _unwrap_deg_seq(pan_deg) + + A = np.column_stack([np.ones_like(x), x]) + try: + coef, *_ = np.linalg.lstsq(A, y, rcond=None) + north_raw, hfov = float(coef[0]), float(coef[1]) + + resid = y - (north_raw + hfov * x) + rms = float(np.sqrt(np.mean(resid**2))) if resid.size else 999.0 + + north_wrapped = wrap_deg(north_raw) + # Если наклон отрицательный — перевернём модель (эквивалент сдвигу на 180°) + if hfov < 0: + hfov = abs(hfov) + north_wrapped = wrap_deg(north_wrapped + 180.0) + + return north_wrapped, hfov, rms + except Exception as e: + logger.debug("[GEO] fit failed: %s", e) + return 0.0, 90.0, 999.0 + + +def _cameras_toml_path() -> Path: + """ + Совместимо с логикой config._load_cameras_from_file(): + - CAMERAS_FILE из окружения + - cameras.toml рядом с модулем config + """ + env = os.getenv("CAMERAS_FILE") + if env: + return Path(env).resolve() + cfg_path = Path(config.__file__).resolve().parent + return (cfg_path / "cameras.toml").resolve() + + +def _rewrite_all_cameras_from_runtime(path: Path) -> None: + """ + Перезаписывает cameras.toml на основе текущего in-memory config.CAMERA_CONFIG. + Пишем только основные поля (как в streaming._rewrite_all_cameras). + """ + lines: List[str] = [] + for cid in sorted(config.CAMERA_CONFIG.keys()): + c = config.CAMERA_CONFIG[cid] + lines.append("[[camera]]") + lines.append(f"id = {int(cid)}") + lines.append(f"ip = \"{c['ip']}\"") + lines.append(f"username = \"{c.get('username', 'admin')}\"") + lines.append(f"password = \"{c.get('password', '')}\"") + lines.append(f"ptz_channel = {int(c.get('ptz_channel', 1))}") + lines.append("wiper_channel = 1") + + # Общие geo-поля, если есть + if isinstance(c.get("north_offset_deg"), (int, float)): + lines.append(f"north_offset_deg = {float(c['north_offset_deg']):.3f}") + + # PTZ или «пуля/панорама» + is_ptz = bool(c.get("preset1") and c.get("preset2")) + if is_ptz: + lines.append(f"preset1 = \"{c.get('preset1', '1')}\"") + lines.append(f"preset2 = \"{c.get('preset2', '2')}\"") + sweep_sign = int(c.get("sweep_sign", c.get("pan_sign", 1))) + lines.append(f"sweep_sign = {sweep_sign}") + # Доп. гео-поля PTZ — если у вас они используются, можно оставить + for k in ("preset1_deg", "preset2_deg", + "preset1_tilt_deg", "preset2_tilt_deg", + "sector_min_deg", "sector_max_deg", + "preset1_deg_geo", "preset2_deg_geo", + "sector_min_deg_geo", "sector_max_deg_geo"): + v = c.get(k) + if isinstance(v, (int, float)): + lines.append(f"{k} = {float(v):.3f}") + else: + # Панорамы/пули — FOV + if isinstance(c.get("bullet_hfov_deg"), (int, float)): + lines.append(f"bullet_hfov_deg = {float(c['bullet_hfov_deg']):.3f}") + if isinstance(c.get("bullet_vfov_deg"), (int, float)): + lines.append(f"bullet_vfov_deg = {float(c['bullet_vfov_deg']):.3f}") + + lines.append("") # пустая строка после блока + + text = ("\n".join(lines).strip() + "\n") if lines else "" + tmp = path.with_suffix(path.suffix + ".tmp") + path.parent.mkdir(parents=True, exist_ok=True) + with open(tmp, "w", encoding="utf-8") as f: + f.write(text) + f.flush() + os.fsync(f.fileno()) + os.replace(tmp, path) + + +def _maybe_persist_to_file(pano_id: int, north: float, hfov: float) -> None: + """ + Условно пишет cameras.toml, если изменилась оценка и сработал дебаунс. + После записи вызывает config.reload_cameras() (best effort). + """ + now = time.time() + + # Дебаунс по времени + last_t = _last_saved_at.get(pano_id, 0.0) + if (now - last_t) < SAVE_DEBOUNCE_SEC: + return + + # Проверка «существенности» изменений + prev = _last_saved_vals.get(pano_id) + if prev is not None: + p_north, p_hfov = prev + if (abs(north - p_north) < MIN_CHANGE_NORTH_DEG) and (abs(hfov - p_hfov) < MIN_CHANGE_HFOV_DEG): + return + + # Готовим обновление в in-memory конфиге + cc = config.CAMERA_CONFIG.get(pano_id, {}) + cc["north_offset_deg"] = float(north) + cc["bullet_hfov_deg"] = float(hfov) + + # Пишем на диск + path = _cameras_toml_path() + try: + _rewrite_all_cameras_from_runtime(path) + _last_saved_at[pano_id] = now + _last_saved_vals[pano_id] = (float(north), float(hfov)) + logger.info("[GEO] saved to %s: pano=%s north=%.1f hfov=%.1f", + path, pano_id, north, hfov) + except Exception as e: + logger.warning("[GEO] save cameras.toml failed: %s", e) + return + + # Мягкий hot-reload, чтобы рантайм пересчитал производные структуры + try: + config.reload_cameras() + logger.info("[GEO] config reloaded after save") + except Exception as e: + logger.warning("[GEO] reload_cameras() failed: %s", e) + + +def feed_pano_sample(pano_id: int, cxn: float, now: float | None = None) -> None: + """ + Кормим наблюдение из постпроцесса панорамы. + Нужно, чтобы была «парная» PTZ (config.PAN_TO_PTZ) и у неё была телеметрия pan_deg. + """ + # есть ли связанная PTZ? + ptz_id = config.PAN_TO_PTZ.get(pano_id) + if ptz_id is None: + return + + # актуальный азимут PTZ + st = state.ptz_states.get(ptz_id, {}) + pan_deg = st.get("pan_deg") + if pan_deg is None: + return + + t = time.time() if now is None else float(now) + + dq = _samples.setdefault(pano_id, deque(maxlen=MAX_SAMPLES)) + dq.append((float(cxn), float(pan_deg), t)) + + # Периодичность переобучения + last = _last_fit_at.get(pano_id, 0.0) + if (t - last) < REESTIMATE_EVERY: + return + + if len(dq) < MIN_SAMPLES: + return + + cx = np.array([s[0] for s in dq], dtype=np.float64) + pd = np.array([s[1] for s in dq], dtype=np.float64) + + # нужен ощутимый охват по кадру + if (cx.max() - cx.min()) < MIN_SPREAD: + return + + north, hfov, rms = _fit_linear(cx, pd) + _last_fit_at[pano_id] = t + + # проверка качества + if rms <= RMS_OK and 30.0 <= hfov <= 160.0: + _estimate[pano_id] = (north, hfov, rms) + + # применяем сразу в рантайм-конфиг (для текущей сессии) + cc = config.CAMERA_CONFIG.get(pano_id, {}) + cc["north_offset_deg"] = float(north) + cc["bullet_hfov_deg"] = float(hfov) + + logger.info( + "[GEO] pano %s AUTOCAL OK: north=%.1f° hfov=%.1f° rms=%.1f° (samples=%d)", + pano_id, north, hfov, rms, len(dq) + ) + + # Сохранить на диск (с дебаунсом и проверкой изменений) + _maybe_persist_to_file(pano_id, north, hfov) + else: + logger.info( + "[GEO] pano %s autocal tentative: north=%.1f°, hfov=%.1f°, rms=%.1f° (need more/better samples)", + pano_id, north, hfov, rms + ) + + +def get_estimate(pano_id: int) -> Optional[Tuple[float, float, float]]: + """Вернуть последнюю валидную оценку: (north_deg, hfov_deg, rms) или None.""" + return _estimate.get(pano_id) diff --git a/ptz_tracker_modular/geom.py b/ptz_tracker_modular/geom.py new file mode 100644 index 0000000..f10a6d1 --- /dev/null +++ b/ptz_tracker_modular/geom.py @@ -0,0 +1,138 @@ +from __future__ import annotations +from typing import Tuple, Optional +from . import config + +# ---------------- Базовая геометрия углов ---------------- + +def normalize360(a: float) -> float: + """Нормирует угол в диапазон [0, 360).""" + a = a % 360.0 + return a if a >= 0.0 else a + 360.0 + +def ang_diff_signed(a: float, b: float) -> float: + """ + Подписанная кратчайшая разница (a - b) в диапазоне [-180, +180). + Знак >0 означает поворот по ЧС от b к a. + """ + return (a - b + 540.0) % 360.0 - 180.0 + +def _wrap_deg_pm180(a: float) -> float: + """Нормировка в диапазон [-180, +180).""" + return (a + 180.0) % 360.0 - 180.0 + +# ---------------- Перевод внутренних углов PTZ в гео ---------------- + +def pan_to_geo(cam_id: int, pan_deg: float | None) -> float | None: + """ + Переводит «внутренний» pan PTZ (0..360, как отдаёт камера) в гео-азимут (0°=С, 90°=В). + Гео = normalize360(north_offset_deg + pan). + Если pan_deg=None или north неизвестен — возвращает None (или 0.0 по желанию). + """ + if pan_deg is None: + return None + north = config.CAMERA_CONFIG.get(cam_id, {}).get("north_offset_deg") + if not isinstance(north, (int, float)): + return None + return normalize360(float(north) + float(pan_deg)) + +# ---------------- Сектора (мягкие/жёсткие края) ---------------- + +def sector_contains(az: float, dmin: float, dmax: float) -> bool: + """ + Проверяет, лежит ли азимут az внутри сектора [dmin, dmax] с учётом жёсткого эпсилона. + Работает и для «завёрнутых» интервалов (например, [320°, 40°]). + """ + az = normalize360(az); dmin = normalize360(dmin); dmax = normalize360(dmax) + if dmin <= dmax: + return dmin - config.EDGE_EPS_HARD <= az <= dmax + config.EDGE_EPS_HARD + return az >= dmin - config.EDGE_EPS_HARD or az <= dmax + config.EDGE_EPS_HARD + +def sector_distances(az: float, dmin: float, dmax: float) -> Tuple[float, float, bool]: + """ + Возвращает (to_left, to_right, inside), где: + to_left = на сколько градусов до левой границы (>=0, если внутри) + to_right = на сколько градусов до правой границы (>=0, если внутри) + inside = флаг попадания в сектор (по sector_contains) + Расстояния считаются по кратчайшей дуге. + """ + az_n = normalize360(az) + dmin_n = normalize360(dmin) + dmax_n = normalize360(dmax) + + inside = sector_contains(az_n, dmin_n, dmax_n) + + # расстояние до границы — модуль подписанной разницы + to_left = abs(ang_diff_signed(dmin_n, az_n)) + to_right = abs(ang_diff_signed(dmax_n, az_n)) + + return to_left, to_right, inside + +def clamp_sector_speed(pan_i: int, az: Optional[float], dmin: Optional[float], dmax: Optional[float], + pan_sign: int = 1) -> int: + """ + Притормаживает/блокирует скорость панорамирования у краёв сектора. + Работает с целочисленной шкалой Hik (-100..+100). Совместимо с логикой в ptz_io. + + - Если az неизвестен, возврат pan_i без изменений (внешний код сам решает, можно ли двигаться). + - Если вне сектора — блокируем. + - Если внутри и «толкаем» в закрытую сторону — блокируем. + - Если внутри — плавно замедляем вблизи ближайшей границы. + """ + if az is None or dmin is None or dmax is None: + return pan_i + to_left, to_right, inside = sector_distances(az, dmin, dmax) + if not inside: + return 0 + + # направление «вправо/влево» с учётом знака головы + pushing_right = (pan_i * pan_sign) > 0 and ang_diff_signed(dmax, az) <= 0.0 + pushing_left = (pan_i * pan_sign) < 0 and ang_diff_signed(dmin, az) >= 0.0 + if pushing_right or pushing_left: + return 0 + + if pan_i == 0: + return 0 + + # плавное замедление у ближайшей границы + dist_min = min(to_left, to_right) + edge_slow = getattr(config, "TRACK_EDGE_SLOWDOWN_DEG", 6.0) + edge_min_k = getattr(config, "TRACK_EDGE_MIN_SCALE", 0.25) + if dist_min < edge_slow: + k = max(edge_min_k, dist_min / max(1e-6, edge_slow)) + pan_i = int(pan_i * k) + + return pan_i + +# ---------------- Прочие утилиты ---------------- + +def is_near(a: float, b: float, tol: float = 0.25) -> bool: + """Близость углов по модулю подписанной разницы.""" + return abs(ang_diff_signed(a, b)) <= tol + +def nearest_preset_token(cam_idx: int, az: float) -> str | None: + """ + Возвращает token ближайшего пресета (preset1|preset2) по текущему азимуту. + Если нет калиброванных preset*_deg — вернёт None. + """ + cfg = config.CAMERA_CONFIG[cam_idx] + p1, p2 = cfg.get("preset1"), cfg.get("preset2") + a1, a2 = cfg.get("preset1_deg"), cfg.get("preset2_deg") + if not (p1 and p2 and isinstance(a1, (int, float)) and isinstance(a2, (int, float))): + return None + d1 = abs(ang_diff_signed(az, a1)) + d2 = abs(ang_diff_signed(az, a2)) + return p1 if d1 <= d2 else p2 + +def project_x_to_bearing(base_az: float, hfov_deg: float, x_norm: float) -> float: + """ + Быстрый расчёт истинного азимута объекта на панораме: + base_az — гео-азимут центра кадра (0°=С, 90°=В), + hfov_deg — горизонтальный угол поля зрения, + x_norm — нормированная x-координата бокса [0..1]. + Возвращает bearing в диапазоне [-180, +180). + """ + # Важно: сначала нормируем базовый азимут, затем добавляем смещение, + # после чего сворачиваем в [-180, +180) для «относительного» румба. + az_center = normalize360(float(base_az)) + delta = (float(x_norm) - 0.5) * float(hfov_deg) + return _wrap_deg_pm180(az_center + delta) diff --git a/ptz_tracker_modular/heartbeat.py b/ptz_tracker_modular/heartbeat.py new file mode 100644 index 0000000..04d034d --- /dev/null +++ b/ptz_tracker_modular/heartbeat.py @@ -0,0 +1,18 @@ +from __future__ import annotations +import asyncio, socket, logging +from . import config +logger = logging.getLogger("PTZTracker") + +async def heartbeat_pinger() -> None: + addr = (config.HEARTBEAT_HOST, config.HEARTBEAT_PORT) + with socket.socket(socket.AF_INET, socket.SOCK_DGRAM) as s: + try: + s.setblocking(False) + except Exception: + pass + while True: + try: + s.sendto(b"hb", addr) + except Exception as e: + logger.debug("heartbeat send failed: %s", e) + await asyncio.sleep(config.HEARTBEAT_INTERVAL) diff --git a/ptz_tracker_modular/hik_isapi.py b/ptz_tracker_modular/hik_isapi.py new file mode 100644 index 0000000..8099040 --- /dev/null +++ b/ptz_tracker_modular/hik_isapi.py @@ -0,0 +1,105 @@ +from __future__ import annotations +import logging +import xml.etree.ElementTree as ET +from typing import Tuple, Optional + +import requests +from requests.auth import HTTPDigestAuth + +from . import config + +logger = logging.getLogger("PTZTracker") + +# Универсальные таймауты и заголовки +_REQ_TIMEOUT = (1.2, 2.5) # (connect, read) +_XML = {"Content-Type": "application/xml; charset=UTF-8"} + +def _auth(cam_id: int) -> Tuple[str, HTTPDigestAuth, int]: + cfg = config.CAMERA_CONFIG[cam_id] + ip = cfg["ip"] + user = cfg.get("username") or cfg.get("user") or "admin" + pw = cfg.get("password") or "" + ch = int(cfg.get("ptz_channel", 1)) + return ip, HTTPDigestAuth(user, pw), ch + +def _get_xml(url: str, auth: HTTPDigestAuth) -> Optional[ET.Element]: + try: + r = requests.get(url, auth=auth, timeout=_REQ_TIMEOUT) + r.raise_for_status() + return ET.fromstring(r.content) + except Exception as e: + logger.debug("[ISAPI] GET %s failed: %s", url, e) + return None + +def _put_xml(url: str, auth: HTTPDigestAuth, xml_body: str) -> bool: + try: + r = requests.put(url, data=xml_body.encode("utf-8"), headers=_XML, auth=auth, timeout=_REQ_TIMEOUT) + r.raise_for_status() + return 200 <= r.status_code < 300 + except Exception as e: + logger.debug("[ISAPI] PUT %s failed: %s", url, e) + return False + +# ---------- Public API ---------- + +def get_ptz_status(cam_id: int) -> Tuple[Optional[float], Optional[float]]: + """ + Возвращает (pan_deg, tilt_deg) для камеры через ISAPI. + Хиквишн обычно отдаёт абсолютные значения pan/tilt в градусах. + Иногда встречается масштаб *10 (0..3600) — нормализуем. + """ + ip, auth, ch = _auth(cam_id) + url = f"http://{ip}/ISAPI/PTZCtrl/channels/{ch}/status" + root = _get_xml(url, auth) + if root is None: + return None, None + + # Ищем разные варианты тегов + pan = None + tilt = None + for tag in ("pan", "absolutePan", "azimuth"): + el = root.find(f".//{tag}") + if el is not None and el.text: + try: + pan = float(el.text) + break + except Exception: + pass + for tag in ("tilt", "absoluteTilt", "elevation"): + el = root.find(f".//{tag}") + if el is not None and el.text: + try: + tilt = float(el.text) + break + except Exception: + pass + + # Нормализация шкалы (редкие модели отдают *10) + def _norm(x: Optional[float]) -> Optional[float]: + if x is None: + return None + if abs(x) > 360.0 and abs(x) <= 3600.0: + x = x / 10.0 + return float(x) + + return _norm(pan), _norm(tilt) + +def read_ptz_azimuth_deg(cam_id: int) -> Optional[float]: + pan, _ = get_ptz_status(cam_id) + return pan # pan — это и есть азимут головы в градусах + +def goto_preset_token(cam_id: int, preset_token: str | int) -> bool: + """ + Переход к пресету по ISAPI. На HIK это: + PUT /ISAPI/PTZCtrl/channels/{ch}/presets/{id}/goto + {id} + """ + ip, auth, ch = _auth(cam_id) + pid = str(preset_token).strip() + url = f"http://{ip}/ISAPI/PTZCtrl/channels/{ch}/presets/{pid}/goto" + + body = f"{pid}" + ok = _put_xml(url, auth, body) + if not ok: + logger.warning("[ISAPI] goto preset failed cam=%s ch=%s preset=%s", cam_id, ch, pid) + return ok diff --git a/ptz_tracker_modular/logging_utils.py b/ptz_tracker_modular/logging_utils.py new file mode 100644 index 0000000..535558a --- /dev/null +++ b/ptz_tracker_modular/logging_utils.py @@ -0,0 +1,19 @@ +import logging +import os + + +def setup_logging() -> logging.Logger: + """Configure application logging and return the PTZTracker logger.""" + os.makedirs("logs", exist_ok=True) + logging.basicConfig( + level=logging.INFO, + format="%(asctime)s - %(levelname)s - %(message)s", + handlers=[ + logging.FileHandler("logs/ptz_tracker.log", encoding="utf-8"), + logging.StreamHandler(), + ], + ) + return logging.getLogger("PTZTracker") + + +logger = setup_logging() diff --git a/ptz_tracker_modular/main.py b/ptz_tracker_modular/main.py new file mode 100644 index 0000000..a21196b --- /dev/null +++ b/ptz_tracker_modular/main.py @@ -0,0 +1,400 @@ +# -*- coding: utf-8 -*- +from __future__ import annotations + +# --- bootstrap for direct script run --- +# Позволяет запускать файл как обычный скрипт: python ptz_tracker_modular/main.py +# и при этом импортировать пакет ptz_tracker_modular +if __package__ is None or __package__ == "": + import sys, pathlib + pkg_root = pathlib.Path(__file__).resolve().parent.parent # папка-родитель, содержащая пакет + if str(pkg_root) not in sys.path: + sys.path.insert(0, str(pkg_root)) + __package__ = "ptz_tracker_modular" +# --------------------------------------- + +import asyncio +import logging +import os +import threading +from queue import Queue +from typing import Iterable + + +def _parse_level(name: str, default=logging.ERROR) -> int: + name = (name or "").strip().upper() + return getattr(logging, name, default) + + +def setup_logging() -> logging.Logger: + """ + Лог только в консоль, по умолчанию уровень ERROR. + Файлы и папка logs не создаются. + """ + # По умолчанию только ошибки; можно переопределить LOG_LEVEL=WARNING/INFO/DEBUG + level = _parse_level(os.getenv("LOG_LEVEL", "ERROR")) + + logging.basicConfig( + level=level, + format="%(asctime)s | %(levelname)-8s | %(name)s | %(message)s", + handlers=[logging.StreamHandler()], + ) + + logger = logging.getLogger("PTZTracker") + # приглушим лишний шум от сторонних библиотек + logging.getLogger("websockets.client").setLevel(logging.WARNING) + logging.getLogger("urllib3.connectionpool").setLevel(logging.WARNING) + + logger.error("Logging initialized, level=%s (console only, no file)", logging.getLevelName(level)) + return logger + + +logger = setup_logging() + +from ptz_tracker_modular import config, state +from ptz_tracker_modular.streaming import ( + MicroBatcher, + video_broadcaster, + cpp_ws_loop, + cpp_detection_loop, + build_servers, +) +from ptz_tracker_modular.status import ptz_status_poller +from ptz_tracker_modular.heartbeat import heartbeat_pinger +from ptz_tracker_modular.sector import sector_autocal_from_presets, sector_init_on_startup +from ptz_tracker_modular.calibration import calibrate_presets, autocal_pan_sign, park_to_patrol_start +from ptz_tracker_modular.patrol import ( + patrol_init_on_startup, + patrol_supervisor_tick_bounded, + patrol_supervisor_tick, + sync_patrol_coordinator_loop, + track_return_watchdog, # <-- импорт вотчдога +) +from ptz_tracker_modular import ptz_io # содержит _ptz_worker + +# (опционально) uvloop +try: + import uvloop # type: ignore + asyncio.set_event_loop_policy(uvloop.EventLoopPolicy()) +except Exception: + pass + + +def _dump_alarm_config(): + """Красиво логируем ключевые параметры тревог при старте (если уровень позволяет).""" + ws_url = getattr(config, "ALARM_WS_URL", "ws://127.0.0.1:3000/ws") + ws_fmt = getattr(config, "ALARM_WS_FORMAT", "swagger") + mac_only = bool(getattr(config, "ALARM_MAC_ONLY", False)) + + http_en = bool(getattr(config, "ALARM_HTTP_ENABLE", False)) + http_mirror = bool(getattr(config, "ALARM_HTTP_MIRROR", False)) + http_base = getattr(config, "ALARM_HTTP_BASE", "http://127.0.0.1:3000") + http_path = getattr(config, "ALARM_HTTP_PATH", "/videodata") + http_tmpl = getattr(config, "ALARM_HTTP_PATH_TMPL", "/videodata") + + ws_bmax = getattr(config, "ALARM_WS_BATCH_MAX", getattr(config, "WS_BATCH_MAX", 16)) + ws_bms = getattr(config, "ALARM_WS_BATCH_MS", getattr(config, "WS_BATCH_MS", 120)) + http_bmax = getattr(config, "ALARM_HTTP_BATCH_MAX", getattr(config, "HTTP_BATCH_MAX", 16)) + http_bms = getattr(config, "ALARM_HTTP_BATCH_MS", getattr(config, "HTTP_BATCH_MS", 120)) + + hb_sec = int(getattr(config, "ALARM_HEARTBEAT_SEC", 60)) + logger.info("[ALARM] WS_URL=%s format=%s mac_only=%s", ws_url, ws_fmt, mac_only) + logger.info("[ALARM] HTTP enable=%s mirror=%s base=%s path=%s tmpl=%s", + http_en, http_mirror, http_base, http_path, http_tmpl) + logger.info("[ALARM] Batching: WS max=%s wait_ms=%s | HTTP max=%s wait_ms=%s", + ws_bmax, ws_bms, http_bmax, http_bms) + logger.info("[ALARM] Heartbeat: every %ss to %s", hb_sec, http_path) + + +def watch(coro: asyncio.Future | asyncio.Task | asyncio.coroutines, name: str) -> asyncio.Task: + """ + Обёртка для задач: если задача упала, логируем исключение. + Возвращает Task (подходит для gather). + """ + async def _guard(): + try: + return await coro # type: ignore[arg-type] + except asyncio.CancelledError: + logger.info("[%s] cancelled", name) + raise + except Exception as e: + logger.exception("[%s] crashed: %s", name, e) + raise + + if not isinstance(coro, (asyncio.Future, asyncio.Task)): + coro = asyncio.create_task(coro) # type: ignore[assignment] + task = asyncio.create_task(_guard(), name=name) # type: ignore[arg-type] + return task + + +async def main() -> None: + # --- Инициализация общего состояния --- + state.MAIN_LOOP = asyncio.get_running_loop() + state.alarm_queue = asyncio.Queue() + state.detection_queue = asyncio.Queue() + state.batcher = MicroBatcher( + max_batch=config.BATCH_MIN, + max_wait_ms=2.0, + adaptive=config.ADAPTIVE_BATCH_ENABLE, + bmin=config.BATCH_MIN, + bmax=config.BATCH_MAX, + target_ms=config.BATCH_TARGET_MS, + ) + + # Импорт alarms ПОСЛЕ инициализации state.*, чтобы внутри не было None + from ptz_tracker_modular import alarms # -> alarms.alarm_sender(), alarms.alarm_http_sender() + + _dump_alarm_config() + + # PTZ worker thread + очередь команд + state.ptz_cmd_q = Queue(maxsize=2048) + state._ptz_thread = threading.Thread( + target=ptz_io._ptz_worker, name="PTZWorker", daemon=True + ) + state._ptz_thread.start() + logger.info("[PTZ] worker thread started") + + # --- ВАЖНО: гарантируем базовые поля стейта для PTZ, иначе patrol может skip'ать камеры --- + # (частая причина "патруль не на всех PTZ": st.get("mode") == None) + for cam_id in config.PTZ_CAM_IDS: + st = state.ensure_cam_state(cam_id, is_ptz=True) + if st.get("mode") is None: + st["mode"] = "IDLE" + + # ---- КАЛИБРОВКИ ---- + for cam_id in config.PTZ_CAM_IDS: + cfg = config.CAMERA_CONFIG[cam_id] + need = ( + cfg.get("preset1_deg") is None + or cfg.get("preset2_deg") is None + or cfg.get("sector_min_deg") is None + or cfg.get("sector_max_deg") is None + ) + if need: + ok = await calibrate_presets(cam_id) + if not ok: + logger.warning( + "Failed to calibrate camera %s, using fallback for sector clamp.", cam_id + ) + + if config.USE_PRESET_EDGES_FOR_SECTOR: + need_sector = any(state.ptz_states[c].get("sector_left_deg") is None for c in config.PTZ_CAM_IDS) + if need_sector: + await sector_autocal_from_presets() + sector_init_on_startup() + + for cam in config.PTZ_CAM_IDS: + try: + await autocal_pan_sign(cam) + except Exception as e: + logger.debug("autocal pan_sign cam %s: %s", cam, e) + + await park_to_patrol_start() + + # ---- Патруль ---- + have_any_ptz = bool(config.PTZ_CAM_IDS) + if have_any_ptz and config.PATROL_ENABLE_ON_START: + if config.SYNC_PATROL_ENABLE and config.SYNC_COHORTS: + logger.info( + "SYNC PTZ patrol enabled (cohorts): %s", + ", ".join(config.SYNC_COHORTS.keys()), + ) + else: + patrol_init_on_startup() + logger.info("PTZ patrol started (standalone)") + elif not have_any_ptz: + logger.info("PTZ patrol skipped: no PTZ cameras configured") + else: + logger.info("PTZ patrol is DISABLED on start") + + logger.info("PTZ tracking + HARD sector clamp + bounded patrol (resync enabled)") + + # ---- WS servers ---- + ctrl_server, video_server = await build_servers() + logger.info("[CTRL WS] listening on 0.0.0.0:%s", config.VUE_CONTROL_WS_PORT) + logger.info("[VIDEO WS] listening on 0.0.0.0:%s", config.VUE_VIDEO_WS_PORT) + + async def patrol_supervisor_loop(): + while True: + try: + patrol_supervisor_tick_bounded() + patrol_supervisor_tick() + except Exception as e: + logger.error("patrol_supervisor error: %s", e) + await asyncio.sleep(0.10) + + async def track_watchdog_loop() -> None: + """ + Supervisor для per-camera watchdog'ов. + + ВАЖНО: track_return_watchdog(cam_id) внутри себя бесконечный цикл. + Поэтому здесь мы НЕ await'им его по очереди, а запускаем отдельной Task + на каждую PTZ-камеру и следим, чтобы: + - вотчдог был запущен на всех cam_id из config.PTZ_CAM_IDS, + - если вотчдог упал — логируем причину и перезапускаем, + - если список PTZ камер изменился (reload) — отменяем лишние задачи. + """ + tasks_by_cam: dict[int, asyncio.Task] = {} + restart_after: dict[int, float] = {} # cam_id -> loop.time(), когда можно рестартить + loop = asyncio.get_running_loop() + + tick_sec = 1.0 + restart_cooldown_sec = 2.0 + + try: + while True: + now = loop.time() + + # Снимок актуального списка PTZ камер (может меняться после reload) + ptz_ids = list(getattr(config, "PTZ_CAM_IDS", []) or []) + ptz_set = set(ptz_ids) + + # 1) Запускаем/перезапускаем вотчдоги для актуальных камер + for cam_id in ptz_ids: + # гарантируем, что стейт камеры существует (чтобы внутри watchdog не было KeyError) + try: + state.ensure_cam_state(cam_id, is_ptz=True) + except Exception as e: + logger.error("[WATCHDOG] ensure_cam_state cam=%s failed: %s", cam_id, e) + continue + + t = tasks_by_cam.get(cam_id) + + if t is None: + # нет задачи — создаём, если прошёл cooldown + if now >= restart_after.get(cam_id, 0.0): + tasks_by_cam[cam_id] = asyncio.create_task( + track_return_watchdog(cam_id), + name=f"track_return_watchdog[{cam_id}]", + ) + continue + + if t.done(): + # задача завершилась — логируем (если было исключение) и планируем рестарт + try: + exc = t.exception() + except asyncio.CancelledError: + exc = None + except Exception as e: + exc = e + + if exc is not None: + logger.exception( + "[WATCHDOG] cam=%s track_return_watchdog crashed: %s", + cam_id, + exc, + ) + + tasks_by_cam.pop(cam_id, None) + restart_after[cam_id] = now + restart_cooldown_sec + + # 2) Камеры, которые больше не PTZ — отменяем их watchdog'и + for cam_id in list(tasks_by_cam.keys()): + if cam_id not in ptz_set: + t = tasks_by_cam.pop(cam_id) + t.cancel() + restart_after.pop(cam_id, None) + # Можно подсказать, что камера теперь не PTZ (не обязательно) + try: + state.ensure_cam_state(cam_id, is_ptz=False) + except Exception: + pass + + await asyncio.sleep(tick_sec) + + except asyncio.CancelledError: + raise + + finally: + # Корректно гасим все per-camera задачи + for t in tasks_by_cam.values(): + t.cancel() + if tasks_by_cam: + await asyncio.gather(*tasks_by_cam.values(), return_exceptions=True) + + # ---- Опциональный smoke-тест тревоги (по первому MAC) ---- + if os.getenv("ALARM_SMOKE", "0") in ("1", "true", "TRUE", "yes", "YES"): + async def _smoke(): + try: + # Берём первый доступный MAC из конфигурации + macs: Iterable[str] = ( + cfg.get("mac") for _id, cfg in sorted(config.CAMERA_CONFIG.items()) + if isinstance(cfg.get("mac"), str) + ) + mac = next((m for m in macs if m), None) + if not mac: + logger.warning("[SMOKE] no MACs in CAMERA_CONFIG — skip") + return + logger.info("[SMOKE] sending test alarms for MAC=%s", mac) + alarms.queue_alarm_mac(mac, True) + await asyncio.sleep(0.2) + alarms.queue_alarm_mac(mac, False) + logger.info("[SMOKE] done") + except Exception as e: + logger.error("[SMOKE] failed: %s", e) + asyncio.create_task(_smoke()) + + # ---- Таски ---- + tasks: list[asyncio.Task | asyncio.Future] = [ + watch(video_broadcaster(), "video_broadcaster"), + watch(cpp_ws_loop(), "cpp_ws_loop"), + watch(cpp_detection_loop(), "cpp_detection_loop"), + watch(alarms.alarm_sender(), "alarm_sender"), # WS-alarms + watch(alarms.alarm_http_sender(), "alarm_http_sender"), # HTTP-alarms (если включено) + watch(alarms.periodic_status_sender(), "periodic_status_sender"), # heartbeat-массив камер + watch(ptz_status_poller(), "ptz_status_poller"), + watch(heartbeat_pinger(), "heartbeat_pinger"), + asyncio.create_task(ctrl_server.wait_closed(), name="ctrl_ws_wait_closed"), + asyncio.create_task(video_server.wait_closed(), name="video_ws_wait_closed"), + ] + + # Запускаем патрульный цикл, если он включён + if config.PATROL_ENABLE_ON_START and config.PTZ_CAM_IDS: + tasks.insert(8, watch(patrol_supervisor_loop(), "patrol_supervisor_loop")) + + # Вотчдог по трекингу/IDLE — запускаем всегда, если есть PTZ-камеры + if config.PTZ_CAM_IDS: + tasks.insert(8, watch(track_watchdog_loop(), "track_watchdog_loop")) + + # SYNC patrol при необходимости + if config.SYNC_PATROL_ENABLE and config.SYNC_COHORTS: + tasks.insert(8, watch(sync_patrol_coordinator_loop(), "sync_patrol_coordinator_loop")) + + try: + await asyncio.gather(*tasks) + finally: + ctrl_server.close() + video_server.close() + await ctrl_server.wait_closed() + await video_server.wait_closed() + +def run(): + try: + asyncio.run(main()) + except KeyboardInterrupt: + logger.info("Stopped by user") + finally: + try: + if state.ptz_cmd_q is not None: + state.ptz_cmd_q.put_nowait(None) # sentinel для PTZ-воркера + if state._ptz_thread is not None: + state._ptz_thread.join(timeout=0.5) + except Exception: + pass + + for _cid, _st in state.ptz_states.items(): + if _st.get("zoom_reset_timer"): + try: + _st["zoom_reset_timer"].cancel() + except Exception: + pass + if _st.get("preset_timer"): + try: + _st["preset_timer"].cancel() + except Exception: + pass + + logger.info("Shutdown complete") + + +if __name__ == "__main__": + run() diff --git a/ptz_tracker_modular/media.py b/ptz_tracker_modular/media.py new file mode 100644 index 0000000..12095eb --- /dev/null +++ b/ptz_tracker_modular/media.py @@ -0,0 +1,70 @@ +from __future__ import annotations +from typing import Tuple, Optional +import cv2 +import numpy as np + +# --- пакетные импорты с fallback при запуске скриптом --- +try: + from . import config as _cfg + from . import state as _state + from .preview import ( + encode_jpeg_bgr as _encode_jpeg_bgr, + maybe_downscale as _maybe_downscale, + publish_preview as _publish_preview, + ) +except Exception: # запуск вне пакета (нежелательно, но поддержим) + import config as _cfg # type: ignore + import state as _state # type: ignore + from preview import ( # type: ignore + encode_jpeg_bgr as _encode_jpeg_bgr, + maybe_downscale as _maybe_downscale, + publish_preview as _publish_preview, + ) + +# Оставляем старые имена функций для обратной совместимости, +# под капотом вызываем реализацию из preview.py. + +def encode_jpeg_bgr(img_bgr: np.ndarray) -> Optional[bytes]: + """Совместимость: прокси к preview.encode_jpeg_bgr.""" + return _encode_jpeg_bgr(img_bgr) + + +def maybe_downscale(img: np.ndarray, target_w: int = None) -> Tuple[np.ndarray, float, float]: + """Совместимость: прокси к preview.maybe_downscale.""" + if target_w is None: + target_w = int(getattr(_cfg, "PREVIEW_TARGET_W", 896)) + return _maybe_downscale(img, target_w) + + +def publish_preview(cam_id: int, img: np.ndarray) -> None: + """ + Совместимость: прокси к preview.publish_preview. + Учитывает флаг PUBLISH_ONLY_IF_CLIENTS внутри preview. + """ + _publish_preview(cam_id, img) + + +def publish_annotated_scaled( + cam_id: int, + frame_bgr: np.ndarray, + x1: int, y1: int, x2: int, y2: int, + label: str +) -> None: + """ + Быстрый аннотатор: даунскейлит кадр под превью, рисует коробку+лейбл и публикует JPEG. + """ + # ранний выход, если никому слать + if getattr(_cfg, "PUBLISH_ONLY_IF_CLIENTS", True) and not _state.video_clients: + return + + down, sx, sy = maybe_downscale(frame_bgr) + if sx != 1.0 or sy != 1.0: + x1 = int(x1 * sx); y1 = int(y1 * sy) + x2 = int(x2 * sx); y2 = int(y2 * sy) + + cv2.rectangle(down, (x1, y1), (x2, y2), (0, 255, 0), 2, lineType=cv2.LINE_4) + cv2.putText( + down, label, (x1, max(0, y1 - 6)), + cv2.FONT_HERSHEY_SIMPLEX, 0.6, (0, 255, 0), 1, cv2.LINE_4 + ) + publish_preview(cam_id, down) diff --git a/ptz_tracker_modular/model.py b/ptz_tracker_modular/model.py new file mode 100644 index 0000000..7723778 --- /dev/null +++ b/ptz_tracker_modular/model.py @@ -0,0 +1,59 @@ +from __future__ import annotations +import logging, os, numpy as np, torch +from ultralytics import YOLO +from typing import List +from . import config +logger = logging.getLogger("PTZTracker") + +try: + from torch.amp import autocast # type: ignore +except Exception: + autocast = None # type: ignore + +device = "cuda" if torch.cuda.is_available() else "cpu" +WEIGHT_PATH = os.getenv("YOLO_WEIGHTS", "/home/electro-pribory/Desktop/weight/electro-pribory.pt") + +model_shared = YOLO(WEIGHT_PATH).to(device) +try: + model_shared.fuse() +except Exception: + pass + +try: + model_shared.model.to(memory_format=torch.channels_last) # type: ignore +except Exception: + pass + +if device == "cuda": + try: + model_shared.model.half() # type: ignore + logger.info("Model set to FP16") + except Exception: + logger.info("FP16 not supported, using FP32") + try: + if hasattr(torch, "compile"): + model_shared.model = torch.compile(model_shared.model, mode="reduce-overhead") # type: ignore + logger.info("torch.compile enabled") + except Exception as e: + logger.info("torch.compile not used: %s", e) + +model_shared.model.eval() +logger.info("Single shared model loaded") + +try: + _warm = np.zeros((config.INFER_IMGSZ, config.INFER_IMGSZ, 3), dtype=np.uint8) + _ = model_shared([_warm], imgsz=config.INFER_IMGSZ, conf=0.1, iou=0.45, verbose=False) + if device == "cuda": + torch.cuda.synchronize() + del _warm +except Exception: + pass + +INFER_ARGS = dict(imgsz=config.INFER_IMGSZ, conf=config.MODEL_CONF, iou=0.45, verbose=False) + +@torch.inference_mode() +def yolo_forward(frames: List[np.ndarray]): + if device == "cuda" and autocast is not None: + with autocast("cuda", dtype=torch.float16): # type: ignore[misc] + return model_shared(frames, **INFER_ARGS) + return model_shared(frames, **INFER_ARGS) diff --git a/ptz_tracker_modular/notify.py b/ptz_tracker_modular/notify.py new file mode 100644 index 0000000..f72097c --- /dev/null +++ b/ptz_tracker_modular/notify.py @@ -0,0 +1,159 @@ +from __future__ import annotations + +import time +from typing import Optional + +from . import state, config +from .events import _send_record_event +from .ptz_io import stop_ptz # <-- важно для "abort goto" + + +def _ema_update(prev: Optional[float], value: float, alpha: float = 0.2) -> float: + return value if prev is None else (1.0 - alpha) * prev + alpha * value + + +def _stat_on_detection(cam_idx: int, ts: float) -> None: + g = state.detect_stats_global + if g["last_ts"] is not None: + dt = ts - g["last_ts"] + if dt > 0.05: + g["ema_interval"] = _ema_update(g["ema_interval"], dt) + g["times"].append(ts) + g["last_ts"] = ts + + c = state.detect_stats_cam[cam_idx] + if c["last_ts"] is not None: + dtc = ts - c["last_ts"] + if dtc > 0.05: + c["ema_interval"] = _ema_update(c["ema_interval"], dtc) + c["times"].append(ts) + c["last_ts"] = ts + + +def notify_detected(cam_idx: int, detected: bool, force: bool = False) -> None: + """ + detected=True/False — «сырое» состояние текущего кадра. + Формирует START/STOP записи (с гистерезисом) ТОЛЬКО для PTZ (или force). + Также ведёт режимы TRACK/SEARCH с hold на потерю, чтобы не срывать трекинг + при прерывистой детекции. + """ + + # Панорамы не создают событий записи, если не включено явно + if cam_idx not in config.PTZ_CAM_IDS and not config.ALLOW_PANORAMA_RECORD and not force: + return + + st = state.ptz_states[cam_idx] + now = time.time() + + # -------------------- Параметры поведения трекинга -------------------- + TRACK_LOSS_HOLD_SEC = float(getattr(config, "TRACK_LOSS_HOLD_SEC", 2.0)) + TRACK_HOLD_WHILE_REC_ACTIVE_SEC = float( + getattr(config, "TRACK_HOLD_WHILE_REC_ACTIVE_SEC", TRACK_LOSS_HOLD_SEC) + ) + + # -------------------- Внутренняя логика трекинга -------------------- + last_raw = st.get("last_detected_state") + + if detected: + # статистика по фронту False->True + if last_raw is not True: + _stat_on_detection(cam_idx, now) + + # фиксируем "видели цель" + st["last_seen"] = now + + # Если в этот момент камера ехала на пресет (goto) — прерываем, + # иначе goto/патруль будет мешать трекингу первые секунды + if now < float(st.get("goto_in_progress_until", 0.0) or 0.0): + try: + stop_ptz(cam_idx) + except Exception: + pass + st["goto_in_progress_until"] = 0.0 + + # сброс кандидата на потерю + st.pop("track_loss_since", None) + + # ВАЖНО: сообщаем патрулю, что идёт трекинг + st["tracking_active"] = True + + # трекинг включается сразу + st["fallback_done"] = False + st["mode"] = "TRACK" + + # опционально: если используешь где-то пост-трекинг флаги + st.pop("post_track_since", None) + st["post_track_park_done"] = False + + else: + # detected == False + if st.get("mode") == "TRACK": + if "track_loss_since" not in st: + st["track_loss_since"] = now + else: + hold = TRACK_HOLD_WHILE_REC_ACTIVE_SEC if bool(st.get("rec_active", False)) else TRACK_LOSS_HOLD_SEC + if (now - float(st["track_loss_since"])) >= hold: + st.pop("track_loss_since", None) + st["mode"] = "SEARCH" + st["tracking_active"] = False + st["post_track_since"] = now + else: + # не трекаем — на всякий случай + st["tracking_active"] = False + + st["last_detected_state"] = detected + + # -------------------- форс-STOP -------------------- + if force: + if st.get("rec_active", False) or st.get("rec_last_sent") is not False: + _send_record_event(cam_idx, False) + + st["rec_active"] = False + st["rec_first_seen"] = None + st["rec_started_at"] = 0.0 + st["rec_last_off_at"] = now + st["rec_last_sent"] = False + + # сброс трекинга + st.pop("track_loss_since", None) + st["tracking_active"] = False + if st.get("mode") == "TRACK": + st["mode"] = "SEARCH" + return + + # -------------------- гистерезис записи -------------------- + if detected: + st["rec_last_seen"] = now + + if not st.get("rec_active", False): + if now - float(st.get("rec_last_off_at", 0.0) or 0.0) < float(getattr(config, "REC_MIN_OFF_SEC", 0.0)): + return + + if st.get("rec_first_seen") is None: + st["rec_first_seen"] = now + + if (now - float(st["rec_first_seen"])) >= float(getattr(config, "REC_ON_CONFIRM_SEC", 0.35)): + st["rec_active"] = True + st["rec_started_at"] = now + if st.get("rec_last_sent") is not True: + _send_record_event(cam_idx, True) # START → C++ + st["rec_last_sent"] = True + return + + # detected == False + st["rec_first_seen"] = None + + if st.get("rec_active", False): + rec_last_seen = float(st.get("rec_last_seen", 0.0) or 0.0) + rec_started_at = float(st.get("rec_started_at", 0.0) or 0.0) + + long_no_see = (now - rec_last_seen) >= float(getattr(config, "REC_OFF_GRACE_SEC", 1.2)) + long_enough = (now - rec_started_at) >= float(getattr(config, "REC_MIN_ON_SEC", 0.0)) + too_long = (now - rec_started_at) >= float(getattr(config, "REC_MAX_ON_SEC", 1e9)) + + if (long_no_see and long_enough) or too_long: + st["rec_active"] = False + st["rec_last_off_at"] = now + if st.get("rec_last_sent") is not False: + _send_record_event(cam_idx, False) # STOP → C++ + st["rec_last_sent"] = False diff --git a/ptz_tracker_modular/patrol.py b/ptz_tracker_modular/patrol.py new file mode 100644 index 0000000..c1f4d27 --- /dev/null +++ b/ptz_tracker_modular/patrol.py @@ -0,0 +1,1099 @@ +from __future__ import annotations +import logging, time, asyncio +from typing import Optional +from . import config, state +from .geom import ang_diff_signed, normalize360, sector_contains, is_near +from .ptz_io import move_ptz_bounded, stop_ptz, goto_preset_token, read_ptz_azimuth_deg +from .notify import notify_detected + +logger = logging.getLogger("PTZTracker") + + +# ===== DEBUG helpers ===== + +def _dbg_state(cam_idx: int, msg: str) -> None: + """ + Унифицированный вывод текущего состояния патруля по камере. + Важно: пишет на уровне DEBUG, чтобы не засрать лог при INFO. + """ + st = state.ptz_states.get(cam_idx, {}) + cfg = config.CAMERA_CONFIG.get(cam_idx, {}) + + logger.debug( + "[PATROL][DBG] cam=%s %s | " + "mode=%s act=%s paused=%s track=%s sync_wait=%s " + "dir=%s endpoint_latch=%s " + "leg=[%s..%s] " + "sector=[%s..%s] " + "az_deg=%s", + cam_idx, msg, + st.get("mode"), + st.get("patrol_active"), + st.get("patrol_paused"), + st.get("tracking_active"), + st.get("sync_wait"), + st.get("patrol_dir"), + st.get("endpoint_latch"), + st.get("leg_start"), + st.get("leg_end"), + cfg.get("sector_min_deg"), + cfg.get("sector_max_deg"), + st.get("az_deg"), + ) + + +def _dbg_skip(cam_idx: int, reason: str) -> None: + """ + Логируем «почему супервизор не трогает камеру», но с троттлингом, + чтобы сообщение не сыпалось на каждый тик. + """ + now = time.time() + key = f"_patrol_dbg_skip_{cam_idx}" + last = getattr(state, key, 0.0) + # не чаще раза в 3 секунды + if now - last >= 3.0: + setattr(state, key, now) + _dbg_state(cam_idx, f"skip: {reason}") + + +# ===== helpers ===== + +def _nearest_valid_preset(cam_idx: int) -> Optional[str]: + cfg = config.CAMERA_CONFIG[cam_idx] + st = state.ptz_states[cam_idx] + az = st.get("az_deg") + p1, p2 = cfg.get("preset1"), cfg.get("preset2") + if not (p1 and p2): + return p1 or p2 + if az is None: + return p1 + + a1, a2 = cfg.get("preset1_deg"), cfg.get("preset2_deg") + if isinstance(a1, (int, float)) and isinstance(a2, (int, float)): + d1 = abs(ang_diff_signed(az, a1)) + d2 = abs(ang_diff_signed(az, a2)) + return p1 if d1 <= d2 else p2 + return p1 + + +def _in_sector(pan: float | None, cfg: dict) -> bool | None: + """ + Обёртка над sector_contains(angle, dmin, dmax), безопасная к None. + Возвращает: + - True/False, если сектор задан и угол валиден; + - None, если сектор не задан или pan некорректен. + """ + smin = cfg.get("sector_min_deg") + smax = cfg.get("sector_max_deg") + if not ( + isinstance(smin, (int, float)) + and isinstance(smax, (int, float)) + and isinstance(pan, (int, float)) + ): + return None + return sector_contains(float(pan), float(smin), float(smax)) + + +def _near_endpoint(pan: float | None, cfg: dict, tol: float = 1.0) -> bool: + """ + Проверка, близко ли мы к одной из preset1_deg / preset2_deg + с учётом возможных None и нечисловых значений. + """ + if not isinstance(pan, (int, float)): + return False + a1 = cfg.get("preset1_deg") + a2 = cfg.get("preset2_deg") + if isinstance(a1, (int, float)) and is_near(float(pan), float(a1), tol): + return True + if isinstance(a2, (int, float)) and is_near(float(pan), float(a2), tol): + return True + return False + +def return_to_preset(cam_idx: int, reason: str = "") -> None: + st = state.ptz_states[cam_idx] + now = time.time() + logger.info("[RETURN] cam %s -> preset (reason=%s)", cam_idx, reason) + + # Анти-спам: если только что уже возвращали — не дёргать снова + if (now - float(st.get("last_return_at", 0.0))) < 2.5: + _dbg_state(cam_idx, "return_to_preset: suppressed by cooldown") + return + + # Гасим запись/детект и переводим в IDLE + notify_detected(cam_idx, False, force=True) + st["mode"] = "IDLE" + st["tracking_active"] = False + st["patrol_paused"] = False # важно: не блокируем future-resume + + # Сброс регуляторов и вспомогательных состояний + st["last_dx"] = 0.0 + st["last_dy"] = 0.0 + st["ix"] = 0.0 + st["iy"] = 0.0 + st["zoom_int"] = 0.0 + st["zoom_prev_cmd"] = 0.0 + st["zoom_ramp_start"] = 0.0 + st["stable_frames"] = 0 + st["alarm_sent"] = False + + st["zoom_state"] = 0 + st["was_zoomed_in"] = False + + # Сброс логики зума/потерь + st["zoom_lock"] = False + st["lock_candidate_since"] = None + st["loss_since"] = None + + # Убираем «эхо» от последней цели и защёлку у пресета + st["last_bbox"] = None + st["endpoint_latch"] = 0 # 0=вне пресетов, 1=P1, 2=P2 + + # Останавливаем текущие команды PTZ + stop_ptz(cam_idx) + + # Выбор ближайшего валидного пресета и установка направления следующей «ноги» + cfg = config.CAMERA_CONFIG[cam_idx] + p1, p2 = cfg.get("preset1"), cfg.get("preset2") + tok = _nearest_valid_preset(cam_idx) + if tok is not None and p1 and p2: + st["patrol_dir"] = +1 if tok == p1 else -1 + + # Важно: пока камера едет на goto, не слать continuous команды + settle = float(getattr(config, "GOTO_SETTLE_SEC", 2.5)) + dwell = float(getattr(config, "END_DWELL_SEC", 1.0) or 1.0) + st["goto_in_progress_until"] = now + settle + + if tok: + goto_preset_token(cam_idx, tok) + + # Сброс режима патруля на время возврата + st["patrol_active"] = False + st["sweep_active"] = False + st["sweep_pan"] = 0.0 + st["sweep_start_at"] = 0.0 + st["leg_start"] = now + st["leg_end"] = now + max(1.0, dwell) + + # Анти-спам метка + st["last_return_at"] = now + + #ВАЖНО: всегда планируем возобновление патруля (и при заданном секторе тоже) + try: + _dbg_state(cam_idx, f"return_to_preset: schedule patrol resume after settle+dwell={settle+dwell:.2f}s") + ensure_patrol_resume(cam_idx, delay=max(0.5, settle + dwell)) + except Exception: + logger.exception("[PATROL] ensure_patrol_resume failed in return_to_preset") + + _dbg_state(cam_idx, f"return_to_preset done reason={reason}") + +def ensure_patrol_resume(cam_idx: int, delay: float = 1.0) -> None: + st = state.ptz_states[cam_idx] + cfg = config.CAMERA_CONFIG[cam_idx] + now = time.time() + + # если идёт трек или стоит пауза — не возобновляем патруль + if st.get("tracking_active") or st.get("patrol_paused"): + _dbg_state(cam_idx, "ensure_patrol_resume: skip (tracking_active or patrol_paused)") + return + + # анти-спам резюмом (важно, иначе будешь вечно в goto) + RESUME_COOLDOWN_SEC = float(getattr(config, "PATROL_RESUME_COOLDOWN_SEC", 2.0)) + last = float(st.get("patrol_resume_last_at", 0.0) or 0.0) + if (now - last) < RESUME_COOLDOWN_SEC: + _dbg_state(cam_idx, "ensure_patrol_resume: suppressed by resume cooldown") + return + st["patrol_resume_last_at"] = now + + # если только что был goto — ждём пока доедет + gip = float(st.get("goto_in_progress_until", 0.0) or 0.0) + if gip > now: + delay = max(delay, gip - now) + + # --- Выбираем ближайший валидный пресет --- + try: + tok = _nearest_valid_preset(cam_idx) # "1"/"2"/... + except Exception: + tok = None + + p1 = cfg.get("preset1") + p2 = cfg.get("preset2") + + # --- ВАЖНО: выставляем направление патруля исходя из того, в каком пресете окажемся --- + # Логика: если стоим на P1 -> едем к P2 (dir=+1), если стоим на P2 -> едем к P1 (dir=-1) + if tok and p1 and p2: + if tok == p1: + st["patrol_dir"] = +1 + st["endpoint_latch"] = 1 + elif tok == p2: + st["patrol_dir"] = -1 + st["endpoint_latch"] = 2 + + # --- Парковка на пресет перед патрулём: ТОЛЬКО если реально не уже на нём --- + do_goto = False + if tok: + az = st.get("az_deg") + # если умеешь — используй твой _near_endpoint(az,cfg,tol) + try: + near = _near_endpoint(az, cfg, tol=float(getattr(config, "PATROL_PARK_TOL_DEG", 1.0))) + except Exception: + near = False + + # не дёргаем goto, если уже рядом с пресетом (иначе вечный goto и патруль “умирает”) + if not near: + do_goto = True + + if do_goto: + stop_ptz(cam_idx) + settle = float(getattr(config, "GOTO_SETTLE_SEC", 2.5)) + st["goto_in_progress_until"] = max(float(st.get("goto_in_progress_until", 0.0) or 0.0), now + settle) + goto_preset_token(cam_idx, tok) + delay = max(delay, settle + 0.5) + + # --- Планируем патруль --- + st["patrol_active"] = True + # чтобы bounded-тактик не скипал из-за mode + if st.get("mode") not in ("IDLE", "SEARCH"): + st["mode"] = "IDLE" + + st["leg_extend_total"] = 0.0 # важно для твоего bounded-продления + st["leg_start"] = now + delay + st["leg_end"] = st["leg_start"] + config.PATROL_LEG_SEC + + # sweep режим только если нет сектора + if cfg.get("sector_min_deg") is None or cfg.get("sector_max_deg") is None: + base = config.SWEEP_PAN_SPEED if st.get("patrol_dir", +1) > 0 else -config.SWEEP_PAN_SPEED + st["sweep_active"] = True + st["sweep_pan"] = float(cfg.get("pan_sign", 1) or 1) * base + st["sweep_start_at"] = st["leg_start"] + config.SWEEP_SETTLE_SEC + else: + st["sweep_active"] = False + st["sweep_pan"] = 0.0 + st["sweep_start_at"] = 0.0 + + _dbg_state(cam_idx, f"ensure_patrol_resume: scheduled leg after delay={delay} tok={tok} dir={st.get('patrol_dir')}") + + + + +def patrol_start_leg(cam_idx: int, dir_sign: int) -> None: + st = state.ptz_states[cam_idx] + + # не стартуем «ногу», если трек или пауза патруля + if st.get("tracking_active") or st.get("patrol_paused"): + _dbg_state(cam_idx, "patrol_start_leg: blocked (tracking_active or patrol_paused)") + return + + cfg = config.CAMERA_CONFIG[cam_idx] + st["patrol_dir"] = 1 if dir_sign >= 0 else -1 + st["patrol_active"] = True + now = time.time() + st["leg_start"] = now + st["leg_end"] = now + config.PATROL_LEG_SEC + + # НИЧЕГО НЕ ДЁРГАЕМ GOTO — патруль дальше ведёт supervisor continuous-ом + # Просто запускаем sweep (если включен unbounded режим) + if config.SMOOTH_SWEEP_ENABLE: + base = config.SWEEP_PAN_SPEED if (st["patrol_dir"] > 0) else -config.SWEEP_PAN_SPEED + st["sweep_active"] = True + st["sweep_pan"] = float(cfg.get("pan_sign", 1) or 1) * base + st["sweep_start_at"] = now + config.SWEEP_SETTLE_SEC + else: + st["sweep_active"] = False + st["sweep_pan"] = 0.0 + st["sweep_start_at"] = 0.0 + + st["shot_pause_until"] = 0.0 + st["shot_next_at"] = now + config.SWEEP_SETTLE_SEC + 0.35 + + logger.info("[PATROL] cam %s leg %s: started (no GOTO)", cam_idx, "+1" if st["patrol_dir"] > 0 else "-1") + _dbg_state(cam_idx, "patrol_start_leg: started") + + + +def patrol_finish_and_reverse(cam_idx: int) -> None: + cfg = config.CAMERA_CONFIG[cam_idx] + st = state.ptz_states[cam_idx] + now = time.time() + _dbg_state(cam_idx, "patrol_finish_and_reverse: begin") + + # Останавливаем sweep-состояния (bounded/unbounded обновится ниже) + st["sweep_active"] = False + st["sweep_pan"] = 0.0 + + # ВАЖНО: никакого GOTO на конец. Просто меняем направление ноги и едем continuous-ом. + st["patrol_dir"] = -st["patrol_dir"] + st["leg_start"] = now + st["leg_end"] = now + config.PATROL_LEG_SEC + + # для unbounded — включаем sweep обратно + if cfg.get("sector_min_deg") is None or cfg.get("sector_max_deg") is None: + base = config.SWEEP_PAN_SPEED if (st["patrol_dir"] > 0) else -config.SWEEP_PAN_SPEED + st["sweep_pan"] = float(cfg.get("pan_sign", 1) or 1) * base + st["sweep_active"] = True + st["sweep_start_at"] = now + float(max(0.0, config.END_DWELL_SEC)) + else: + # bounded — sweep не нужен, bounded supervisor сам поведёт + st["sweep_active"] = False + st["sweep_pan"] = 0.0 + st["sweep_start_at"] = 0.0 + + st["mode"] = "IDLE" + notify_detected(cam_idx, False, force=True) + + _dbg_state(cam_idx, "patrol_finish_and_reverse: reversed leg (no GOTO)") + + + +def patrol_start_leg_bounded(cam_idx: int, dir_sign: int) -> None: + st = state.ptz_states[cam_idx] + + if st.get("tracking_active") or st.get("patrol_paused"): + _dbg_state(cam_idx, "patrol_start_leg_bounded: blocked (tracking_active or patrol_paused)") + return + + cfg = config.CAMERA_CONFIG[cam_idx] + if cfg.get("sector_min_deg") is None or cfg.get("sector_max_deg") is None: + _dbg_state(cam_idx, "patrol_start_leg_bounded: no sector -> fallback to unbounded") + patrol_start_leg(cam_idx, dir_sign) + return + + st["patrol_dir"] = 1 if dir_sign >= 0 else -1 + st["patrol_active"] = True + now = time.time() + st["leg_start"] = now + st["leg_end"] = now + config.PATROL_LEG_SEC + + # bounded режим: движение делает patrol_supervisor_tick_bounded() (continuous), + # поэтому тут GOTO не используем + st["sweep_active"] = False + st["sweep_pan"] = 0.0 + st["sweep_start_at"] = 0.0 + st["shot_pause_until"] = 0.0 + st["shot_next_at"] = now + 1.0 + + logger.info("[PATROL/BOUNDED] cam %s leg %s: started (no GOTO)", cam_idx, "+1" if st["patrol_dir"] > 0 else "-1") + _dbg_state(cam_idx, "patrol_start_leg_bounded: started") + +def patrol_supervisor_tick_bounded() -> None: + now = time.time() + + dead_band_deg = 0.25 + min_step_cmd = 0.02 + az_none_fallback_sec = 2.0 + + # New (configurable) + end_tol_deg = float(getattr(config, "PATROL_END_TOL_DEG", 1.2)) + extend_step_sec = float(getattr(config, "PATROL_LEG_EXTEND_SEC", 2.0)) + extend_max_sec = float(getattr(config, "PATROL_LEG_EXTEND_MAX_SEC", 20.0)) + + max_speed = float(getattr(config, "PATROL_MAX_SPEED", 0.25)) + gain_deg = float(getattr(config, "PATROL_PAN_GAIN_DEG", 45.0)) + scale = float(getattr(config, "PATROL_SPEED_SCALE", 0.70)) + + for cam in config.PTZ_CAM_IDS: + st = state.ptz_states[cam] + cfg = config.CAMERA_CONFIG[cam] + + # While camera is doing goto — don't send continuous commands + if now < float(st.get("goto_in_progress_until", 0.0) or 0.0): + _dbg_skip(cam, "bounded: goto_in_progress") + continue + + if st.get("tracking_active") or st.get("patrol_paused"): + _dbg_skip(cam, "bounded: tracking_active or patrol_paused") + continue + + if cfg.get("sector_min_deg") is None or cfg.get("sector_max_deg") is None: + _dbg_skip(cam, "bounded: no sector_min/max_deg") + continue + + if not st.get("patrol_active"): + _dbg_skip(cam, "bounded: patrol_active=False") + continue + + if st.get("mode") not in ("IDLE", "SEARCH"): + _dbg_skip(cam, f"bounded: mode={st.get('mode')} not IDLE/SEARCH") + continue + + az1 = cfg.get("preset1_deg") + az2 = cfg.get("preset2_deg") + + if not isinstance(az1, (int, float)) or not isinstance(az2, (int, float)): + logger.warning( + "[PATROL] cam %s: no preset azimuths -> fallback to smooth sweep", + cam, + ) + cfg["sector_min_deg"] = None + cfg["sector_max_deg"] = None + continue + + span = abs(ang_diff_signed(az2, az1)) + if span < 1.0: + logger.warning( + "[PATROL] cam %s: too small span (%.2f°) -> fallback to smooth sweep", + cam, + span, + ) + cfg["sector_min_deg"] = None + cfg["sector_max_deg"] = None + continue + + current_az = st.get("az_deg") + if current_az is None: + if st.get("az_none_since") is None: + st["az_none_since"] = now + + if now - st["az_none_since"] >= az_none_fallback_sec: + logger.warning( + "[PATROL] cam %s: azimuth is None > %.1fs -> temporary unbounded sweep", + cam, + az_none_fallback_sec, + ) + cfg["sector_min_deg"] = None + cfg["sector_max_deg"] = None + continue + + st["az_none_since"] = None + + # Determine the "end" azimuth for this leg + patrol_dir = st.get("patrol_dir", +1) + end_az = normalize360(az2 if patrol_dir > 0 else az1) + + # If leg time is over — don't reverse until we actually reached the end + if now >= float(st.get("leg_end", 0.0) or 0.0): + err_to_end = abs(ang_diff_signed(end_az, float(current_az))) + + if err_to_end <= end_tol_deg: + _dbg_state( + cam, + f"bounded: leg_end reached AND near end (err={err_to_end:.2f}°) -> reverse", + ) + patrol_finish_and_reverse(cam) + continue + + # Extend the leg, but with a cap + ext_total = float(st.get("leg_extend_total", 0.0) or 0.0) + if ext_total < extend_max_sec: + st["leg_end"] = now + extend_step_sec + st["leg_extend_total"] = ext_total + extend_step_sec + _dbg_state( + cam, + ( + "bounded: leg_end but NOT at end " + f"(err={err_to_end:.2f}°) -> extend +{extend_step_sec}s " + f"(total={st['leg_extend_total']:.1f}s)" + ), + ) + else: + _dbg_state( + cam, + ( + "bounded: extend limit reached " + f"(err={err_to_end:.2f}°) -> reverse anyway" + ), + ) + patrol_finish_and_reverse(cam) + continue + + # Normal trajectory: target_az by smoothstep/ease + elapsed = max(0.0, now - st["leg_start"]) + progress = max( + 0.0, + min(1.0, elapsed / max(1e-6, config.PATROL_LEG_SEC)), + ) + ease = (3.0 * progress * progress) - (2.0 * progress * progress * progress) + + if patrol_dir > 0: + target_az = az1 + ang_diff_signed(az2, az1) * ease + else: + target_az = az2 + ang_diff_signed(az1, az2) * ease + + target_az = normalize360(target_az) + + err_deg = ang_diff_signed(target_az, float(current_az)) + if abs(err_deg) <= dead_band_deg: + pan_speed = 0.0 + else: + pan_speed = scale * (err_deg / max(1e-6, gain_deg)) + pan_speed = max(-max_speed, min(max_speed, pan_speed)) + + if abs(pan_speed) < min_step_cmd: + pan_speed = 0.0 + + if pan_speed != 0.0: + move_ptz_bounded(cam, pan_speed, 0.0, 0.0) + _dbg_state( + cam, + ( + f"bounded: move pan_speed={pan_speed:.3f} " + f"err_deg={err_deg:.3f} target_az={target_az:.2f} end_az={end_az:.2f}" + ), + ) + + +def patrol_supervisor_tick() -> None: + now = time.time() + + for cam in config.PTZ_CAM_IDS: + st = state.ptz_states[cam] + cfg = config.CAMERA_CONFIG[cam] + + # While camera is doing goto — don't send continuous commands + if now < float(st.get("goto_in_progress_until", 0.0) or 0.0): + _dbg_skip(cam, "unbounded: goto_in_progress") + continue + + if st.get("tracking_active") or st.get("patrol_paused"): + _dbg_skip(cam, "unbounded: tracking_active or patrol_paused") + continue + + # If bounded mode is active (sector is defined) — unbounded tick does nothing + if cfg.get("sector_min_deg") is not None and cfg.get("sector_max_deg") is not None: + continue + + if not ( + config.SMOOTH_SWEEP_ENABLE + and st.get("patrol_active") + and st.get("sweep_active") + ): + _dbg_skip( + cam, + ( + "sweep not active | " + f"smooth={config.SMOOTH_SWEEP_ENABLE} " + f"patrol={st.get('patrol_active')} " + f"sweep={st.get('sweep_active')}" + ), + ) + continue + + if st.get("patrol_active") and now >= st.get("leg_end", 0.0): + _dbg_state(cam, "unbounded: leg_end reached -> reverse") + patrol_finish_and_reverse(cam) + continue + + if now < st.get("sweep_start_at", 0.0): + continue + + if st.get("mode") not in ("IDLE", "SEARCH"): + _dbg_skip(cam, f"unbounded: mode={st.get('mode')} not IDLE/SEARCH") + continue + + elapsed = max(0.0, now - st["leg_start"]) + remain = max(0.0, st["leg_end"] - now) + + r_up = min(1.0, elapsed / max(1e-3, config.SWEEP_RAMP_SEC)) + r_dn = min(1.0, remain / max(1e-3, config.SWEEP_RAMP_SEC)) + ramp = min(r_up, r_dn) + + pan = max(-1.0, min(1.0, st.get("sweep_pan", 0.0) * ramp)) + if abs(pan) > 1e-3: + move_ptz_bounded(cam, pan, 0.0, 0.0) + _dbg_state(cam, f"unbounded: move pan={pan:.3f} ramp={ramp:.3f}") + + +def patrol_init_on_startup() -> None: + for cam in config.PTZ_ORDER: + dir_sign = config.PATROL_START_DIR.get(cam, +1) + cfg = config.CAMERA_CONFIG[cam] + if cfg.get("sector_min_deg") is not None and cfg.get("sector_max_deg") is not None: + patrol_start_leg_bounded(cam, dir_sign) + else: + patrol_start_leg(cam, dir_sign) + start_token = cfg.get("preset1") if dir_sign >= 0 else cfg.get("preset2") + dir_txt = "+1" if dir_sign >= 0 else "-1" + logger.info( + "[PATROL] cam %s leg %s: goto START %s", + cam, dir_txt, start_token + ) + _dbg_state(cam, f"patrol_init_on_startup: dir_sign={dir_sign}") + +def patrol_resync_on_endpoint(cam: int, az: float) -> None: + st = state.ptz_states[cam]; cfg = config.CAMERA_CONFIG[cam] + + # не трогаем ресинк, если трек или пауза патруля + if st.get("tracking_active") or st.get("patrol_paused"): + _dbg_state(cam, "patrol_resync_on_endpoint: skip (tracking_active or patrol_paused)") + return + + # Инициализация защёлки по месту + if "endpoint_latch" not in st: + st["endpoint_latch"] = 0 # 0=вне пресетов, 1=P1, 2=P2 + + if not st.get("patrol_active"): + st["endpoint_latch"] = 0 + _dbg_state(cam, "patrol_resync_on_endpoint: patrol_active=False -> reset latch") + return + + p1a, p2a = cfg.get("preset1_deg"), cfg.get("preset2_deg") + if p1a is None or p2a is None: + st["endpoint_latch"] = 0 + _dbg_state(cam, "patrol_resync_on_endpoint: no preset azimuths -> reset latch") + return + + now = time.time() + pan_sign = float(cfg.get("pan_sign", 1) or 1) + + def _dir_sign(a_from: float, a_to: float) -> float: + d = ang_diff_signed(a_to, a_from) + return 1.0 if d >= 0.0 else -1.0 + + # Находимся у P1 + if is_near(az, p1a): + if st.get("endpoint_latch") == 1: + return # уже защёлкнулись на P1 — не перезапускать ногу + st["endpoint_latch"] = 1 + st["patrol_dir"] = +1 + st["leg_start"] = now + st["leg_end"] = now + config.PATROL_LEG_SEC + st["mode"] = "IDLE" + if config.SMOOTH_SWEEP_ENABLE: + base_dir = _dir_sign(p1a, p2a) + st["sweep_active"] = True + st["sweep_pan"] = pan_sign * config.SWEEP_PAN_SPEED * base_dir + st["sweep_start_at"] = now + config.SWEEP_SETTLE_SEC + else: + st["sweep_active"] = False; st["sweep_pan"] = 0.0; st["sweep_start_at"] = 0.0 + logger.info("[PATROL][RESYNC] cam %s at P1 → leg to P2 (dir=+1)", cam) + _dbg_state(cam, "patrol_resync_on_endpoint: latch=P1, leg to P2") + return + + # Находимся у P2 + if is_near(az, p2a): + if st.get("endpoint_latch") == 2: + return # уже защёлкнулись на P2 + st["endpoint_latch"] = 2 + st["patrol_dir"] = -1 + st["leg_start"] = now + st["leg_end"] = now + config.PATROL_LEG_SEC + st["mode"] = "IDLE" + if config.SMOOTH_SWEEP_ENABLE: + base_dir = _dir_sign(p2a, p1a) + st["sweep_active"] = True + st["sweep_pan"] = pan_sign * config.SWEEP_PAN_SPEED * base_dir + st["sweep_start_at"] = now + config.SWEEP_SETTLE_SEC + else: + st["sweep_active"] = False; st["sweep_pan"] = 0.0; st["sweep_start_at"] = 0.0 + logger.info("[PATROL][RESYNC] cam %s at P2 → leg to P1 (dir=-1)", cam) + _dbg_state(cam, "patrol_resync_on_endpoint: latch=P2, leg to P1") + return + + # Ушли с пресета — снимаем защёлку + st["endpoint_latch"] = 0 + _dbg_state(cam, "patrol_resync_on_endpoint: left presets -> latch=0") + + +# ===== SYNC patrol ===== + +def is_in_sync_group(cam: int) -> Optional[str]: + return config.sync_group_by_cam.get(cam) + + +def cohort_dir_for_cam(cam: int, group_dir: int) -> int: + sign = config.cohort_sign_by_cam.get(cam, +1) + return +1 if sign * int(group_dir) >= 0 else -1 + + +def _start_token_for_dir(cfg: dict, dir_sign: int) -> Optional[str]: + p1, p2 = cfg.get("preset1"), cfg.get("preset2") + if not (p1 and p2): return None + return p1 if dir_sign >= 0 else p2 + + +def sync_mark_wait(cam: int) -> None: + st = state.ptz_states[cam] + st["patrol_active"] = False + st["sweep_active"] = False + st["sweep_pan"] = 0.0 + st["mode"] = "IDLE" + st["sync_wait"] = True + notify_detected(cam, False, force=True) + _dbg_state(cam, "sync_mark_wait: waiting for cohort leg") + + +def sync_park_to_start(cam: int, next_dir: int) -> None: + st = state.ptz_states[cam] + # в синхро-режиме тоже не дёргать PTZ, если трек/пауза + if st.get("tracking_active") or st.get("patrol_paused"): + _dbg_state(cam, "sync_park_to_start: blocked (tracking_active or patrol_paused)") + return + cfg = config.CAMERA_CONFIG[cam] + tok = _start_token_for_dir(cfg, next_dir) + stop_ptz(cam) + if tok: + goto_preset_token(cam, tok) + sync_mark_wait(cam) + _dbg_state(cam, f"sync_park_to_start: parked to start preset={tok}, next_dir={next_dir}") + + +def sync_activate_leg(cam: int, dir_sign: int, start_at: float, end_at: float) -> None: + st = state.ptz_states[cam] + # не активировать «ногу», если трек/пауза + if st.get("tracking_active") or st.get("patrol_paused"): + _dbg_state(cam, "sync_activate_leg: blocked (tracking_active or patrol_paused)") + return + st["patrol_dir"] = 1 if dir_sign >= 0 else -1 + st["patrol_active"] = True + st["leg_start"] = start_at + st["leg_end"] = end_at + st["sweep_active"] = False + st["sweep_pan"] = 0.0 + st["sweep_start_at"] = 0.0 + st["sync_wait"] = False + _dbg_state(cam, f"sync_activate_leg: dir_sign={dir_sign}, leg=[{start_at}..{end_at}]") + + +async def sync_patrol_coordinator_loop(): + while True: + t_now = time.time() + try: + for gname, coh in (config.SYNC_COHORTS or {}).items(): + stg = state.sync_state[gname] + if t_now < stg.get("next_start_at", 0.0): + continue + group_dir = stg["dir"] + next_start = t_now + config.SYNC_BARRIER_SEC + for cam in (coh.get("plus", []) + coh.get("minus", [])): + st = state.ptz_states[cam] + if st.get("mode") == "TRACK": + _dbg_skip(cam, f"sync: skip park, mode=TRACK (group={gname})") + continue + # не парковать, если трек/пауза + if st.get("tracking_active") or st.get("patrol_paused"): + _dbg_skip(cam, f"sync: skip park (tracking_active or patrol_paused), group={gname}") + continue + want_dir = cohort_dir_for_cam(cam, group_dir) + sync_park_to_start(cam, want_dir) + + await asyncio.sleep(max(0.0, next_start - time.time())) + + leg_start = time.time() + leg_end = leg_start + config.PATROL_LEG_SEC + for cam in (coh.get("plus", []) + coh.get("minus", [])): + st = state.ptz_states[cam] + if st.get("mode") == "TRACK": + _dbg_skip(cam, f"sync: skip leg activate, mode=TRACK (group={gname})") + continue + # не активировать, если трек/пауза + if st.get("tracking_active") or st.get("patrol_paused"): + _dbg_skip(cam, f"sync: skip leg activate (tracking_active or patrol_paused), group={gname}") + continue + want_dir = cohort_dir_for_cam(cam, group_dir) + sync_activate_leg(cam, want_dir, leg_start, leg_end) + + dwell = max(0.0, float(config.SYNC_DWELL_SEC)) + stg["leg_idx"] += 1 + stg["leg_start"] = leg_start + stg["leg_end"] = leg_end + stg["next_start_at"] = leg_end + dwell + stg["dir"] = -group_dir + + await asyncio.sleep(0.05) + except Exception as e: + logger.error("[SYNC] coordinator error: %s", e) + await asyncio.sleep(0.2) + +async def track_return_watchdog(cam_idx: int) -> None: + """ + Watchdog: + - TRACK: следит за выходом за сектор и лимитом времени трекинга + - IDLE/SEARCH: + * после окончания TRACK: один раз паркует в пресет (post-track park once) и возобновляет патруль + * если патруль активен: следит за "вылетом" из сектора и плохим tilt/zoom → паркует в пресет и возобновляет + * если патруль НЕ активен: возвращает к пресетам, если камера долго вне пресетов + """ + import time + from . import state, config + from .patrol import ensure_patrol_resume + from .sector import sector_contains # если у тебя это в другом модуле — поправь импорт + # _near_endpoint и return_to_preset должны быть доступны в текущем модуле/области + # (как у тебя в версии выше) + + st = state.ptz_states[cam_idx] + cfg = config.CAMERA_CONFIG.get(cam_idx, {}) + + # ====== TRACK (во время трекинга) ====== + TRACK_OFFSECTOR_GRACE_SEC = float(getattr(config, "TRACK_OFFSECTOR_GRACE_SEC", 3.0)) + TRACK_MAX_UNINTERRUPTED_SEC = float(getattr(config, "TRACK_MAX_UNINTERRUPTED_SEC", 30.0)) + + # ====== POST-TRACK (после потери цели) ====== + POST_TRACK_RETURN_SEC = float(getattr(config, "POST_TRACK_RETURN_SEC", 1.4)) # ждать после потери цели перед парковкой + POST_TRACK_GUARD_SEC = float(getattr(config, "POST_TRACK_GUARD_SEC", 4.0)) # сколько времени блокируем повторные "парковки" после post-track park + + # ====== IDLE/SEARCH (без трекинга) ====== + IDLE_OFFPRESET_GRACE_SEC = float(getattr(config, "IDLE_OFFPRESET_GRACE_SEC", 7.0)) + + # если патруль активен и камера "вылетела" из сектора — сколько ждать до парковки + PATROL_OFFSECTOR_RETURN_SEC = float(getattr(config, "PATROL_OFFSECTOR_RETURN_SEC", 6.0)) + + # tilt-контроль в патруле + PATROL_TILT_TOL_DEG = float(getattr(config, "PATROL_TILT_TOL_DEG", 4.0)) + PATROL_BAD_TILT_GRACE_SEC = float(getattr(config, "PATROL_BAD_TILT_GRACE_SEC", 2.5)) + + # zoom-сброс в патруле (если нет детекций) + PATROL_ZOOM_RESET_SEC = float(getattr(config, "PATROL_ZOOM_RESET_SEC", 9.0)) + PATROL_RECOVER_AFTER_PARK_SEC = float(getattr(config, "PATROL_RECOVER_AFTER_PARK_SEC", 1.0)) + + # защита от спама парковкой + PARK_COOLDOWN_SEC = float(getattr(config, "WATCHDOG_PARK_COOLDOWN_SEC", 6.0)) + # короткая пауза после парковки, чтобы статус успел обновиться (иначе можно триггерить повторно на старых данных) + PARK_IGNORE_SEC = float(getattr(config, "WATCHDOG_PARK_IGNORE_SEC", 1.6)) + + # ===== helpers ===== + def _norm_tilt_cfg(v: float) -> float: + """ + Нормализация tilt из cameras.toml/конфига. + Часто tilt хранится в десятых/сотых градуса, а status отдаёт градусы. + """ + av = abs(v) + if 900 < av <= 90000: + return v / 100.0 + if 90 < av <= 9000: + return v / 10.0 + return v + + def _get_tilt_deg() -> float | None: + v = st.get("tilt_deg") + if isinstance(v, (int, float)): + return float(v) + v = st.get("tilt") + if isinstance(v, (int, float)): + return float(v) + return None + + def _expected_patrol_tilt_deg() -> float | None: + """ + Возвращает ожидаемый tilt патруля в градусах или None если данных нет/мусор. + Жёстко режем мусор вроде -138, -150 и т.п., чтобы не ломать патруль. + """ + # 1) явный patrol_tilt_deg + v = cfg.get("patrol_tilt_deg") + if isinstance(v, (int, float)): + vv = _norm_tilt_cfg(float(v)) + if abs(vv) <= 90.0: + return vv + return None + + # 2) fallback: tilt пресетов + for key in ("preset1_tilt_deg", "preset2_tilt_deg"): + v = cfg.get(key) + if isinstance(v, (int, float)): + vv = _norm_tilt_cfg(float(v)) + if abs(vv) <= 90.0: + return vv + return None + + def _in_sector_safe(az: float | None) -> bool | None: + smin = cfg.get("sector_min_deg") + smax = cfg.get("sector_max_deg") + if not (isinstance(az, (int, float)) and isinstance(smin, (int, float)) and isinstance(smax, (int, float))): + return None + return sector_contains(float(az), float(smin), float(smax)) + + def _reset_accumulators(): + st.pop("offsector_since", None) + st.pop("offpreset_since", None) + st.pop("patrol_offsector_since", None) + st.pop("patrol_bad_tilt_since", None) + st.pop("patrol_zoom_bad_since", None) + + def _throttled_park(reason: str) -> bool: + """ + Единая точка парковки: уважает goto_in_progress + cooldown + ignore window. + Возвращает True если реально начали парковку. + """ + now = time.time() + + # не мешаем выполнению goto + if now < float(st.get("goto_in_progress_until", 0.0) or 0.0): + return False + + # "тихий период" после предыдущей парковки — даём статусу обновиться + if now < float(st.get("watchdog_ignore_until", 0.0) or 0.0): + return False + + last = float(st.get("watchdog_last_park_at", 0.0) or 0.0) + if (now - last) < PARK_COOLDOWN_SEC: + return False + + st["watchdog_last_park_at"] = now + st["watchdog_ignore_until"] = now + PARK_IGNORE_SEC + + # сбросить накопители, чтобы сразу после goto не сработало снова + _reset_accumulators() + + # важное: подготовить патруль к нормальному старту после парковки + st["mode"] = "IDLE" + st["endpoint_latch"] = 0 + st["leg_extend_total"] = 0.0 + + return_to_preset(cam_idx, reason=reason) + ensure_patrol_resume(cam_idx, delay=PATROL_RECOVER_AFTER_PARK_SEC) + return True + + # ===== loop ===== + while True: + await asyncio.sleep(0.2) + now = time.time() + + # пока едем на goto — ничего не трогаем + if now < float(st.get("goto_in_progress_until", 0.0) or 0.0): + _reset_accumulators() + # post-track таймер не трогаем, но "парковать" не будем пока goto идёт + continue + + # если в ignore window — тоже не дёргаем (статус ещё может быть старый) + if now < float(st.get("watchdog_ignore_until", 0.0) or 0.0): + _reset_accumulators() + continue + + az = st.get("az_deg") + mode = st.get("mode") + + # ---- фикс: детектируем переход TRACK -> не TRACK ---- + last_mode = st.get("last_mode") + if last_mode == "TRACK" and mode != "TRACK": + st["post_track_since"] = now + st["post_track_park_done"] = False + st["last_mode"] = mode + + # ====== TRACK ====== + if mode == "TRACK": + if "track_start_at" not in st: + st["track_start_at"] = now + + # во время трека не запускаем post-track паркинг + st.pop("post_track_since", None) + st["post_track_park_done"] = False + st.pop("post_track_guard_until", None) + + # контроль выхода из сектора во время TRACK + if isinstance(az, (int, float)): + in_sector = _in_sector_safe(float(az)) + near_endpoint = _near_endpoint(az, cfg, tol=1.0) + + if in_sector is False and not near_endpoint: + if "offsector_since" not in st: + st["offsector_since"] = now + elif now - st["offsector_since"] > TRACK_OFFSECTOR_GRACE_SEC: + st.pop("offsector_since", None) + _throttled_park("track_offsector_timeout") + continue + else: + st.pop("offsector_since", None) + + # лимит непрерывного трекинга + if now - float(st.get("track_start_at", now)) > TRACK_MAX_UNINTERRUPTED_SEC: + st.pop("track_start_at", None) + _throttled_park("track_max_duration") + continue + + else: + st.pop("track_start_at", None) + st.pop("offsector_since", None) + + # ====== IDLE / SEARCH ====== + if mode in ("IDLE", "SEARCH"): + patrol_active = bool(st.get("patrol_active")) + + # ---- (0) Post-track park ONCE ---- + pts = st.get("post_track_since") + guard_until = float(st.get("post_track_guard_until", 0.0) or 0.0) + if isinstance(pts, (int, float)) and not st.get("post_track_park_done", False): + # не даём другим правилам сразу после трека дёргать парковку много раз + if now < guard_until: + _reset_accumulators() + continue + + if (now - float(pts)) >= POST_TRACK_RETURN_SEC: + started = _throttled_park("track_end_park") + if started: + st["post_track_park_done"] = True + st["post_track_guard_until"] = now + POST_TRACK_GUARD_SEC + continue + + # ---- 1) если патруль активен: самовосстановление по сектору/tilt/zoom ---- + if patrol_active: + # (A) если камера реально вылетела из сектора — вернуть + in_sector = _in_sector_safe(float(az)) if isinstance(az, (int, float)) else None + near_endpoint = _near_endpoint(az, cfg, tol=1.0) + + if in_sector is False and not near_endpoint: + if "patrol_offsector_since" not in st: + st["patrol_offsector_since"] = now + elif now - st["patrol_offsector_since"] >= PATROL_OFFSECTOR_RETURN_SEC: + st.pop("patrol_offsector_since", None) + _throttled_park("patrol_offsector") + continue + else: + st.pop("patrol_offsector_since", None) + + # (B) tilt “застрял” (после трека/ручного) + exp_tilt = _expected_patrol_tilt_deg() + cur_tilt = _get_tilt_deg() + + # если exp_tilt мусор — просто не проверяем tilt + if (exp_tilt is not None) and (cur_tilt is not None): + bad_tilt = abs(float(cur_tilt) - float(exp_tilt)) > PATROL_TILT_TOL_DEG + if bad_tilt: + if "patrol_bad_tilt_since" not in st: + st["patrol_bad_tilt_since"] = now + elif now - st["patrol_bad_tilt_since"] >= PATROL_BAD_TILT_GRACE_SEC: + st.pop("patrol_bad_tilt_since", None) + _throttled_park("patrol_bad_tilt") + continue + else: + st.pop("patrol_bad_tilt_since", None) + else: + st.pop("patrol_bad_tilt_since", None) + + # (C) zoom “застрял”: если давно нет цели, а zoom_state/lock не нулевые — парк в пресет + last_seen = float(st.get("last_seen", 0.0) or 0.0) + no_target_for = (now - last_seen) if last_seen > 0 else 1e9 + + zoom_stuck = ( + bool(st.get("zoom_lock", False)) + or (abs(float(st.get("zoom_prev_cmd", 0.0) or 0.0)) > 1e-3) + or (int(st.get("zoom_state", 0) or 0) != 0) + or (abs(float(st.get("zoom_int", 0.0) or 0.0)) > 0.02) + ) + + if (no_target_for >= PATROL_ZOOM_RESET_SEC) and zoom_stuck: + if "patrol_zoom_bad_since" not in st: + st["patrol_zoom_bad_since"] = now + elif now - st["patrol_zoom_bad_since"] >= 0.8: + st.pop("patrol_zoom_bad_since", None) + _throttled_park("patrol_zoom_reset") + continue + else: + st.pop("patrol_zoom_bad_since", None) + + # при активном патруле offpreset-таймер не нужен + st.pop("offpreset_since", None) + continue + + # ---- 2) если патруля нет: классическое “idle away from presets too long” ---- + near_preset = _near_endpoint(az, cfg, tol=1.0) + if near_preset: + st.pop("offpreset_since", None) + else: + if "offpreset_since" not in st: + st["offpreset_since"] = now + elif now - st["offpreset_since"] > IDLE_OFFPRESET_GRACE_SEC: + st.pop("offpreset_since", None) + _throttled_park("idle_offpreset_timeout") + continue + + else: + st.pop("offpreset_since", None) + st.pop("patrol_offsector_since", None) + st.pop("patrol_bad_tilt_since", None) + st.pop("patrol_zoom_bad_since", None) + + + + + + + + diff --git a/ptz_tracker_modular/postprocess(main).py b/ptz_tracker_modular/postprocess(main).py new file mode 100644 index 0000000..1484dee --- /dev/null +++ b/ptz_tracker_modular/postprocess(main).py @@ -0,0 +1,587 @@ +from __future__ import annotations +import time +import logging +from typing import Any, List, Tuple + +import numpy as np +import torch +import cv2 + +from . import config, state +from .utils import clipf, wrap_deg, deg_to_compass +from .preview import maybe_downscale, publish_preview +from . import alarms +from .notify import notify_detected +from .ptz_io import move_ptz_bounded, call_autofocus, stop_ptz +from .patrol import return_to_preset, ensure_patrol_resume + +logger = logging.getLogger("PTZTracker") + +_device = "cuda" if torch.cuda.is_available() else "cpu" +_targets_tensor = torch.tensor(config.TARGET_CLASSES, device=_device) + +# === Zoom / устойчивость === +_Z_HOLD_AFTER_LOSS = getattr(config, "ZOOM_LOSS_HOLD_SEC", 0.70) +_Z_SIGN_HYST = getattr(config, "ZOOM_SIGN_HYST", 0.020) +_Z_MIN_CMD = getattr(config, "Z_MIN_CMD", 0.05) +_Z_IN_RAMP = getattr(config, "Z_RAMP_TIME", 0.45) +_Z_FF_GAIN = getattr(config, "Z_FF_GAIN", 0.55) +_Z_P = getattr(config, "Z_P", 2.4) +_Z_I = getattr(config, "Z_I", 0.16) +_Z_INT_CLAMP = getattr(config, "Z_INT_CLAMP", 0.15) +_Z_SPEED_LIMIT_OUT = getattr(config, "Z_SPEED_LIMIT_OUT", -0.65) +_Z_SPEED_LIMIT_IN = getattr(config, "Z_SPEED_LIMIT_IN", 0.45) +_Z_CENTER_GATE_IN = getattr(config, "Z_CENTER_GATE_BASE", 0.06) +_Z_CENTER_GATE_SCALE = getattr(config, "Z_CENTER_GATE_SCALE", 0.20) +_Z_UNLOCK_EDGE = getattr(config, "Z_UNLOCK_EDGE", 0.050) +_Z_PAN_BOOST_IN = getattr(config, "Z_PAN_BOOST_WHEN_IN", getattr(config, "Z_PAN_BOOST", 1.35)) + +# === TRACK lifecycle === +TRACK_END_LOSS_SEC = float(getattr(config, "TRACK_END_LOSS_SEC", 0.90)) # сколько ждать без детекции, чтобы завершить TRACK +TRACK_END_RETURN_COOLDOWN_SEC = float(getattr(config, "TRACK_END_RETURN_COOLDOWN_SEC", 2.5)) # анти-спам goto +TRACK_END_RESUME_DELAY_SEC = float(getattr(config, "TRACK_END_RESUME_DELAY_SEC", 1.0)) # после парковки + +def _publish_with_boxes( + cam_id: int, + img_bgr: np.ndarray, + boxes_xyxy: List[Tuple[int, int, int, int]], + labels: List[str], +) -> None: + if config.PUBLISH_ONLY_IF_CLIENTS and not state.video_clients: + return + down, sx, sy = maybe_downscale(img_bgr, config.PREVIEW_TARGET_W) + if boxes_xyxy: + for (x1, y1, x2, y2), lab in zip(boxes_xyxy, labels): + if sx != 1.0 or sy != 1.0: + x1 = int(x1 * sx); y1 = int(y1 * sy) + x2 = int(x2 * sx); y2 = int(y2 * sy) + cv2.rectangle(down, (x1, y1), (x2, y2), (0, 255, 0), 2, lineType=cv2.LINE_4) + cv2.putText( + down, lab, (x1, max(0, y1 - 6)), + cv2.FONT_HERSHEY_SIMPLEX, 0.6, (0, 255, 0), 1, cv2.LINE_4, + ) + publish_preview(cam_id, down) + + +def choose_auto_action(cam_idx: int, now: float) -> str: + st = state.ptz_states[cam_idx] + dt_cam = now - st["last_seen"] + if dt_cam <= config.SHORT_LOSS_SEC: + return "FOLLOW_SHORT" + if dt_cam <= config.LONG_LOSS_SEC: + return "SEARCH" + ema_g = state.detect_stats_global["ema_interval"] + if ema_g is None: + return "SEARCH" + dyn_thr = max(config.ABS_MIN_FALLBACK, 3.0 * float(ema_g)) + dyn_thr = min(dyn_thr, config.ABS_MAX_FALLBACK) + if config.FALLBACK_PRESET_CAP and config.FALLBACK_PRESET_CAP > 0: + dyn_thr = min(dyn_thr, config.FALLBACK_PRESET_CAP) + if dt_cam > dyn_thr: + return "FALLBACK" + return "SEARCH" if ema_g < 20.0 else "HOLD" + + +def _maybe_return_and_resume(cam_idx: int, reason: str, min_interval_sec: float) -> bool: + st = state.ptz_states[cam_idx] + now = time.time() + last = float(st.get("last_return_at", 0.0) or 0.0) + if (now - last) < float(min_interval_sec): + return False + + st["last_return_at"] = now + return_to_preset(cam_idx, reason) + ensure_patrol_resume(cam_idx, delay=1.0) + return True + + +def _enter_track(cam_idx: int) -> None: + st = state.ptz_states[cam_idx] + if not st.get("tracking_active"): + # запоминаем, был ли патруль активен — чтобы потом восстановить + st["_patrol_was_active_before_track"] = bool(st.get("patrol_active", False)) + st["tracking_active"] = True + st["mode"] = "TRACK" + # патрульные команды должны замолчать + st["patrol_active"] = False + st["sync_wait"] = False + + +def _end_track_and_park(cam_idx: int, *, reason: str) -> None: + st = state.ptz_states[cam_idx] + now = time.time() + + # если уже едем на пресет — не дублируем + if now < float(st.get("goto_in_progress_until", 0.0) or 0.0): + return + + last = float(st.get("last_track_end_return_at", 0.0) or 0.0) + if (now - last) < TRACK_END_RETURN_COOLDOWN_SEC: + return + st["last_track_end_return_at"] = now + + # СРАЗУ ставим "за нято goto", чтобы второй источник не успел добавить ещё один goto + settle = float(getattr(config, "GOTO_SETTLE_SEC", 2.5)) + st["goto_in_progress_until"] = now + settle + + st["tracking_active"] = False + st["mode"] = "IDLE" + notify_detected(cam_idx, False, force=True) + stop_ptz(cam_idx) + + return_to_preset(cam_idx, reason=reason) + ensure_patrol_resume(cam_idx, delay=max(0.8, TRACK_END_RESUME_DELAY_SEC)) + +def postprocess_control(cam_idx: int, frame_bgr: np.ndarray, res: Any, is_ptz: bool) -> None: + boxes_t = getattr(res, "boxes", None) + h, w = frame_bgr.shape[:2] + w_inv, h_inv = (1.0 / w), (1.0 / h) + now = time.time() + + st = state.ptz_states.setdefault(cam_idx, {}) + st.setdefault("mode", "auto" if config.AUTO_STRATEGY else "manual") + st.setdefault("last_seen", 0.0) + st.setdefault("loss_since", None) + st.setdefault("last_bbox", None) + + st.setdefault("prev_t", None) + st.setdefault("prev_cx", 0.5) + st.setdefault("prev_cy", 0.5) + st.setdefault("vx", 0.0) + st.setdefault("vy", 0.0) + st.setdefault("ix", 0.0) + st.setdefault("iy", 0.0) + + st.setdefault("w_frac_ema", None) + st.setdefault("prev_w_frac", None) + st.setdefault("w_growth", 0.0) + + st.setdefault("zoom_state", 0) + st.setdefault("zoom_prev_cmd", 0.0) + st.setdefault("zoom_int", 0.0) + st.setdefault("zoom_last_change", 0.0) + st.setdefault("zoom_ramp_start", 0.0) + st.setdefault("zoom_lock", False) + st.setdefault("lock_candidate_since", None) + st.setdefault("last_zoom_seen", 0.0) + + st.setdefault("last_dx", 0.0) + st.setdefault("last_dy", 0.0) + st.setdefault("stable_frames", 0) + st.setdefault("fallback_done", False) + st.setdefault("patrol_active", False) + st.setdefault("tracking_active", False) + st.setdefault("last_return_at", 0.0) + st.setdefault("last_track_end_return_at", 0.0) + + st.setdefault("attack_origin_pan", None) + st.setdefault("prev_pan_deg", None) + st.setdefault("prev_pan_t", None) + st.setdefault("last_heading_log", 0.0) + + _cfg = config.CAMERA_CONFIG.get(int(cam_idx), {}) if hasattr(config, "CAMERA_CONFIG") else {} + _PAN_SIGN: float = float(_cfg.get("pan_sign", 1) or 1) + _TILT_SIGN: float = float(_cfg.get("tilt_sign", -1) or -1) + + MAX_ANN_BOXES = 12 + det_xyxy: List[Tuple[int, int, int, int]] = [] + det_labels: List[str] = [] + best_target = None + + if boxes_t is not None and len(boxes_t) > 0: + confs = boxes_t.conf + clss = boxes_t.cls + mask = (confs >= config.DETECT_CONF) & torch.isin(clss, _targets_tensor) + if mask.any(): + xyxy_all = boxes_t.xyxy[mask].detach().cpu().numpy().astype(int) + conf_all = confs[mask].detach().cpu().numpy() + cls_all = clss[mask].detach().cpu().numpy().astype(int) + + x1 = xyxy_all[:, 0]; y1 = xyxy_all[:, 1] + x2 = xyxy_all[:, 2]; y2 = xyxy_all[:, 3] + cy = (y1 + y2) * 0.5 * h_inv + score_all = conf_all + 0.05 * ((x2 - x1) * w_inv) + ((cy < 0.45).astype(np.float32) * 0.12) + order = score_all.argsort()[::-1] + + topk = order[:MAX_ANN_BOXES] + for i in topk: + lab = f"{config.CLASS_NAMES.get(int(cls_all[i]), 'obj')} {float(conf_all[i]):.2f}" + det_xyxy.append((int(xyxy_all[i, 0]), int(xyxy_all[i, 1]), int(xyxy_all[i, 2]), int(xyxy_all[i, 3]))) + det_labels.append(lab) + + ibest = int(order[0]) + best_target = (xyxy_all[ibest].tolist(), float(conf_all[ibest]), int(cls_all[ibest])) + + # ========================= + # ===== есть детекция ====== + # ========================= + if best_target is not None: + (x1, y1, x2, y2), best_conf, _ = best_target + st["loss_since"] = None + st["last_bbox"] = (int(x1), int(y1), int(x2), int(y2)) + st["last_zoom_seen"] = now + st["last_seen"] = now + + _publish_with_boxes(cam_idx, frame_bgr, det_xyxy, det_labels) + + # --------- Панорама ---------- + if not is_ptz: + notify_detected(cam_idx, True) + + cx = (x1 + x2) * 0.5; cy = (y1 + y2) * 0.5 + cxn = cx * w_inv; cyn = cy * h_inv + if st["prev_t"] is not None: + dt = max(now - st["prev_t"], 1e-3) + st["vx"] = 0.85 * st["vx"] + 0.15 * ((cxn - st["prev_cx"]) / dt) + st["vy"] = 0.85 * st["vy"] + 0.15 * ((cyn - st["prev_cy"]) / dt) + st["prev_t"], st["prev_cx"], st["prev_cy"] = now, cxn, cyn + + try: + from . import geo_autocal + geo_autocal.feed_pano_sample(cam_idx, cxn, now) + except Exception: + pass + + base_az = float(config.get_cam_base_azimuth_deg(cam_idx)) + hfov = float(config.get_cam_hfov_deg(cam_idx)) + bearing = wrap_deg(base_az + (cxn - 0.5) * hfov) + + if st.get("attack_origin_pan") is None: + st["attack_origin_pan"] = bearing + st["prev_pan_deg"] = bearing + st["prev_pan_t"] = now + st["last_heading_log"] = 0.0 + + origin = float(st["attack_origin_pan"]) + cur_compass = deg_to_compass(float(bearing)) + org_compass = deg_to_compass(origin) + delta = wrap_deg(float(bearing) - origin) + cw_ccw = "CW" if delta >= 0.0 else "CCW" + + pan_rate = None + prev_t = st.get("prev_pan_t") + prev_p = st.get("prev_pan_deg") + if prev_t is not None and prev_p is not None: + dtp = max(1e-3, now - float(prev_t)) + pan_rate = wrap_deg(float(bearing) - float(prev_p)) / dtp + st["prev_pan_deg"] = float(bearing) + st["prev_pan_t"] = now + + if (now - float(st.get("last_heading_log", 0.0))) >= 0.7: + if pan_rate is None: + logger.info( + "[ATTACK/PANO] cam %d from %s (%.1f°) -> %s (%.1f°), Δ=%.1f° %s", + cam_idx, org_compass, origin, cur_compass, float(bearing), + float(delta), cw_ccw + ) + else: + logger.info( + "[ATTACK/PANO] cam %d from %s (%.1f°) -> %s (%.1f°), Δ=%.1f° %s, rate=%.1f°/s", + cam_idx, org_compass, origin, cur_compass, float(bearing), + float(delta), cw_ccw, float(pan_rate) + ) + st["last_heading_log"] = now + + ptz_id = config.PAN_TO_PTZ.get(cam_idx) + if ptz_id is not None: + pst = state.ptz_states.setdefault(ptz_id, {}) + pst["last_seen"] = now + pst["loss_since"] = None + pst["last_return_at"] = max(float(pst.get("last_return_at", 0.0) or 0.0), now) + + alarms.ingest_detection(cam_idx, True) + return + + # --------- PTZ: включаем TRACK сразу ---------- + _enter_track(cam_idx) + notify_detected(cam_idx, True) + + cx = (x1 + x2) * 0.5; cy = (y1 + y2) * 0.5 + cxn = cx * w_inv; cyn = cy * h_inv + w_frac = (x2 - x1) * w_inv + + if st["prev_t"] is not None: + dt = max(now - st["prev_t"], 1e-3) + vx_i = (cxn - st["prev_cx"]) / dt + vy_i = (cyn - st["prev_cy"]) / dt + vel_smooth = clipf(float(getattr(config, "VEL_SMOOTH", 0.85)), 0.0, 0.99) + st["vx"] = vel_smooth * st["vx"] + (1.0 - vel_smooth) * vx_i + st["vy"] = vel_smooth * st["vy"] + (1.0 - vel_smooth) * vy_i + st["prev_t"], st["prev_cx"], st["prev_cy"] = now, cxn, cyn + + wf = st["w_frac_ema"] + alpha = getattr(config, "W_EMA_ALPHA", 0.30) + st["w_frac_ema"] = w_frac if wf is None else (1.0 - alpha) * wf + alpha * w_frac + w_smooth = st["w_frac_ema"] + if st["prev_w_frac"] is not None: + dtw = max(now - (st["prev_t"] or now), 1e-3) + st["w_growth"] = (w_smooth - st["prev_w_frac"]) / dtw + st["prev_w_frac"] = w_smooth + + speed = abs(st["vx"]) + abs(st["vy"]) + far_factor = clipf(1.0 - max(w_smooth, (y2 - y1) * h_inv), 0.0, 1.0) + base_lead = float(getattr(config, "BASE_LEAD", 0.12)) + lead_gain = float(getattr(config, "LEAD_GAIN", 0.35)) + lead_max = float(getattr(config, "LEAD_MAX", 0.30)) + lead_hi = max(base_lead, lead_max) + lead_time = clipf(base_lead + lead_gain * speed * far_factor, base_lead, lead_hi) + cxn_pred = clipf(cxn + st["vx"] * lead_time, 0.0, 1.0) + cyn_pred = clipf(cyn + st["vy"] * lead_time, 0.0, 1.0) + dx = cxn_pred - 0.5 + dy = cyn_pred - 0.5 + + sm_alpha = getattr(config, "AIM_SMOOTH", 0.55) + sm_dx = st["last_dx"] * (1 - sm_alpha) + dx * sm_alpha + sm_dy = st["last_dy"] * (1 - sm_alpha) + dy * sm_alpha + + if abs(sm_dx) > getattr(config, "DEAD_ZONE", 0.018): + st["ix"] = clipf(st["ix"] + sm_dx * 0.02, -0.20, 0.20) + else: + st["ix"] *= 0.9 + if abs(sm_dy) > getattr(config, "DEAD_ZONE", 0.018): + st["iy"] = clipf(st["iy"] + sm_dy * 0.02, -0.20, 0.20) + else: + st["iy"] *= 0.9 + + ddx = sm_dx - st["last_dx"] + ddy = sm_dy - st["last_dy"] + st["last_dx"], st["last_dy"] = sm_dx, sm_dy + + conf_norm = clipf((best_conf - config.DETECT_CONF) / max(1e-3, 0.40 - config.DETECT_CONF), 0.0, 1.0) + aim_gain = 0.6 + 0.4 * conf_norm + + pan_speed = clipf( + (getattr(config, "AIM_P", 1.85) * sm_dx + getattr(config, "AIM_I", 0.07) * st["ix"] + getattr(config, "AIM_D", 0.15) * ddx) * aim_gain, + -1.0, 1.0 + ) + tilt_speed = clipf( + (getattr(config, "AIM_P", 1.85) * sm_dy + getattr(config, "AIM_I", 0.07) * st["iy"] + getattr(config, "AIM_D", 0.15) * ddy) * aim_gain, + -1.0, 1.0 + ) + + center_err = max(abs(dx), abs(dy)) + left = x1 * w_inv; right = 1.0 - x2 * w_inv + top = y1 * h_inv; bottom = 1.0 - y2 * h_inv + min_margin = min(left, right, top, bottom) + + # === zoom logic === + state_zoom = st.get("zoom_state", 0) + can_switch = (now - float(st.get("zoom_last_change", 0.0) or 0.0)) >= float(getattr(config, "Z_MIN_SWITCH_INTERVAL", 0.50)) + + gate_dyn = max(0.05, _Z_CENTER_GATE_IN + _Z_CENTER_GATE_SCALE * (1.0 - clipf(w_smooth, 0.0, 1.0))) + center_good = center_err < gate_dyn + + vel = abs(st["vx"]) + abs(st["vy"]) + z_target = (1.0 - clipf(vel / max(1e-6, float(getattr(config, "Z_VEL_REF", 0.45))), 0.0, 1.0)) * float(getattr(config, "Z_TARGET_SLOW", 0.16)) \ + + clipf(vel / max(1e-6, float(getattr(config, "Z_VEL_REF", 0.45))), 0.0, 1.0) * float(getattr(config, "Z_TARGET_FAST", 0.12)) + z_band_min = z_target - float(getattr(config, "Z_DELTA_IN", 0.008)) + z_band_max = z_target + float(getattr(config, "Z_DELTA_OUT", 0.060)) + + out_edge_crit = min_margin < float(getattr(config, "Z_OUT_EDGE", 0.07)) + near_edge = (min_margin < 0.05) + + if st.get("zoom_lock", False): + if ( + out_edge_crit + or (vel > float(getattr(config, "Z_UNLOCK_VEL_THR", 0.060))) + or (abs(float(st.get("w_growth", 0.0) or 0.0)) > float(getattr(config, "Z_UNLOCK_GROWTH_THR", 0.015))) + or (center_err > float(getattr(config, "Z_UNLOCK_CENTER_THR", 0.100))) + or (min_margin < _Z_UNLOCK_EDGE) + ): + st["zoom_lock"] = False + st["lock_candidate_since"] = None + else: + locked_candidate = ( + center_good + and (vel < float(getattr(config, "Z_UNLOCK_VEL_THR", 0.060))) + and (abs(float(st.get("w_growth", 0.0) or 0.0)) < float(getattr(config, "Z_FREEZE_GROWTH_THR", 0.004))) + and (min_margin >= float(getattr(config, "Z_SAFETY_EDGE", 0.05))) + ) + if locked_candidate: + if st.get("lock_candidate_since") is None: + st["lock_candidate_since"] = now + elif (now - float(st["lock_candidate_since"])) >= float(getattr(config, "Z_LOCK_ARM_SECS", 0.8)): + st["zoom_lock"] = True + else: + st["lock_candidate_since"] = None + + want_in = (w_smooth < (z_band_min - _Z_SIGN_HYST)) and center_good and (not near_edge) and (not st.get("zoom_lock", False)) + want_out = (w_smooth > (z_band_max + _Z_SIGN_HYST)) or out_edge_crit + + next_state = state_zoom + if state_zoom >= 0 and want_in and can_switch: + next_state = +1 + elif state_zoom <= 0 and want_out and can_switch: + next_state = -1 + elif not want_in and not want_out: + next_state = 0 + + if next_state != state_zoom: + st["zoom_state"] = next_state + st["zoom_last_change"] = now + + err_w = z_target - w_smooth + if abs(err_w) < float(getattr(config, "Z_DEADBAND", 0.010)): + err_w = 0.0 + st["zoom_int"] = clipf(float(st.get("zoom_int", 0.0) or 0.0) + err_w * 0.04, -_Z_INT_CLAMP, _Z_INT_CLAMP) + zoom_pi = _Z_P * err_w + _Z_I * st["zoom_int"] + zoom_ff = _Z_FF_GAIN * (-(float(st.get("w_growth", 0.0) or 0.0))) + zoom_cmd = clipf(zoom_pi + zoom_ff, _Z_SPEED_LIMIT_OUT, _Z_SPEED_LIMIT_IN) + + if st.get("zoom_lock", False): + zoom_cmd = min(zoom_cmd, -0.20) if out_edge_crit else 0.0 + st["zoom_int"] *= 0.90 + else: + if st["zoom_state"] > 0: + zoom_cmd = max(zoom_cmd, 0.0) + elif st["zoom_state"] < 0: + zoom_cmd = min(zoom_cmd, 0.0) + else: + zoom_cmd = 0.0 + + if abs(zoom_cmd) < _Z_MIN_CMD: + zoom_cmd = 0.0 + + cmd_mag_est = max(abs(pan_speed), abs(tilt_speed)) + if (cmd_mag_est > float(getattr(config, "Z_VELOCITY_GATE", 0.45))) and (zoom_cmd > 0.0): + zoom_cmd *= 0.5 + if near_edge and zoom_cmd > 0.0: + zoom_cmd = 0.0 + if min_margin < float(getattr(config, "Z_SAFETY_EDGE", 0.05)): + zoom_cmd = min(zoom_cmd, -0.25) + + prev_z = float(st.get("zoom_prev_cmd", 0.0) or 0.0) + if zoom_cmd > 0.0: + if prev_z <= 0.0: + st["zoom_ramp_start"] = now + frac = clipf((now - float(st.get("zoom_ramp_start", now) or now)) / max(1e-3, _Z_IN_RAMP), 0.0, 1.0) + zoom_cmd *= (0.25 + 0.75 * frac) + else: + st["zoom_ramp_start"] = 0.0 + st["zoom_prev_cmd"] = zoom_cmd + + if zoom_cmd > 0.0 and center_good and not near_edge: + call_autofocus(cam_idx) + + dx_abs, dy_abs = abs(dx), abs(dy) + if (dx_abs < 0.040) and (dy_abs < 0.040): + st["stable_frames"] = int(st.get("stable_frames", 0) or 0) + 1 + else: + st["stable_frames"] = 0 + + st["fallback_done"] = False + + adj_pan, adj_tilt = pan_speed, tilt_speed + if zoom_cmd > 0.0: + adj_pan = clipf(adj_pan * _Z_PAN_BOOST_IN, -1.0, 1.0) + adj_tilt = clipf(adj_tilt * _Z_PAN_BOOST_IN, -1.0, 1.0) + + move_ptz_bounded(cam_idx, _PAN_SIGN * adj_pan, _TILT_SIGN * adj_tilt, zoom_cmd) + alarms.ingest_detection(cam_idx, True) + return + + # ========================= + # ===== детекций нет ====== + # ========================= + alarms.ingest_detection(cam_idx, False) + notify_detected(cam_idx, False) + + # если мы были в TRACK и детекция пропала — заканчиваем TRACK и паркуем камеру в пресет + if is_ptz and st.get("tracking_active"): + # считаем, что "потеряли" если нет детекции > TRACK_END_LOSS_SEC + if (now - float(st.get("last_seen", 0.0) or 0.0)) >= TRACK_END_LOSS_SEC: + _end_track_and_park(cam_idx, reason="track_end_park") + _publish_with_boxes(cam_idx, frame_bgr, [], []) + return + + # ниже — твоя старая логика “потерь/возврата” (оставляем) + st["zoom_lock"] = False + st["lock_candidate_since"] = None + + if st.get("loss_since") is None: + st["loss_since"] = now + st["attack_origin_pan"] = None + + recently_tracked = (st.get("last_seen") is not None) and ((now - float(st["last_seen"])) < 10.0) + + near_edge_flag = False + lb = st.get("last_bbox") + if lb is not None: + x1, y1, x2, y2 = lb + left = x1 * w_inv; right = 1.0 - x2 * w_inv + top = y1 * h_inv; bottom = 1.0 - y2 * h_inv + if ( + (left < config.EDGE_LOSS_MARGIN) + or (right < config.EDGE_LOSS_MARGIN) + or (top < config.VERT_LOSS_MARGIN) + or (bottom < config.VERT_LOSS_MARGIN) + ): + near_edge_flag = True + + if is_ptz and ((now - float(st.get("last_zoom_seen", 0.0) or 0.0)) < _Z_HOLD_AFTER_LOSS): + move_ptz_bounded( + cam_idx, + _PAN_SIGN * (float(st.get("last_dx", 0.0) or 0.0) * 0.6), + _TILT_SIGN * (float(st.get("last_dy", 0.0) or 0.0) * 0.6), + float(getattr(config, "RECOVER_ZOOM_OUT", -0.35)) * 0.25, + ) + _publish_with_boxes(cam_idx, frame_bgr, [], []) + return + + edge_grace = float(getattr(config, "EDGE_LOSS_GRACE_SEC", 1.8)) + timed_out = (st.get("loss_since") is not None) and ((now - float(st["loss_since"])) >= float(config.LOSS_TO_PRESET_SEC)) + near_edge_and_lost = ( + near_edge_flag + and (st.get("loss_since") is not None) + and ((now - float(st["loss_since"])) >= edge_grace) + ) + + if is_ptz and recently_tracked and (near_edge_and_lost or timed_out): + _maybe_return_and_resume( + cam_idx, + "loss_or_timeout", + float(getattr(config, "LOSS_RETURN_COOLDOWN_SEC", 8.0)), + ) + _publish_with_boxes(cam_idx, frame_bgr, [], []) + return + + if config.AUTO_STRATEGY and is_ptz: + action = choose_auto_action(cam_idx, now) + + # если патруль активен и не TRACK — не мешаем патрулю + if st.get("patrol_active", False) and st.get("mode") in ("IDLE", "SEARCH"): + st["stable_frames"] = 0 + _publish_with_boxes(cam_idx, frame_bgr, [], []) + return + + if action == "FOLLOW_SHORT": + move_ptz_bounded( + cam_idx, + _PAN_SIGN * float(st.get("last_dx", 0.0) or 0.0), + _TILT_SIGN * float(st.get("last_dy", 0.0) or 0.0), + float(getattr(config, "RECOVER_ZOOM_OUT", -0.35)) * 0.6, + ) + + elif action in ("SEARCH", "HOLD"): + if action == "HOLD" and (now - float(st.get("last_seen", now) or now)) < 8.0: + st["last_dx"] = float(st.get("last_dx", 0.0) or 0.0) * 0.85 + st["last_dy"] = float(st.get("last_dy", 0.0) or 0.0) * 0.85 + move_ptz_bounded( + cam_idx, + _PAN_SIGN * float(st.get("last_dx", 0.0) or 0.0), + _TILT_SIGN * float(st.get("last_dy", 0.0) or 0.0), + float(getattr(config, "RECOVER_ZOOM_OUT", -0.35)) * 0.4, + ) + else: + stop_ptz(cam_idx) + + elif action == "FALLBACK": + if not st.get("patrol_active", False): + _maybe_return_and_resume( + cam_idx, + "fallback", + float(getattr(config, "FALLBACK_RETURN_COOLDOWN_SEC", 12.0)), + ) + + st["stable_frames"] = 0 + _publish_with_boxes(cam_idx, frame_bgr, [], []) diff --git a/ptz_tracker_modular/postprocess.py b/ptz_tracker_modular/postprocess.py new file mode 100644 index 0000000..596bfc9 --- /dev/null +++ b/ptz_tracker_modular/postprocess.py @@ -0,0 +1,668 @@ +from __future__ import annotations +import time +import math +import logging +from typing import Any, List, Tuple + +import numpy as np +import torch +import cv2 + +from . import config, state +from .utils import clipf, wrap_deg, deg_to_compass +from .preview import maybe_downscale, publish_preview +from . import alarms +from .notify import notify_detected +from .ptz_io import move_ptz_bounded, call_autofocus, stop_ptz +from .patrol import return_to_preset, ensure_patrol_resume + +logger = logging.getLogger("PTZTracker") + +_device = "cuda" if torch.cuda.is_available() else "cpu" +_targets_tensor = torch.tensor(config.TARGET_CLASSES, device=_device) + +# === Zoom / устойчивость === +_Z_HOLD_AFTER_LOSS = getattr(config, "ZOOM_LOSS_HOLD_SEC", 0.70) +_Z_SIGN_HYST = getattr(config, "ZOOM_SIGN_HYST", 0.020) +_Z_MIN_CMD = getattr(config, "Z_MIN_CMD", 0.05) +_Z_IN_RAMP = getattr(config, "Z_RAMP_TIME", 0.45) +_Z_FF_GAIN = getattr(config, "Z_FF_GAIN", 0.55) +_Z_P = getattr(config, "Z_P", 2.4) +_Z_I = getattr(config, "Z_I", 0.16) +_Z_INT_CLAMP = getattr(config, "Z_INT_CLAMP", 0.15) +_Z_SPEED_LIMIT_OUT = getattr(config, "Z_SPEED_LIMIT_OUT", -0.65) +_Z_SPEED_LIMIT_IN = getattr(config, "Z_SPEED_LIMIT_IN", 0.45) +_Z_CENTER_GATE_IN = getattr(config, "Z_CENTER_GATE_BASE", 0.06) +_Z_CENTER_GATE_SCALE = getattr(config, "Z_CENTER_GATE_SCALE", 0.20) +_Z_UNLOCK_EDGE = getattr(config, "Z_UNLOCK_EDGE", 0.050) +_Z_PAN_BOOST_IN = getattr(config, "Z_PAN_BOOST_WHEN_IN", getattr(config, "Z_PAN_BOOST", 1.35)) + +# === TRACK lifecycle === +TRACK_END_LOSS_SEC = float(getattr(config, "TRACK_END_LOSS_SEC", 0.90)) # сколько ждать без детекции, чтобы завершить TRACK +TRACK_END_RETURN_COOLDOWN_SEC = float(getattr(config, "TRACK_END_RETURN_COOLDOWN_SEC", 2.5)) # анти-спам goto +TRACK_END_RESUME_DELAY_SEC = float(getattr(config, "TRACK_END_RESUME_DELAY_SEC", 1.0)) # после парковки + +def _publish_with_boxes( + cam_id: int, + img_bgr: np.ndarray, + boxes_xyxy: List[Tuple[int, int, int, int]], + labels: List[str], +) -> None: + if config.PUBLISH_ONLY_IF_CLIENTS and not state.video_clients: + return + down, sx, sy = maybe_downscale(img_bgr, config.PREVIEW_TARGET_W) + if boxes_xyxy: + for (x1, y1, x2, y2), lab in zip(boxes_xyxy, labels): + if sx != 1.0 or sy != 1.0: + x1 = int(x1 * sx); y1 = int(y1 * sy) + x2 = int(x2 * sx); y2 = int(y2 * sy) + cv2.rectangle(down, (x1, y1), (x2, y2), (0, 255, 0), 2, lineType=cv2.LINE_4) + cv2.putText( + down, lab, (x1, max(0, y1 - 6)), + cv2.FONT_HERSHEY_SIMPLEX, 0.6, (0, 255, 0), 1, cv2.LINE_4, + ) + publish_preview(cam_id, down) + + +def choose_auto_action(cam_idx: int, now: float) -> str: + st = state.ptz_states[cam_idx] + dt_cam = now - st["last_seen"] + if dt_cam <= config.SHORT_LOSS_SEC: + return "FOLLOW_SHORT" + if dt_cam <= config.LONG_LOSS_SEC: + return "SEARCH" + ema_g = state.detect_stats_global["ema_interval"] + if ema_g is None: + return "SEARCH" + dyn_thr = max(config.ABS_MIN_FALLBACK, 3.0 * float(ema_g)) + dyn_thr = min(dyn_thr, config.ABS_MAX_FALLBACK) + if config.FALLBACK_PRESET_CAP and config.FALLBACK_PRESET_CAP > 0: + dyn_thr = min(dyn_thr, config.FALLBACK_PRESET_CAP) + if dt_cam > dyn_thr: + return "FALLBACK" + return "SEARCH" if ema_g < 20.0 else "HOLD" + + +def _maybe_return_and_resume(cam_idx: int, reason: str, min_interval_sec: float) -> bool: + st = state.ptz_states[cam_idx] + now = time.time() + last = float(st.get("last_return_at", 0.0) or 0.0) + if (now - last) < float(min_interval_sec): + return False + + st["last_return_at"] = now + return_to_preset(cam_idx, reason) + ensure_patrol_resume(cam_idx, delay=1.0) + return True + + +def _enter_track(cam_idx: int) -> None: + st = state.ptz_states[cam_idx] + if not st.get("tracking_active"): + # запоминаем, был ли патруль активен — чтобы потом восстановить + st["_patrol_was_active_before_track"] = bool(st.get("patrol_active", False)) + st["tracking_active"] = True + st["mode"] = "TRACK" + # патрульные команды должны замолчать + st["patrol_active"] = False + st["sync_wait"] = False + + +def _end_track_and_park(cam_idx: int, *, reason: str) -> None: + st = state.ptz_states[cam_idx] + now = time.time() + + # если уже едем на пресет — не дублируем + if now < float(st.get("goto_in_progress_until", 0.0) or 0.0): + return + + last = float(st.get("last_track_end_return_at", 0.0) or 0.0) + if (now - last) < TRACK_END_RETURN_COOLDOWN_SEC: + return + st["last_track_end_return_at"] = now + + # СРАЗУ ставим "за нято goto", чтобы второй источник не успел добавить ещё один goto + settle = float(getattr(config, "GOTO_SETTLE_SEC", 2.5)) + st["goto_in_progress_until"] = now + settle + + st["tracking_active"] = False + st["mode"] = "IDLE" + notify_detected(cam_idx, False, force=True) + stop_ptz(cam_idx) + + return_to_preset(cam_idx, reason=reason) + ensure_patrol_resume(cam_idx, delay=max(0.8, TRACK_END_RESUME_DELAY_SEC)) + +def postprocess_control(cam_idx: int, frame_bgr: np.ndarray, res: Any, is_ptz: bool) -> None: + boxes_t = getattr(res, "boxes", None) + h, w = frame_bgr.shape[:2] + w_inv, h_inv = (1.0 / w), (1.0 / h) + now = time.time() + + st = state.ptz_states.setdefault(cam_idx, {}) + st.setdefault("mode", "auto" if config.AUTO_STRATEGY else "manual") + st.setdefault("last_seen", 0.0) + st.setdefault("loss_since", None) + st.setdefault("last_bbox", None) + + st.setdefault("prev_t", None) + st.setdefault("prev_cx", 0.5) + st.setdefault("prev_cy", 0.5) + st.setdefault("vx", 0.0) + st.setdefault("vy", 0.0) + st.setdefault("ix", 0.0) + st.setdefault("iy", 0.0) + + st.setdefault("w_frac_ema", None) + st.setdefault("prev_w_frac", None) + st.setdefault("w_growth", 0.0) + + st.setdefault("zoom_state", 0) + st.setdefault("zoom_prev_cmd", 0.0) + st.setdefault("zoom_int", 0.0) + st.setdefault("zoom_last_change", 0.0) + st.setdefault("zoom_ramp_start", 0.0) + st.setdefault("zoom_lock", False) + st.setdefault("lock_candidate_since", None) + st.setdefault("last_zoom_seen", 0.0) + + st.setdefault("last_dx", 0.0) + st.setdefault("last_dy", 0.0) + st.setdefault("stable_frames", 0) + st.setdefault("fallback_done", False) + st.setdefault("patrol_active", False) + st.setdefault("tracking_active", False) + st.setdefault("last_return_at", 0.0) + st.setdefault("last_track_end_return_at", 0.0) + + st.setdefault("attack_origin_pan", None) + st.setdefault("prev_pan_deg", None) + st.setdefault("prev_pan_t", None) + st.setdefault("last_heading_log", 0.0) + + _cfg = config.CAMERA_CONFIG.get(int(cam_idx), {}) if hasattr(config, "CAMERA_CONFIG") else {} + _PAN_SIGN: float = float(_cfg.get("pan_sign", 1) or 1) + _TILT_SIGN: float = float(_cfg.get("tilt_sign", -1) or -1) + + MAX_ANN_BOXES = 12 + det_xyxy: List[Tuple[int, int, int, int]] = [] + det_labels: List[str] = [] + best_target = None + + if boxes_t is not None and len(boxes_t) > 0: + confs = boxes_t.conf + clss = boxes_t.cls + mask = (confs >= config.DETECT_CONF) & torch.isin(clss, _targets_tensor) + if mask.any(): + xyxy_all = boxes_t.xyxy[mask].detach().cpu().numpy().astype(int) + conf_all = confs[mask].detach().cpu().numpy() + cls_all = clss[mask].detach().cpu().numpy().astype(int) + + x1 = xyxy_all[:, 0]; y1 = xyxy_all[:, 1] + x2 = xyxy_all[:, 2]; y2 = xyxy_all[:, 3] + cy = (y1 + y2) * 0.5 * h_inv + score_all = conf_all + 0.05 * ((x2 - x1) * w_inv) + ((cy < 0.45).astype(np.float32) * 0.12) + order = score_all.argsort()[::-1] + + topk = order[:MAX_ANN_BOXES] + for i in topk: + lab = f"{config.CLASS_NAMES.get(int(cls_all[i]), 'obj')} {float(conf_all[i]):.2f}" + det_xyxy.append((int(xyxy_all[i, 0]), int(xyxy_all[i, 1]), int(xyxy_all[i, 2]), int(xyxy_all[i, 3]))) + det_labels.append(lab) + + ibest = int(order[0]) + best_target = (xyxy_all[ibest].tolist(), float(conf_all[ibest]), int(cls_all[ibest])) + + # ========================= + # ===== есть детекция ====== + # ========================= + if best_target is not None: + (x1, y1, x2, y2), best_conf, _ = best_target + st["loss_since"] = None + st["last_bbox"] = (int(x1), int(y1), int(x2), int(y2)) + st["last_zoom_seen"] = now + st["last_seen"] = now + + _publish_with_boxes(cam_idx, frame_bgr, det_xyxy, det_labels) + + # --------- Панорама ---------- + if not is_ptz: + notify_detected(cam_idx, True) + + cx = (x1 + x2) * 0.5; cy = (y1 + y2) * 0.5 + cxn = cx * w_inv; cyn = cy * h_inv + if st["prev_t"] is not None: + dt = max(now - st["prev_t"], 1e-3) + st["vx"] = 0.85 * st["vx"] + 0.15 * ((cxn - st["prev_cx"]) / dt) + st["vy"] = 0.85 * st["vy"] + 0.15 * ((cyn - st["prev_cy"]) / dt) + st["prev_t"], st["prev_cx"], st["prev_cy"] = now, cxn, cyn + + try: + from . import geo_autocal + geo_autocal.feed_pano_sample(cam_idx, cxn, now) + except Exception: + pass + + base_az = float(config.get_cam_base_azimuth_deg(cam_idx)) + hfov = float(config.get_cam_hfov_deg(cam_idx)) + bearing = wrap_deg(base_az + (cxn - 0.5) * hfov) + + if st.get("attack_origin_pan") is None: + st["attack_origin_pan"] = bearing + st["prev_pan_deg"] = bearing + st["prev_pan_t"] = now + st["last_heading_log"] = 0.0 + + origin = float(st["attack_origin_pan"]) + cur_compass = deg_to_compass(float(bearing)) + org_compass = deg_to_compass(origin) + delta = wrap_deg(float(bearing) - origin) + cw_ccw = "CW" if delta >= 0.0 else "CCW" + + pan_rate = None + prev_t = st.get("prev_pan_t") + prev_p = st.get("prev_pan_deg") + if prev_t is not None and prev_p is not None: + dtp = max(1e-3, now - float(prev_t)) + pan_rate = wrap_deg(float(bearing) - float(prev_p)) / dtp + st["prev_pan_deg"] = float(bearing) + st["prev_pan_t"] = now + + if (now - float(st.get("last_heading_log", 0.0))) >= 0.7: + if pan_rate is None: + logger.info( + "[ATTACK/PANO] cam %d from %s (%.1f°) -> %s (%.1f°), Δ=%.1f° %s", + cam_idx, org_compass, origin, cur_compass, float(bearing), + float(delta), cw_ccw + ) + else: + logger.info( + "[ATTACK/PANO] cam %d from %s (%.1f°) -> %s (%.1f°), Δ=%.1f° %s, rate=%.1f°/s", + cam_idx, org_compass, origin, cur_compass, float(bearing), + float(delta), cw_ccw, float(pan_rate) + ) + st["last_heading_log"] = now + + ptz_id = config.PAN_TO_PTZ.get(cam_idx) + if ptz_id is not None: + pst = state.ptz_states.setdefault(ptz_id, {}) + pst["last_seen"] = now + pst["loss_since"] = None + pst["last_return_at"] = max(float(pst.get("last_return_at", 0.0) or 0.0), now) + + alarms.ingest_detection(cam_idx, True) + return + + # --------- PTZ: включаем TRACK сразу ---------- + _enter_track(cam_idx) + notify_detected(cam_idx, True) + + cx = (x1 + x2) * 0.5; cy = (y1 + y2) * 0.5 + cxn = cx * w_inv; cyn = cy * h_inv + w_frac = (x2 - x1) * w_inv + + prev_t = st.get("prev_t") + if prev_t is not None: + dt = max(now - float(prev_t), 1e-3) + vx_i = (cxn - st["prev_cx"]) / dt + vy_i = (cyn - st["prev_cy"]) / dt + vel_smooth = clipf(float(getattr(config, "VEL_SMOOTH", 0.85)), 0.0, 0.99) + st["vx"] = vel_smooth * st["vx"] + (1.0 - vel_smooth) * vx_i + st["vy"] = vel_smooth * st["vy"] + (1.0 - vel_smooth) * vy_i + + wf = st["w_frac_ema"] + alpha = getattr(config, "W_EMA_ALPHA", 0.30) + st["w_frac_ema"] = w_frac if wf is None else (1.0 - alpha) * wf + alpha * w_frac + w_smooth = st["w_frac_ema"] + if st["prev_w_frac"] is not None and prev_t is not None: + dtw = max(now - float(prev_t), 1e-3) + st["w_growth"] = (w_smooth - st["prev_w_frac"]) / dtw + else: + st["w_growth"] = 0.0 + # optional clamp to avoid spikes from detector jitter + wg_clamp = float(getattr(config, "W_GROWTH_CLAMP", 2.0)) + st["w_growth"] = clipf(float(st.get("w_growth", 0.0) or 0.0), -wg_clamp, wg_clamp) + st["prev_w_frac"] = w_smooth + + # update prev_* only after derivative computations + st["prev_t"], st["prev_cx"], st["prev_cy"] = now, cxn, cyn + + speed = abs(st["vx"]) + abs(st["vy"]) + far_factor = clipf(1.0 - max(w_smooth, (y2 - y1) * h_inv), 0.0, 1.0) + base_lead = float(getattr(config, "BASE_LEAD", 0.12)) + lead_gain = float(getattr(config, "LEAD_GAIN", 0.35)) + lead_max = float(getattr(config, "LEAD_MAX", 0.30)) + lead_hi = max(base_lead, lead_max) + lead_time = clipf(base_lead + lead_gain * speed * far_factor, base_lead, lead_hi) + # If target is far from center, disable aggressive lead to avoid overshoot during large slews. + err_mag = max(abs(cxn - 0.5), abs(cyn - 0.5)) + lead_off = float(getattr(config, "LEAD_OFF_ERR", 0.18)) + if err_mag > lead_off: + lead_time = base_lead + pred_min = float(getattr(config, "PRED_CLAMP_MIN", 0.05)) + pred_max = float(getattr(config, "PRED_CLAMP_MAX", 0.95)) + cxn_pred = clipf(cxn + st["vx"] * lead_time, pred_min, pred_max) + cyn_pred = clipf(cyn + st["vy"] * lead_time, pred_min, pred_max) + dx = cxn_pred - 0.5 + dy = cyn_pred - 0.5 + + sm_alpha = getattr(config, "AIM_SMOOTH", 0.55) + sm_dx = st["last_dx"] * (1 - sm_alpha) + dx * sm_alpha + sm_dy = st["last_dy"] * (1 - sm_alpha) + dy * sm_alpha + + if abs(sm_dx) > getattr(config, "DEAD_ZONE", 0.018): + st["ix"] = clipf(st["ix"] + sm_dx * 0.02, -0.20, 0.20) + else: + st["ix"] *= 0.9 + if abs(sm_dy) > getattr(config, "DEAD_ZONE", 0.018): + st["iy"] = clipf(st["iy"] + sm_dy * 0.02, -0.20, 0.20) + else: + st["iy"] *= 0.9 + + ddx = sm_dx - st["last_dx"] + ddy = sm_dy - st["last_dy"] + st["last_dx"], st["last_dy"] = sm_dx, sm_dy + + conf_norm = clipf((best_conf - config.DETECT_CONF) / max(1e-3, 0.40 - config.DETECT_CONF), 0.0, 1.0) + aim_gain = 0.6 + 0.4 * conf_norm + + pan_speed = clipf( + (getattr(config, "AIM_P", 1.85) * sm_dx + getattr(config, "AIM_I", 0.07) * st["ix"] + getattr(config, "AIM_D", 0.15) * ddx) * aim_gain, + -1.0, 1.0 + ) + tilt_speed = clipf( + (getattr(config, "AIM_P", 1.85) * sm_dy + getattr(config, "AIM_I", 0.07) * st["iy"] + getattr(config, "AIM_D", 0.15) * ddy) * aim_gain, + -1.0, 1.0 + ) + + center_err = max(abs(dx), abs(dy)) + left = x1 * w_inv; right = 1.0 - x2 * w_inv + top = y1 * h_inv; bottom = 1.0 - y2 * h_inv + min_margin = min(left, right, top, bottom) + + # === zoom logic === + state_zoom = st.get("zoom_state", 0) + can_switch = (now - float(st.get("zoom_last_change", 0.0) or 0.0)) >= float(getattr(config, "Z_MIN_SWITCH_INTERVAL", 0.50)) + + gate_dyn = max(0.05, _Z_CENTER_GATE_IN + _Z_CENTER_GATE_SCALE * (1.0 - clipf(w_smooth, 0.0, 1.0))) + center_good = center_err < gate_dyn + + vel = abs(st["vx"]) + abs(st["vy"]) + z_target = (1.0 - clipf(vel / max(1e-6, float(getattr(config, "Z_VEL_REF", 0.45))), 0.0, 1.0)) * float(getattr(config, "Z_TARGET_SLOW", 0.16)) \ + + clipf(vel / max(1e-6, float(getattr(config, "Z_VEL_REF", 0.45))), 0.0, 1.0) * float(getattr(config, "Z_TARGET_FAST", 0.12)) + z_band_min = z_target - float(getattr(config, "Z_DELTA_IN", 0.008)) + z_band_max = z_target + float(getattr(config, "Z_DELTA_OUT", 0.060)) + + out_edge_crit = min_margin < float(getattr(config, "Z_OUT_EDGE", 0.07)) + near_edge = (min_margin < 0.05) + + if st.get("zoom_lock", False): + if ( + out_edge_crit + or (vel > float(getattr(config, "Z_UNLOCK_VEL_THR", 0.060))) + or (abs(float(st.get("w_growth", 0.0) or 0.0)) > float(getattr(config, "Z_UNLOCK_GROWTH_THR", 0.015))) + or (center_err > float(getattr(config, "Z_UNLOCK_CENTER_THR", 0.100))) + or (min_margin < _Z_UNLOCK_EDGE) + ): + st["zoom_lock"] = False + st["lock_candidate_since"] = None + else: + locked_candidate = ( + center_good + and (vel < float(getattr(config, "Z_UNLOCK_VEL_THR", 0.060))) + and (abs(float(st.get("w_growth", 0.0) or 0.0)) < float(getattr(config, "Z_FREEZE_GROWTH_THR", 0.004))) + and (min_margin >= float(getattr(config, "Z_SAFETY_EDGE", 0.05))) + ) + if locked_candidate: + if st.get("lock_candidate_since") is None: + st["lock_candidate_since"] = now + elif (now - float(st["lock_candidate_since"])) >= float(getattr(config, "Z_LOCK_ARM_SECS", 0.8)): + st["zoom_lock"] = True + else: + st["lock_candidate_since"] = None + + want_in = (w_smooth < (z_band_min - _Z_SIGN_HYST)) and center_good and (not near_edge) and (not st.get("zoom_lock", False)) + want_out = (w_smooth > (z_band_max + _Z_SIGN_HYST)) or out_edge_crit + + next_state = state_zoom + if state_zoom >= 0 and want_in and can_switch: + next_state = +1 + elif state_zoom <= 0 and want_out and can_switch: + next_state = -1 + elif not want_in and not want_out: + next_state = 0 + + if next_state != state_zoom: + st["zoom_state"] = next_state + st["zoom_last_change"] = now + + err_w = z_target - w_smooth + if abs(err_w) < float(getattr(config, "Z_DEADBAND", 0.010)): + err_w = 0.0 + st["zoom_int"] = clipf(float(st.get("zoom_int", 0.0) or 0.0) + err_w * 0.04, -_Z_INT_CLAMP, _Z_INT_CLAMP) + zoom_pi = _Z_P * err_w + _Z_I * st["zoom_int"] + zoom_ff = _Z_FF_GAIN * (-(float(st.get("w_growth", 0.0) or 0.0))) + zoom_cmd = clipf(zoom_pi + zoom_ff, _Z_SPEED_LIMIT_OUT, _Z_SPEED_LIMIT_IN) + + if st.get("zoom_lock", False): + zoom_cmd = min(zoom_cmd, -0.20) if out_edge_crit else 0.0 + st["zoom_int"] *= 0.90 + else: + if st["zoom_state"] > 0: + zoom_cmd = max(zoom_cmd, 0.0) + elif st["zoom_state"] < 0: + zoom_cmd = min(zoom_cmd, 0.0) + else: + zoom_cmd = 0.0 + + if abs(zoom_cmd) < _Z_MIN_CMD: + zoom_cmd = 0.0 + + cmd_mag_est = max(abs(pan_speed), abs(tilt_speed)) + if (cmd_mag_est > float(getattr(config, "Z_VELOCITY_GATE", 0.45))) and (zoom_cmd > 0.0): + zoom_cmd = 0.0 + err_gate = float(getattr(config, "Z_ERR_GATE", 0.18)) + if (max(abs(sm_dx), abs(sm_dy)) > err_gate) and (zoom_cmd > 0.0): + zoom_cmd = 0.0 + if near_edge and zoom_cmd > 0.0: + zoom_cmd = 0.0 + if min_margin < float(getattr(config, "Z_SAFETY_EDGE", 0.05)): + zoom_cmd = min(zoom_cmd, -0.25) + + prev_z = float(st.get("zoom_prev_cmd", 0.0) or 0.0) + if zoom_cmd > 0.0: + if prev_z <= 0.0: + st["zoom_ramp_start"] = now + frac = clipf((now - float(st.get("zoom_ramp_start", now) or now)) / max(1e-3, _Z_IN_RAMP), 0.0, 1.0) + zoom_cmd *= (0.25 + 0.75 * frac) + else: + st["zoom_ramp_start"] = 0.0 + st["zoom_prev_cmd"] = zoom_cmd + + if zoom_cmd > 0.0 and center_good and not near_edge: + call_autofocus(cam_idx) + + dx_abs, dy_abs = abs(dx), abs(dy) + if (dx_abs < 0.040) and (dy_abs < 0.040): + st["stable_frames"] = int(st.get("stable_frames", 0) or 0) + 1 + else: + st["stable_frames"] = 0 + + st["fallback_done"] = False + + adj_pan, adj_tilt = pan_speed, tilt_speed + if zoom_cmd > 0.0: + adj_pan = clipf(adj_pan * _Z_PAN_BOOST_IN, -1.0, 1.0) + adj_tilt = clipf(adj_tilt * _Z_PAN_BOOST_IN, -1.0, 1.0) + + move_ptz_bounded(cam_idx, _PAN_SIGN * adj_pan, _TILT_SIGN * adj_tilt, zoom_cmd) + alarms.ingest_detection(cam_idx, True) + return + + # ========================= + # ===== детекций нет ====== + # ========================= + alarms.ingest_detection(cam_idx, False) + notify_detected(cam_idx, False) + + # если мы были в TRACK и детекция пропала — заканчиваем TRACK и паркуем камеру в пресет + if is_ptz and st.get("tracking_active"): + # считаем, что "потеряли" если нет детекции > TRACK_END_LOSS_SEC + if (now - float(st.get("last_seen", 0.0) or 0.0)) >= TRACK_END_LOSS_SEC: + _end_track_and_park(cam_idx, reason="track_end_park") + _publish_with_boxes(cam_idx, frame_bgr, [], []) + return + + # ниже — твоя старая логика “потерь/возврата” (оставляем) + st["zoom_lock"] = False + st["lock_candidate_since"] = None + + if st.get("loss_since") is None: + st["loss_since"] = now + st["attack_origin_pan"] = None + + recently_tracked = (st.get("last_seen") is not None) and ((now - float(st["last_seen"])) < 10.0) + + near_edge_flag = False + lb = st.get("last_bbox") + if lb is not None: + x1, y1, x2, y2 = lb + left = x1 * w_inv; right = 1.0 - x2 * w_inv + top = y1 * h_inv; bottom = 1.0 - y2 * h_inv + if ( + (left < config.EDGE_LOSS_MARGIN) + or (right < config.EDGE_LOSS_MARGIN) + or (top < config.VERT_LOSS_MARGIN) + or (bottom < config.VERT_LOSS_MARGIN) + ): + near_edge_flag = True + + if is_ptz and ((now - float(st.get("last_zoom_seen", 0.0) or 0.0)) < _Z_HOLD_AFTER_LOSS): + move_ptz_bounded( + cam_idx, + _PAN_SIGN * (float(st.get("last_dx", 0.0) or 0.0) * 0.6), + _TILT_SIGN * (float(st.get("last_dy", 0.0) or 0.0) * 0.6), + float(getattr(config, "RECOVER_ZOOM_OUT", -0.35)) * 0.25, + ) + _publish_with_boxes(cam_idx, frame_bgr, [], []) + return + + edge_grace = float(getattr(config, "EDGE_LOSS_GRACE_SEC", 1.8)) + timed_out = (st.get("loss_since") is not None) and ((now - float(st["loss_since"])) >= float(config.LOSS_TO_PRESET_SEC)) + near_edge_and_lost = ( + near_edge_flag + and (st.get("loss_since") is not None) + and ((now - float(st["loss_since"])) >= edge_grace) + ) + + if is_ptz and recently_tracked and (near_edge_and_lost or timed_out): + _maybe_return_and_resume( + cam_idx, + "loss_or_timeout", + float(getattr(config, "LOSS_RETURN_COOLDOWN_SEC", 8.0)), + ) + _publish_with_boxes(cam_idx, frame_bgr, [], []) + return + + if config.AUTO_STRATEGY and is_ptz: + action = choose_auto_action(cam_idx, now) + + # если патруль активен и не TRACK — не мешаем патрулю + if st.get("patrol_active", False) and st.get("mode") in ("IDLE", "SEARCH"): + st["stable_frames"] = 0 + + # ---------- устойчивое поведение при кратких провалах детекции ---------- + last_seen = float(st.get("last_seen", 0.0) or 0.0) + dt_loss = now - last_seen + + def _predict_dxdy() -> Tuple[float, float]: + cx0 = float(st.get("prev_cx", 0.5) or 0.5) + cy0 = float(st.get("prev_cy", 0.5) or 0.5) + vx = float(st.get("vx", 0.0) or 0.0) + vy = float(st.get("vy", 0.0) or 0.0) + w0 = st.get("w_frac_ema") + w0 = float(w0) if w0 is not None else 0.08 # типичный "далёкий" объект + speed = float((vx * vx + vy * vy) ** 0.5) + far_factor = clipf(1.0 - w0, 0.0, 1.0) + + base_lead = float(getattr(config, "BASE_LEAD", 0.12)) + lead_gain = float(getattr(config, "LEAD_GAIN", 0.35)) + lead_max = float(getattr(config, "LEAD_MAX", 0.30)) + lead_hi = max(base_lead, lead_max) + + lead_time = clipf(base_lead + lead_gain * speed * far_factor, base_lead, lead_hi) + err_mag = max(abs(cx0 - 0.5), abs(cy0 - 0.5)) + lead_off = float(getattr(config, "LEAD_OFF_ERR", 0.18)) + if err_mag > lead_off: + lead_time = base_lead + pred_min = float(getattr(config, "PRED_CLAMP_MIN", 0.05)) + pred_max = float(getattr(config, "PRED_CLAMP_MAX", 0.95)) + cxp = clipf(cx0 + vx * lead_time, pred_min, pred_max) + cyp = clipf(cy0 + vy * lead_time, pred_min, pred_max) + dx = cxp - 0.5 + dy = cyp - 0.5 + dcl = float(getattr(config, "PRED_DX_CLAMP", 0.35)) + return clipf(dx, -dcl, dcl), clipf(dy, -dcl, dcl) + + pred_dx, pred_dy = _predict_dxdy() + + short_thr = float(getattr(config, "SHORT_LOSS_SEC", 0.25)) + long_thr = float(getattr(config, "LONG_LOSS_SEC", 1.2)) + + if dt_loss <= short_thr: + decay = 1.0 + zoom_loss = 0.0 # freeze zoom на коротких провалах + elif dt_loss <= long_thr: + tau = float(getattr(config, "LOSS_DECAY_TAU", 0.35)) + decay = float(math.exp(-(dt_loss - short_thr) / max(1e-3, tau))) + zoom_loss = float(getattr(config, "RECOVER_ZOOM_OUT", -0.35)) * 0.35 # мягко расширяем кадр + else: + decay = 0.0 + zoom_loss = float(getattr(config, "RECOVER_ZOOM_OUT", -0.35)) * 0.6 + + st["stable_frames"] = 0 + + # HOLD_PREDICT: пока цель могла "мигнуть" — ведём по прогнозу, не накапливая задержку + if dt_loss <= long_thr: + # обновим last_dx/last_dy прогнозом, чтобы другие ветки (если сработают) не тянули старьё + st["last_dx"], st["last_dy"] = float(pred_dx), float(pred_dy) + move_ptz_bounded( + cam_idx, + _PAN_SIGN * float(pred_dx) * float(decay), + _TILT_SIGN * float(pred_dy) * float(decay), + float(zoom_loss), + ) + _publish_with_boxes(cam_idx, frame_bgr, [], []) + return + + if action == "FOLLOW_SHORT": + move_ptz_bounded( + cam_idx, + _PAN_SIGN * float(st.get("last_dx", 0.0) or 0.0), + _TILT_SIGN * float(st.get("last_dy", 0.0) or 0.0), + float(getattr(config, "RECOVER_ZOOM_OUT", -0.35)) * 0.6, + ) + + elif action in ("SEARCH", "HOLD"): + if action == "HOLD" and (now - float(st.get("last_seen", now) or now)) < 8.0: + st["last_dx"] = float(st.get("last_dx", 0.0) or 0.0) * 0.85 + st["last_dy"] = float(st.get("last_dy", 0.0) or 0.0) * 0.85 + move_ptz_bounded( + cam_idx, + _PAN_SIGN * float(st.get("last_dx", 0.0) or 0.0), + _TILT_SIGN * float(st.get("last_dy", 0.0) or 0.0), + float(getattr(config, "RECOVER_ZOOM_OUT", -0.35)) * 0.4, + ) + else: + stop_ptz(cam_idx) + + elif action == "FALLBACK": + if not st.get("patrol_active", False): + _maybe_return_and_resume( + cam_idx, + "fallback", + float(getattr(config, "FALLBACK_RETURN_COOLDOWN_SEC", 12.0)), + ) + + st["stable_frames"] = 0 + _publish_with_boxes(cam_idx, frame_bgr, [], []) diff --git a/ptz_tracker_modular/postprocess04.02.py b/ptz_tracker_modular/postprocess04.02.py new file mode 100644 index 0000000..adcc57d --- /dev/null +++ b/ptz_tracker_modular/postprocess04.02.py @@ -0,0 +1,667 @@ +from __future__ import annotations +import time +import math +import logging +from typing import Any, List, Tuple + +import numpy as np +import torch +import cv2 + +from . import config, state +from .utils import clipf, wrap_deg, deg_to_compass +from .preview import maybe_downscale, publish_preview +from . import alarms +from .notify import notify_detected +from .ptz_io import move_ptz_bounded, call_autofocus, stop_ptz +from .patrol import return_to_preset, ensure_patrol_resume + +logger = logging.getLogger("PTZTracker") + +_device = "cuda" if torch.cuda.is_available() else "cpu" +_targets_tensor = torch.tensor(config.TARGET_CLASSES, device=_device) + +# === Zoom / устойчивость === +_Z_HOLD_AFTER_LOSS = getattr(config, "ZOOM_LOSS_HOLD_SEC", 0.70) +_Z_SIGN_HYST = getattr(config, "ZOOM_SIGN_HYST", 0.020) +_Z_MIN_CMD = getattr(config, "Z_MIN_CMD", 0.05) +_Z_IN_RAMP = getattr(config, "Z_RAMP_TIME", 0.45) +_Z_FF_GAIN = getattr(config, "Z_FF_GAIN", 0.55) +_Z_P = getattr(config, "Z_P", 2.4) +_Z_I = getattr(config, "Z_I", 0.16) +_Z_INT_CLAMP = getattr(config, "Z_INT_CLAMP", 0.15) +_Z_SPEED_LIMIT_OUT = getattr(config, "Z_SPEED_LIMIT_OUT", -0.65) +_Z_SPEED_LIMIT_IN = getattr(config, "Z_SPEED_LIMIT_IN", 0.45) +_Z_CENTER_GATE_IN = getattr(config, "Z_CENTER_GATE_BASE", 0.06) +_Z_CENTER_GATE_SCALE = getattr(config, "Z_CENTER_GATE_SCALE", 0.20) +_Z_UNLOCK_EDGE = getattr(config, "Z_UNLOCK_EDGE", 0.050) +_Z_PAN_BOOST_IN = getattr(config, "Z_PAN_BOOST_WHEN_IN", getattr(config, "Z_PAN_BOOST", 1.35)) + +# === TRACK lifecycle === +TRACK_END_LOSS_SEC = float(getattr(config, "TRACK_END_LOSS_SEC", 0.90)) # сколько ждать без детекции, чтобы завершить TRACK +TRACK_END_RETURN_COOLDOWN_SEC = float(getattr(config, "TRACK_END_RETURN_COOLDOWN_SEC", 2.5)) # анти-спам goto +TRACK_END_RESUME_DELAY_SEC = float(getattr(config, "TRACK_END_RESUME_DELAY_SEC", 1.0)) # после парковки + +def _publish_with_boxes( + cam_id: int, + img_bgr: np.ndarray, + boxes_xyxy: List[Tuple[int, int, int, int]], + labels: List[str], +) -> None: + if config.PUBLISH_ONLY_IF_CLIENTS and not state.video_clients: + return + down, sx, sy = maybe_downscale(img_bgr, config.PREVIEW_TARGET_W) + if boxes_xyxy: + for (x1, y1, x2, y2), lab in zip(boxes_xyxy, labels): + if sx != 1.0 or sy != 1.0: + x1 = int(x1 * sx); y1 = int(y1 * sy) + x2 = int(x2 * sx); y2 = int(y2 * sy) + cv2.rectangle(down, (x1, y1), (x2, y2), (0, 255, 0), 2, lineType=cv2.LINE_4) + cv2.putText( + down, lab, (x1, max(0, y1 - 6)), + cv2.FONT_HERSHEY_SIMPLEX, 0.6, (0, 255, 0), 1, cv2.LINE_4, + ) + publish_preview(cam_id, down) + + +def choose_auto_action(cam_idx: int, now: float) -> str: + st = state.ptz_states[cam_idx] + dt_cam = now - st["last_seen"] + if dt_cam <= config.SHORT_LOSS_SEC: + return "FOLLOW_SHORT" + if dt_cam <= config.LONG_LOSS_SEC: + return "SEARCH" + ema_g = state.detect_stats_global["ema_interval"] + if ema_g is None: + return "SEARCH" + dyn_thr = max(config.ABS_MIN_FALLBACK, 3.0 * float(ema_g)) + dyn_thr = min(dyn_thr, config.ABS_MAX_FALLBACK) + if config.FALLBACK_PRESET_CAP and config.FALLBACK_PRESET_CAP > 0: + dyn_thr = min(dyn_thr, config.FALLBACK_PRESET_CAP) + if dt_cam > dyn_thr: + return "FALLBACK" + return "SEARCH" if ema_g < 20.0 else "HOLD" + + +def _maybe_return_and_resume(cam_idx: int, reason: str, min_interval_sec: float) -> bool: + st = state.ptz_states[cam_idx] + now = time.time() + last = float(st.get("last_return_at", 0.0) or 0.0) + if (now - last) < float(min_interval_sec): + return False + + st["last_return_at"] = now + return_to_preset(cam_idx, reason) + ensure_patrol_resume(cam_idx, delay=1.0) + return True + + +def _enter_track(cam_idx: int) -> None: + st = state.ptz_states[cam_idx] + if not st.get("tracking_active"): + # запоминаем, был ли патруль активен — чтобы потом восстановить + st["_patrol_was_active_before_track"] = bool(st.get("patrol_active", False)) + st["tracking_active"] = True + st["mode"] = "TRACK" + # патрульные команды должны замолчать + st["patrol_active"] = False + st["sync_wait"] = False + + +def _end_track_and_park(cam_idx: int, *, reason: str) -> None: + st = state.ptz_states[cam_idx] + now = time.time() + + # если уже едем на пресет — не дублируем + if now < float(st.get("goto_in_progress_until", 0.0) or 0.0): + return + + last = float(st.get("last_track_end_return_at", 0.0) or 0.0) + if (now - last) < TRACK_END_RETURN_COOLDOWN_SEC: + return + st["last_track_end_return_at"] = now + + # СРАЗУ ставим "за нято goto", чтобы второй источник не успел добавить ещё один goto + settle = float(getattr(config, "GOTO_SETTLE_SEC", 2.5)) + st["goto_in_progress_until"] = now + settle + + st["tracking_active"] = False + st["mode"] = "IDLE" + notify_detected(cam_idx, False, force=True) + stop_ptz(cam_idx) + + return_to_preset(cam_idx, reason=reason) + ensure_patrol_resume(cam_idx, delay=max(0.8, TRACK_END_RESUME_DELAY_SEC)) + +def postprocess_control(cam_idx: int, frame_bgr: np.ndarray, res: Any, is_ptz: bool) -> None: + boxes_t = getattr(res, "boxes", None) + h, w = frame_bgr.shape[:2] + w_inv, h_inv = (1.0 / w), (1.0 / h) + now = time.time() + + st = state.ptz_states.setdefault(cam_idx, {}) + st.setdefault("mode", "auto" if config.AUTO_STRATEGY else "manual") + st.setdefault("last_seen", 0.0) + st.setdefault("loss_since", None) + st.setdefault("last_bbox", None) + + st.setdefault("prev_t", None) + st.setdefault("prev_cx", 0.5) + st.setdefault("prev_cy", 0.5) + st.setdefault("vx", 0.0) + st.setdefault("vy", 0.0) + st.setdefault("ix", 0.0) + st.setdefault("iy", 0.0) + + st.setdefault("w_frac_ema", None) + st.setdefault("prev_w_frac", None) + st.setdefault("w_growth", 0.0) + + st.setdefault("zoom_state", 0) + st.setdefault("zoom_prev_cmd", 0.0) + st.setdefault("zoom_int", 0.0) + st.setdefault("zoom_last_change", 0.0) + st.setdefault("zoom_ramp_start", 0.0) + st.setdefault("zoom_lock", False) + st.setdefault("lock_candidate_since", None) + st.setdefault("last_zoom_seen", 0.0) + + st.setdefault("last_dx", 0.0) + st.setdefault("last_dy", 0.0) + st.setdefault("stable_frames", 0) + st.setdefault("fallback_done", False) + st.setdefault("patrol_active", False) + st.setdefault("tracking_active", False) + st.setdefault("last_return_at", 0.0) + st.setdefault("last_track_end_return_at", 0.0) + + st.setdefault("attack_origin_pan", None) + st.setdefault("prev_pan_deg", None) + st.setdefault("prev_pan_t", None) + st.setdefault("last_heading_log", 0.0) + + _cfg = config.CAMERA_CONFIG.get(int(cam_idx), {}) if hasattr(config, "CAMERA_CONFIG") else {} + _PAN_SIGN: float = float(_cfg.get("pan_sign", 1) or 1) + _TILT_SIGN: float = float(_cfg.get("tilt_sign", -1) or -1) + + MAX_ANN_BOXES = 12 + det_xyxy: List[Tuple[int, int, int, int]] = [] + det_labels: List[str] = [] + best_target = None + + if boxes_t is not None and len(boxes_t) > 0: + confs = boxes_t.conf + clss = boxes_t.cls + mask = (confs >= config.DETECT_CONF) & torch.isin(clss, _targets_tensor) + if mask.any(): + xyxy_all = boxes_t.xyxy[mask].detach().cpu().numpy().astype(int) + conf_all = confs[mask].detach().cpu().numpy() + cls_all = clss[mask].detach().cpu().numpy().astype(int) + + x1 = xyxy_all[:, 0]; y1 = xyxy_all[:, 1] + x2 = xyxy_all[:, 2]; y2 = xyxy_all[:, 3] + cy = (y1 + y2) * 0.5 * h_inv + score_all = conf_all + 0.05 * ((x2 - x1) * w_inv) + ((cy < 0.45).astype(np.float32) * 0.12) + order = score_all.argsort()[::-1] + + topk = order[:MAX_ANN_BOXES] + for i in topk: + lab = f"{config.CLASS_NAMES.get(int(cls_all[i]), 'obj')} {float(conf_all[i]):.2f}" + det_xyxy.append((int(xyxy_all[i, 0]), int(xyxy_all[i, 1]), int(xyxy_all[i, 2]), int(xyxy_all[i, 3]))) + det_labels.append(lab) + + ibest = int(order[0]) + best_target = (xyxy_all[ibest].tolist(), float(conf_all[ibest]), int(cls_all[ibest])) + + # ========================= + # ===== есть детекция ====== + # ========================= + if best_target is not None: + (x1, y1, x2, y2), best_conf, _ = best_target + st["loss_since"] = None + st["last_bbox"] = (int(x1), int(y1), int(x2), int(y2)) + st["last_zoom_seen"] = now + st["last_seen"] = now + + _publish_with_boxes(cam_idx, frame_bgr, det_xyxy, det_labels) + + # --------- Панорама ---------- + if not is_ptz: + notify_detected(cam_idx, True) + + cx = (x1 + x2) * 0.5; cy = (y1 + y2) * 0.5 + cxn = cx * w_inv; cyn = cy * h_inv + if st["prev_t"] is not None: + dt = max(now - st["prev_t"], 1e-3) + st["vx"] = 0.85 * st["vx"] + 0.15 * ((cxn - st["prev_cx"]) / dt) + st["vy"] = 0.85 * st["vy"] + 0.15 * ((cyn - st["prev_cy"]) / dt) + st["prev_t"], st["prev_cx"], st["prev_cy"] = now, cxn, cyn + + try: + from . import geo_autocal + geo_autocal.feed_pano_sample(cam_idx, cxn, now) + except Exception: + pass + + base_az = float(config.get_cam_base_azimuth_deg(cam_idx)) + hfov = float(config.get_cam_hfov_deg(cam_idx)) + bearing = wrap_deg(base_az + (cxn - 0.5) * hfov) + + if st.get("attack_origin_pan") is None: + st["attack_origin_pan"] = bearing + st["prev_pan_deg"] = bearing + st["prev_pan_t"] = now + st["last_heading_log"] = 0.0 + + origin = float(st["attack_origin_pan"]) + cur_compass = deg_to_compass(float(bearing)) + org_compass = deg_to_compass(origin) + delta = wrap_deg(float(bearing) - origin) + cw_ccw = "CW" if delta >= 0.0 else "CCW" + + pan_rate = None + prev_t = st.get("prev_pan_t") + prev_p = st.get("prev_pan_deg") + if prev_t is not None and prev_p is not None: + dtp = max(1e-3, now - float(prev_t)) + pan_rate = wrap_deg(float(bearing) - float(prev_p)) / dtp + st["prev_pan_deg"] = float(bearing) + st["prev_pan_t"] = now + + if (now - float(st.get("last_heading_log", 0.0))) >= 0.7: + if pan_rate is None: + logger.info( + "[ATTACK/PANO] cam %d from %s (%.1f°) -> %s (%.1f°), Δ=%.1f° %s", + cam_idx, org_compass, origin, cur_compass, float(bearing), + float(delta), cw_ccw + ) + else: + logger.info( + "[ATTACK/PANO] cam %d from %s (%.1f°) -> %s (%.1f°), Δ=%.1f° %s, rate=%.1f°/s", + cam_idx, org_compass, origin, cur_compass, float(bearing), + float(delta), cw_ccw, float(pan_rate) + ) + st["last_heading_log"] = now + + ptz_id = config.PAN_TO_PTZ.get(cam_idx) + if ptz_id is not None: + pst = state.ptz_states.setdefault(ptz_id, {}) + pst["last_seen"] = now + pst["loss_since"] = None + pst["last_return_at"] = max(float(pst.get("last_return_at", 0.0) or 0.0), now) + + alarms.ingest_detection(cam_idx, True) + return + + # --------- PTZ: включаем TRACK сразу ---------- + _enter_track(cam_idx) + notify_detected(cam_idx, True) + + cx = (x1 + x2) * 0.5; cy = (y1 + y2) * 0.5 + cxn = cx * w_inv; cyn = cy * h_inv + w_frac = (x2 - x1) * w_inv + + prev_t = st.get("prev_t") + if prev_t is not None: + dt = max(now - float(prev_t), 1e-3) + vx_i = (cxn - st["prev_cx"]) / dt + vy_i = (cyn - st["prev_cy"]) / dt + vel_smooth = clipf(float(getattr(config, "VEL_SMOOTH", 0.85)), 0.0, 0.99) + st["vx"] = vel_smooth * st["vx"] + (1.0 - vel_smooth) * vx_i + st["vy"] = vel_smooth * st["vy"] + (1.0 - vel_smooth) * vy_i + + wf = st["w_frac_ema"] + alpha = getattr(config, "W_EMA_ALPHA", 0.30) + st["w_frac_ema"] = w_frac if wf is None else (1.0 - alpha) * wf + alpha * w_frac + w_smooth = st["w_frac_ema"] + if st["prev_w_frac"] is not None and prev_t is not None: + dtw = max(now - float(prev_t), 1e-3) + st["w_growth"] = (w_smooth - st["prev_w_frac"]) / dtw + else: + st["w_growth"] = 0.0 + # optional clamp to avoid spikes from detector jitter + wg_clamp = float(getattr(config, "W_GROWTH_CLAMP", 2.0)) + st["w_growth"] = clipf(float(st.get("w_growth", 0.0) or 0.0), -wg_clamp, wg_clamp) + st["prev_w_frac"] = w_smooth + + # update prev_* only after derivative computations + st["prev_t"], st["prev_cx"], st["prev_cy"] = now, cxn, cyn + + speed = abs(st["vx"]) + abs(st["vy"]) + far_factor = clipf(1.0 - max(w_smooth, (y2 - y1) * h_inv), 0.0, 1.0) + base_lead = float(getattr(config, "BASE_LEAD", 0.12)) + lead_gain = float(getattr(config, "LEAD_GAIN", 0.35)) + lead_max = float(getattr(config, "LEAD_MAX", 0.30)) + lead_hi = max(base_lead, lead_max) + lead_time = clipf(base_lead + lead_gain * speed * far_factor, base_lead, lead_hi) + # If target is far from center, disable aggressive lead to avoid overshoot during large slews. + err_mag = max(abs(cxn - 0.5), abs(cyn - 0.5)) + lead_off = float(getattr(config, "LEAD_OFF_ERR", 0.18)) + if err_mag > lead_off: + lead_time = base_lead + pred_min = float(getattr(config, "PRED_CLAMP_MIN", 0.05)) + pred_max = float(getattr(config, "PRED_CLAMP_MAX", 0.95)) + cxn_pred = clipf(cxn + st["vx"] * lead_time, pred_min, pred_max) + cyn_pred = clipf(cyn + st["vy"] * lead_time, pred_min, pred_max) + dx = cxn_pred - 0.5 + dy = cyn_pred - 0.5 + + sm_alpha = getattr(config, "AIM_SMOOTH", 0.55) + sm_dx = st["last_dx"] * (1 - sm_alpha) + dx * sm_alpha + sm_dy = st["last_dy"] * (1 - sm_alpha) + dy * sm_alpha + + if abs(sm_dx) > getattr(config, "DEAD_ZONE", 0.018): + st["ix"] = clipf(st["ix"] + sm_dx * 0.02, -0.20, 0.20) + else: + st["ix"] *= 0.9 + if abs(sm_dy) > getattr(config, "DEAD_ZONE", 0.018): + st["iy"] = clipf(st["iy"] + sm_dy * 0.02, -0.20, 0.20) + else: + st["iy"] *= 0.9 + + ddx = sm_dx - st["last_dx"] + ddy = sm_dy - st["last_dy"] + st["last_dx"], st["last_dy"] = sm_dx, sm_dy + + conf_norm = clipf((best_conf - config.DETECT_CONF) / max(1e-3, 0.40 - config.DETECT_CONF), 0.0, 1.0) + aim_gain = 0.6 + 0.4 * conf_norm + + pan_speed = clipf( + (getattr(config, "AIM_P", 1.85) * sm_dx + getattr(config, "AIM_I", 0.07) * st["ix"] + getattr(config, "AIM_D", 0.15) * ddx) * aim_gain, + -1.0, 1.0 + ) + tilt_speed = clipf( + (getattr(config, "AIM_P", 1.85) * sm_dy + getattr(config, "AIM_I", 0.07) * st["iy"] + getattr(config, "AIM_D", 0.15) * ddy) * aim_gain, + -1.0, 1.0 + ) + + center_err = max(abs(dx), abs(dy)) + left = x1 * w_inv; right = 1.0 - x2 * w_inv + top = y1 * h_inv; bottom = 1.0 - y2 * h_inv + min_margin = min(left, right, top, bottom) + + # === zoom logic === + state_zoom = st.get("zoom_state", 0) + can_switch = (now - float(st.get("zoom_last_change", 0.0) or 0.0)) >= float(getattr(config, "Z_MIN_SWITCH_INTERVAL", 0.50)) + + gate_dyn = max(0.05, _Z_CENTER_GATE_IN + _Z_CENTER_GATE_SCALE * (1.0 - clipf(w_smooth, 0.0, 1.0))) + center_good = center_err < gate_dyn + + vel = abs(st["vx"]) + abs(st["vy"]) + z_target = (1.0 - clipf(vel / max(1e-6, float(getattr(config, "Z_VEL_REF", 0.45))), 0.0, 1.0)) * float(getattr(config, "Z_TARGET_SLOW", 0.16)) \ + + clipf(vel / max(1e-6, float(getattr(config, "Z_VEL_REF", 0.45))), 0.0, 1.0) * float(getattr(config, "Z_TARGET_FAST", 0.12)) + z_band_min = z_target - float(getattr(config, "Z_DELTA_IN", 0.008)) + z_band_max = z_target + float(getattr(config, "Z_DELTA_OUT", 0.060)) + + out_edge_crit = min_margin < float(getattr(config, "Z_OUT_EDGE", 0.07)) + near_edge = (min_margin < 0.05) + + if st.get("zoom_lock", False): + if ( + out_edge_crit + or (vel > float(getattr(config, "Z_UNLOCK_VEL_THR", 0.060))) + or (abs(float(st.get("w_growth", 0.0) or 0.0)) > float(getattr(config, "Z_UNLOCK_GROWTH_THR", 0.015))) + or (center_err > float(getattr(config, "Z_UNLOCK_CENTER_THR", 0.100))) + or (min_margin < _Z_UNLOCK_EDGE) + ): + st["zoom_lock"] = False + st["lock_candidate_since"] = None + else: + locked_candidate = ( + center_good + and (vel < float(getattr(config, "Z_UNLOCK_VEL_THR", 0.060))) + and (abs(float(st.get("w_growth", 0.0) or 0.0)) < float(getattr(config, "Z_FREEZE_GROWTH_THR", 0.004))) + and (min_margin >= float(getattr(config, "Z_SAFETY_EDGE", 0.05))) + ) + if locked_candidate: + if st.get("lock_candidate_since") is None: + st["lock_candidate_since"] = now + elif (now - float(st["lock_candidate_since"])) >= float(getattr(config, "Z_LOCK_ARM_SECS", 0.8)): + st["zoom_lock"] = True + else: + st["lock_candidate_since"] = None + + want_in = (w_smooth < (z_band_min - _Z_SIGN_HYST)) and center_good and (not near_edge) and (not st.get("zoom_lock", False)) + want_out = (w_smooth > (z_band_max + _Z_SIGN_HYST)) or out_edge_crit + + next_state = state_zoom + if state_zoom >= 0 and want_in and can_switch: + next_state = +1 + elif state_zoom <= 0 and want_out and can_switch: + next_state = -1 + elif not want_in and not want_out: + next_state = 0 + + if next_state != state_zoom: + st["zoom_state"] = next_state + st["zoom_last_change"] = now + + err_w = z_target - w_smooth + if abs(err_w) < float(getattr(config, "Z_DEADBAND", 0.010)): + err_w = 0.0 + st["zoom_int"] = clipf(float(st.get("zoom_int", 0.0) or 0.0) + err_w * 0.04, -_Z_INT_CLAMP, _Z_INT_CLAMP) + zoom_pi = _Z_P * err_w + _Z_I * st["zoom_int"] + zoom_ff = _Z_FF_GAIN * (-(float(st.get("w_growth", 0.0) or 0.0))) + zoom_cmd = clipf(zoom_pi + zoom_ff, _Z_SPEED_LIMIT_OUT, _Z_SPEED_LIMIT_IN) + + if st.get("zoom_lock", False): + zoom_cmd = min(zoom_cmd, -0.20) if out_edge_crit else 0.0 + st["zoom_int"] *= 0.90 + else: + if st["zoom_state"] > 0: + zoom_cmd = max(zoom_cmd, 0.0) + elif st["zoom_state"] < 0: + zoom_cmd = min(zoom_cmd, 0.0) + else: + zoom_cmd = 0.0 + + if abs(zoom_cmd) < _Z_MIN_CMD: + zoom_cmd = 0.0 + + cmd_mag_est = max(abs(pan_speed), abs(tilt_speed)) + if (cmd_mag_est > float(getattr(config, "Z_VELOCITY_GATE", 0.45))) and (zoom_cmd > 0.0): + zoom_cmd = 0.0 + err_gate = float(getattr(config, "Z_ERR_GATE", 0.18)) + if (max(abs(sm_dx), abs(sm_dy)) > err_gate) and (zoom_cmd > 0.0): + zoom_cmd = 0.0 + if near_edge and zoom_cmd > 0.0: + zoom_cmd = 0.0 + if min_margin < float(getattr(config, "Z_SAFETY_EDGE", 0.05)): + zoom_cmd = min(zoom_cmd, -0.25) + + prev_z = float(st.get("zoom_prev_cmd", 0.0) or 0.0) + if zoom_cmd > 0.0: + if prev_z <= 0.0: + st["zoom_ramp_start"] = now + frac = clipf((now - float(st.get("zoom_ramp_start", now) or now)) / max(1e-3, _Z_IN_RAMP), 0.0, 1.0) + zoom_cmd *= (0.25 + 0.75 * frac) + else: + st["zoom_ramp_start"] = 0.0 + st["zoom_prev_cmd"] = zoom_cmd + + if zoom_cmd > 0.0 and center_good and not near_edge: + call_autofocus(cam_idx) + + dx_abs, dy_abs = abs(dx), abs(dy) + if (dx_abs < 0.040) and (dy_abs < 0.040): + st["stable_frames"] = int(st.get("stable_frames", 0) or 0) + 1 + else: + st["stable_frames"] = 0 + + st["fallback_done"] = False + + adj_pan, adj_tilt = pan_speed, tilt_speed + if zoom_cmd > 0.0: + adj_pan = clipf(adj_pan * _Z_PAN_BOOST_IN, -1.0, 1.0) + adj_tilt = clipf(adj_tilt * _Z_PAN_BOOST_IN, -1.0, 1.0) + + move_ptz_bounded(cam_idx, _PAN_SIGN * adj_pan, _TILT_SIGN * adj_tilt, zoom_cmd) + alarms.ingest_detection(cam_idx, True) + return + + # ========================= + # ===== детекций нет ====== + # ========================= + alarms.ingest_detection(cam_idx, False) + notify_detected(cam_idx, False) + + # если мы были в TRACK и детекция пропала — заканчиваем TRACK и паркуем камеру в пресет + if is_ptz and st.get("tracking_active"): + # считаем, что "потеряли" если нет детекции > TRACK_END_LOSS_SEC + if (now - float(st.get("last_seen", 0.0) or 0.0)) >= TRACK_END_LOSS_SEC: + _end_track_and_park(cam_idx, reason="track_end_park") + _publish_with_boxes(cam_idx, frame_bgr, [], []) + return + + # ниже — твоя старая логика “потерь/возврата” (оставляем) + st["zoom_lock"] = False + st["lock_candidate_since"] = None + + if st.get("loss_since") is None: + st["loss_since"] = now + st["attack_origin_pan"] = None + + recently_tracked = (st.get("last_seen") is not None) and ((now - float(st["last_seen"])) < 10.0) + + near_edge_flag = False + lb = st.get("last_bbox") + if lb is not None: + x1, y1, x2, y2 = lb + left = x1 * w_inv; right = 1.0 - x2 * w_inv + top = y1 * h_inv; bottom = 1.0 - y2 * h_inv + if ( + (left < config.EDGE_LOSS_MARGIN) + or (right < config.EDGE_LOSS_MARGIN) + or (top < config.VERT_LOSS_MARGIN) + or (bottom < config.VERT_LOSS_MARGIN) + ): + near_edge_flag = True + + if is_ptz and ((now - float(st.get("last_zoom_seen", 0.0) or 0.0)) < _Z_HOLD_AFTER_LOSS): + move_ptz_bounded( + cam_idx, + _PAN_SIGN * (float(st.get("last_dx", 0.0) or 0.0) * 0.6), + _TILT_SIGN * (float(st.get("last_dy", 0.0) or 0.0) * 0.6), + float(getattr(config, "RECOVER_ZOOM_OUT", -0.35)) * 0.25, + ) + _publish_with_boxes(cam_idx, frame_bgr, [], []) + return + + edge_grace = float(getattr(config, "EDGE_LOSS_GRACE_SEC", 1.8)) + timed_out = (st.get("loss_since") is not None) and ((now - float(st["loss_since"])) >= float(config.LOSS_TO_PRESET_SEC)) + near_edge_and_lost = ( + near_edge_flag + and (st.get("loss_since") is not None) + and ((now - float(st["loss_since"])) >= edge_grace) + ) + + if is_ptz and recently_tracked and (near_edge_and_lost or timed_out): + _maybe_return_and_resume( + cam_idx, + "loss_or_timeout", + float(getattr(config, "LOSS_RETURN_COOLDOWN_SEC", 8.0)), + ) + _publish_with_boxes(cam_idx, frame_bgr, [], []) + return + + if config.AUTO_STRATEGY and is_ptz: + action = choose_auto_action(cam_idx, now) + + # если патруль активен и не TRACK — не мешаем патрулю + if st.get("patrol_active", False) and st.get("mode") in ("IDLE", "SEARCH"): + st["stable_frames"] = 0 + + # ---------- устойчивое поведение при кратких провалах детекции ---------- + last_seen = float(st.get("last_seen", 0.0) or 0.0) + dt_loss = now - last_seen + + def _predict_dxdy() -> Tuple[float, float]: + cx0 = float(st.get("prev_cx", 0.5) or 0.5) + cy0 = float(st.get("prev_cy", 0.5) or 0.5) + vx = float(st.get("vx", 0.0) or 0.0) + vy = float(st.get("vy", 0.0) or 0.0) + w0 = st.get("w_frac_ema") + w0 = float(w0) if w0 is not None else 0.08 # типичный "далёкий" объект + speed = float((vx * vx + vy * vy) ** 0.5) + far_factor = clipf(1.0 - w0, 0.0, 1.0) + + base_lead = float(getattr(config, "BASE_LEAD", 0.12)) + lead_gain = float(getattr(config, "LEAD_GAIN", 0.35)) + lead_max = float(getattr(config, "LEAD_MAX", 0.30)) + lead_hi = max(base_lead, lead_max) + + lead_time = clipf(base_lead + lead_gain * speed * far_factor, base_lead, lead_hi) + err_mag = max(abs(cx0 - 0.5), abs(cy0 - 0.5)) + lead_off = float(getattr(config, "LEAD_OFF_ERR", 0.18)) + if err_mag > lead_off: + lead_time = base_lead + pred_min = float(getattr(config, "PRED_CLAMP_MIN", 0.05)) + pred_max = float(getattr(config, "PRED_CLAMP_MAX", 0.95)) + cxp = clipf(cx0 + vx * lead_time, pred_min, pred_max) + cyp = clipf(cy0 + vy * lead_time, pred_min, pred_max) + dx = cxp - 0.5 + dy = cyp - 0.5 + dcl = float(getattr(config, "PRED_DX_CLAMP", 0.35)) + return clipf(dx, -dcl, dcl), clipf(dy, -dcl, dcl) + + pred_dx, pred_dy = _predict_dxdy() + + short_thr = float(getattr(config, "SHORT_LOSS_SEC", 0.25)) + long_thr = float(getattr(config, "LONG_LOSS_SEC", 1.2)) + + if dt_loss <= short_thr: + decay = 1.0 + zoom_loss = 0.0 # freeze zoom на коротких провалах + elif dt_loss <= long_thr: + tau = float(getattr(config, "LOSS_DECAY_TAU", 0.35)) + decay = float(math.exp(-(dt_loss - short_thr) / max(1e-3, tau))) + zoom_loss = float(getattr(config, "RECOVER_ZOOM_OUT", -0.35)) * 0.35 # мягко расширяем кадр + else: + decay = 0.0 + zoom_loss = float(getattr(config, "RECOVER_ZOOM_OUT", -0.35)) * 0.6 + + st["stable_frames"] = 0 + + # HOLD_PREDICT: пока цель могла "мигнуть" — ведём по прогнозу, не накапливая задержку + if dt_loss <= long_thr: + # обновим last_dx/last_dy прогнозом, чтобы другие ветки (если сработают) не тянули старьё + st["last_dx"], st["last_dy"] = float(pred_dx), float(pred_dy) + move_ptz_bounded( + cam_idx, + _PAN_SIGN * float(pred_dx) * float(decay), + _TILT_SIGN * float(pred_dy) * float(decay), + float(zoom_loss), + ) + _publish_with_boxes(cam_idx, frame_bgr, [], []) + return + + if action == "FOLLOW_SHORT": + move_ptz_bounded( + cam_idx, + _PAN_SIGN * float(st.get("last_dx", 0.0) or 0.0), + _TILT_SIGN * float(st.get("last_dy", 0.0) or 0.0), + float(getattr(config, "RECOVER_ZOOM_OUT", -0.35)) * 0.6, + ) + + elif action in ("SEARCH", "HOLD"): + if action == "HOLD" and (now - float(st.get("last_seen", now) or now)) < 8.0: + st["last_dx"] = float(st.get("last_dx", 0.0) or 0.0) * 0.85 + st["last_dy"] = float(st.get("last_dy", 0.0) or 0.0) * 0.85 + move_ptz_bounded( + cam_idx, + _PAN_SIGN * float(st.get("last_dx", 0.0) or 0.0), + _TILT_SIGN * float(st.get("last_dy", 0.0) or 0.0), + float(getattr(config, "RECOVER_ZOOM_OUT", -0.35)) * 0.4, + ) + else: + stop_ptz(cam_idx) + + elif action == "FALLBACK": + if not st.get("patrol_active", False): + _maybe_return_and_resume( + cam_idx, + "fallback", + float(getattr(config, "FALLBACK_RETURN_COOLDOWN_SEC", 12.0)), + ) + + st["stable_frames"] = 0 \ No newline at end of file diff --git a/ptz_tracker_modular/preview.py b/ptz_tracker_modular/preview.py new file mode 100644 index 0000000..20ed14b --- /dev/null +++ b/ptz_tracker_modular/preview.py @@ -0,0 +1,128 @@ +from __future__ import annotations +import cv2 +import numpy as np +from typing import Any, Tuple, Optional +from . import state, config + +CV2_JPEG_PARAMS = [ + int(cv2.IMWRITE_JPEG_QUALITY), int(config.PREVIEW_JPEG_QUALITY), + int(cv2.IMWRITE_JPEG_PROGRESSIVE), 0, + int(cv2.IMWRITE_JPEG_OPTIMIZE), 0 +] + + +def encode_jpeg_bgr(img_bgr: np.ndarray) -> Optional[bytes]: + ok, jpeg = cv2.imencode(".jpg", img_bgr, CV2_JPEG_PARAMS) + return jpeg.tobytes() if ok else None + + +def maybe_downscale(img: np.ndarray, target_w: int = config.PREVIEW_TARGET_W) -> Tuple[np.ndarray, float, float]: + h, w = img.shape[:2] + if w <= target_w: + return img, 1.0, 1.0 + scale = target_w / float(w) + return cv2.resize(img, (target_w, int(h * scale)), interpolation=cv2.INTER_AREA), scale, scale + + +# ----------------------------- HUD (новое) ----------------------------- + +def _fmt(v: Optional[float], suffix: str = "°", nd: int = 1) -> str: + if v is None: + return "—" + try: + return f"{float(v):.{nd}f}{suffix}" + except Exception: + return "—" + +def _bearing_to_compass(b: Optional[float]) -> str: + if b is None: + return "—" + names = ["N","NNE","NE","ENE","E","ESE","SE","SSE", + "S","SSW","SW","WSW","W","WNW","NW","NNW"] + i = int(((float(b) % 360.0) + 11.25) // 22.5) % 16 + return names[i] + +def _draw_text_block(img: np.ndarray, lines: list[str], org=(10, 24)) -> None: + """ + Рисует компактный полупрозрачный блок с текстом в левом верхнем углу. + """ + x0, y0 = org + pad_x, pad_y = 8, 6 + line_h = 22 + # вычислим ширину по самой длинной строке + max_w = 0 + for t in lines: + (tw, th), _ = cv2.getTextSize(t, cv2.FONT_HERSHEY_SIMPLEX, 0.6, 2) + max_w = max(max_w, tw) + box_w = max_w + pad_x * 2 + box_h = line_h * len(lines) + pad_y * 2 + + x1, y1 = x0, y0 - 18 + x2, y2 = x1 + box_w, y1 + box_h + x2 = min(x2, img.shape[1] - 1) + y2 = min(y2, img.shape[0] - 1) + + # полупрозрачный фон + overlay = img.copy() + cv2.rectangle(overlay, (x1, y1), (x2, y2), (0, 0, 0), -1) + cv2.addWeighted(overlay, 0.35, img, 0.65, 0, img) + + # текст + y = y1 + pad_y + 16 + for t in lines: + cv2.putText(img, t, (x1 + pad_x, y), cv2.FONT_HERSHEY_SIMPLEX, 0.6, (255, 255, 255), 2, cv2.LINE_AA) + y += line_h + +def draw_status_hud(cam_id: int, img_bgr: np.ndarray) -> None: + """ + Отрисовывает HUD поверх кадра: + Bearing (гео), Δ от preset1, Attack (курс+румб), raw pan/tilt. + Ничего не делает, если данных нет. + """ + st = state.ptz_states.get(cam_id, {}) + bearing = st.get("bearing_curr_geo") + delta = st.get("delta_from_preset1_geo") + attack = st.get("attack_bearing_geo") + attack_compass = st.get("attack_compass") + az_deg = st.get("az_deg") + tilt_deg = st.get("tilt_deg") + + lines = [] + if bearing is not None: + lines.append(f"Bearing: {_fmt(bearing)} ({_bearing_to_compass(bearing)})") + if delta is not None: + sign = "+" if float(delta) >= 0 else "" + lines.append(f"ΔPreset1: {sign}{_fmt(delta)}") + if attack is not None or attack_compass: + comp = attack_compass if isinstance(attack_compass, str) else _bearing_to_compass(attack) + lines.append(f"Attack: {_fmt(attack)} ({comp})") + if az_deg is not None or tilt_deg is not None: + lines.append(f"raw pan/tilt: {_fmt(az_deg)} / {_fmt(tilt_deg)}") + + if lines: + _draw_text_block(img_bgr, lines, org=(10, 28)) + + +# --------------------------- публикация ---------------------------- + +def publish_preview(cam_id: int, img: np.ndarray) -> None: + """ + Публикатор JPEG-превью. При наличии флага config.PREVIEW_DRAW_HUD + поверх кадра будет нарисован компактный HUD с курсом/дельтой/атакой. + """ + if getattr(config, "PREVIEW_DRAW_HUD", False): + try: + draw_status_hud(cam_id, img) + except Exception: + # в превью важнее стабильность, ошибки HUD не должны мешать стриму + pass + + if config.PUBLISH_ONLY_IF_CLIENTS and not state.video_clients: + return + + jpeg = encode_jpeg_bgr(img) + if not jpeg: + return + fid = state.frame_id_by_cam.get(cam_id, 0) + 1 + state.frame_id_by_cam[cam_id] = fid + state.latest_jpeg_by_cam[cam_id] = (fid, jpeg) diff --git a/ptz_tracker_modular/ptz_io(main).py b/ptz_tracker_modular/ptz_io(main).py new file mode 100644 index 0000000..670da66 --- /dev/null +++ b/ptz_tracker_modular/ptz_io(main).py @@ -0,0 +1,646 @@ +from __future__ import annotations + +import logging +import time +from enum import Enum +from typing import NamedTuple, Union, Optional, Tuple + +import requests +from requests.auth import HTTPDigestAuth +from requests.adapters import HTTPAdapter + +try: + from urllib3.util.retry import Retry # type: ignore +except Exception: # на всякий случай + Retry = None # type: ignore + +from . import config, state +from .utils import clipf, quantf +from .geom import sector_contains, ang_diff_signed, normalize360 + +logger = logging.getLogger("PTZTracker.PTZ") + +_sess = requests.Session() +_sess.trust_env = False + +if Retry is not None: + _adapter = HTTPAdapter( + pool_connections=64, + pool_maxsize=64, + max_retries=Retry( + total=2, + backoff_factor=0.2, + status_forcelist=[500, 502, 503, 504], + raise_on_status=False, + ), + ) +else: + _adapter = HTTPAdapter(pool_connections=64, pool_maxsize=64, max_retries=0) + +_sess.mount("http://", _adapter) +_sess.mount("https://", _adapter) + +PTZ_HEADERS = {"Content-Type": "application/xml", "Connection": "keep-alive"} + +AF_CAP_KEY = "af_caps" # ключ в state.ptz_states[cam_idx] + + +def _af_caps(st: dict) -> dict: + caps = st.get(AF_CAP_KEY) + if not isinstance(caps, dict): + caps = {} + st[AF_CAP_KEY] = caps + return caps + + +def _base(cam_idx: int): + """ + Возвращает: + base(str), ch(int), auth, verify_tls(bool), timeout_sec(float), cfg(dict) + где base = "{scheme}://{host}:{port}" + """ + cfg = config.CAMERA_CONFIG[cam_idx] + scheme = cfg.get("scheme") or ("https" if cfg.get("https") else "http") + host = cfg["ip"] + port = int(cfg.get("port") or (443 if scheme == "https" else 80)) + ch = int(cfg.get("ptz_channel", 1)) + + user = cfg.get("username") or cfg.get("user") or cfg.get("login") or "admin" + pwd = cfg.get("password") or cfg.get("pass") or cfg.get("pwd") or "" + + auth = HTTPDigestAuth(user, pwd) + base = f"{scheme}://{host}:{port}" + verify = bool(cfg.get("verify_tls", False)) + timeout = float(cfg.get("ptz_timeout_sec", 3.0)) + + return base, ch, auth, verify, timeout, cfg + + +def _ptz_put( + url: str, + *, + data: bytes | str = b"", + headers=None, + auth=None, + timeout: float = 3.0, + verify: bool = False, +) -> Tuple[bool, int, str]: + """ + Унифицированный PUT с логом и отрезанным телом ответа. + Возвращает (ok, http_code, body[:300]). + """ + try: + r = _sess.put(url, data=data, headers=headers or {}, auth=auth, timeout=timeout, verify=verify) + body = (r.text or "")[:300] + ok = 200 <= r.status_code < 300 + if not ok: + logger.warning("PTZ HTTP %s url=%s body=%r", r.status_code, url, body) + return ok, r.status_code, body + except Exception as e: + logger.warning("PTZ HTTP EXC url=%s timeout=%.1f verify=%s: %s", url, timeout, verify, e) + return False, 0, str(e) + + +def _ptz_get(url: str, *, auth=None, timeout: float = 3.0, verify: bool = False): + try: + r = _sess.get(url, auth=auth, timeout=timeout, verify=verify) + return r, None + except Exception as e: + return None, e + + +class PTZCmd(Enum): + MOVE = "move" + GOTO = "goto" + FOCUS = "focus" + + +class MoveTuple(NamedTuple): + typ: PTZCmd + cam_idx: int + pan_i: int + tilt_i: int + zoom_i: int + + +class GotoTuple(NamedTuple): + typ: PTZCmd + cam_idx: int + token: str + + +class FocusTuple(NamedTuple): + typ: PTZCmd + cam_idx: int + + +PTZCommand = Union[MoveTuple, GotoTuple, FocusTuple, None] + + +# ------------------------------ +# Zoom reset helper (важно для патруля) +# ------------------------------ +def request_zoom_reset(cam_idx: int, duration_sec: float = 2.0, strength: float | None = None) -> None: + """ + Просим "мягко" сбросить зум в течение duration_sec через continuous zoom-out. + strength: отрицательное значение (например -0.35). Если None — берём из config. + """ + st = state.ptz_states[cam_idx] + now = time.time() + st["zoom_reset_until"] = now + float(max(0.2, duration_sec)) + if strength is None: + strength = float(getattr(config, "ZOOM_RESET_STRENGTH", -0.35)) + st["zoom_reset_strength"] = float(strength) + + +def _apply_zoom_reset_overlay(cam_idx: int, zoom: float) -> float: + """ + Если активен zoom_reset — принудительно добавляем zoom-out (отрицательное). + """ + st = state.ptz_states[cam_idx] + until = float(st.get("zoom_reset_until", 0.0) or 0.0) + if time.time() < until: + strength = float(st.get("zoom_reset_strength", getattr(config, "ZOOM_RESET_STRENGTH", -0.35)) or -0.35) + # гарантируем, что команда будет "наружу" минимум strength + return min(float(zoom), float(strength)) + return zoom + + +def move_ptz(cam_idx: int, pan: float, tilt: float, zoom: float = 0.0) -> None: + """ + Отправка относительного движения (continuous). Троттлинг + квантизация. + """ + st = state.ptz_states[cam_idx] + now = time.monotonic() + min_dt = getattr(config, "PTZ_MIN_INTERVAL_SEC", 0.12) + if now - st.get("last_ptz_send", 0.0) < min_dt: + return + + q = getattr(config, "PTZ_QUANT", 0.01) + pan_f = clipf(quantf(pan, q), -1.0, 1.0) + tilt_f = clipf(quantf(tilt, q), -1.0, 1.0) + zoom_f = clipf(quantf(zoom, q), -1.0, 1.0) + + lp, lt, lz = st.get("last_cmd", (0.0, 0.0, 0.0)) + eps = getattr(config, "PTZ_EPS", 0.01) + if (abs(pan_f - lp) < eps) and (abs(tilt_f - lt) < eps) and (abs(zoom_f - lz) < eps): + return + + st["last_ptz_send"] = now + st["last_cmd"] = (pan_f, tilt_f, zoom_f) + + pan_i, tilt_i, zoom_i = int(pan_f * 100), int(tilt_f * 100), int(zoom_f * 100) + if state.ptz_cmd_q is not None: + try: + state.ptz_cmd_q.put_nowait(MoveTuple(PTZCmd.MOVE, cam_idx, pan_i, tilt_i, zoom_i)) + except Exception: + pass + + +def stop_ptz(cam_idx: int) -> None: + move_ptz(cam_idx, 0.0, 0.0, 0.0) + + +def goto_preset_token(cam_idx: int, token: str) -> None: + if state.ptz_cmd_q is not None: + try: + state.ptz_cmd_q.put_nowait(GotoTuple(PTZCmd.GOTO, cam_idx, token)) + except Exception: + pass + + +def call_autofocus(cam_idx: int) -> None: + """ + Троттлинг + запуск автофокуса, если камера это поддерживает и не в кулдауне. + Управляется флагом config.AUTOFOCUS_TRY_ISAPI. + """ + if not getattr(config, "AUTOFOCUS_TRY_ISAPI", False): + return + + st = state.ptz_states[cam_idx] + now = time.monotonic() + + # не чаще 1 раза в 2с + if now - st.get("last_focus_time", 0.0) < 2.0: + return + st["last_focus_time"] = now + + caps = _af_caps(st) + cd = float(getattr(config, "AUTOFOCUS_COOLDOWN_SEC", 600.0)) + + # если знаем, что AF нет — уважаем кулдаун + if caps.get("has") is False and (now - caps.get("last_probe_fail", 0.0) < cd): + return + + if state.ptz_cmd_q is not None: + try: + state.ptz_cmd_q.put_nowait(FocusTuple(PTZCmd.FOCUS, cam_idx)) + except Exception: + pass + + +def move_ptz_bounded(cam_idx: int, pan_speed: float, tilt_speed: float, zoom: float = 0.0) -> None: + """ + Обёртка над move_ptz. + + ВАЖНО: у тебя ограничение сектора реализовано внутри _ptz_worker (по mode/az/sector). + Здесь добавляем только "overlay" сброса зума, чтобы патруль мог вернуть масштаб обратно. + """ + zoom = _apply_zoom_reset_overlay(cam_idx, zoom) + move_ptz(cam_idx, pan_speed, tilt_speed, zoom) + + +# ----------------------------------------------------------------------------- +# Статус PTZ +# ----------------------------------------------------------------------------- +def _parse_ptz_status(xml_text: str) -> Tuple[Optional[float], Optional[float]]: + import xml.etree.ElementTree as ET + + def _norm_pan(v: float) -> float: + av = abs(v) + # сотые или десятые градуса (часто бывает) + if av > 3600 and av <= 36000: + return v / 100.0 + if av > 360 and av <= 3600: + return v / 10.0 + return v + + def _norm_tilt(v: float) -> float: + av = abs(v) + if av > 900 and av <= 9000: + return v / 100.0 + if av > 90 and av <= 900: + return v / 10.0 + return v + + try: + root = ET.fromstring(xml_text) + + def local(tag: str) -> str: + return tag.split("}", 1)[-1] if "}" in tag else tag + + # 1) СНАЧАЛА ищем “правильные” теги + pri_pan = ("azimuth", "absolutepan") + pri_tilt = ("elevation", "absolutetilt") + + pan_val: Optional[float] = None + tilt_val: Optional[float] = None + + for el in root.iter(): + name = local(el.tag).lower() + if el.text is None: + continue + txt = el.text.strip() + if not txt: + continue + + if pan_val is None and name in pri_pan: + try: + pan_val = _norm_pan(float(txt)) + except Exception: + pass + + if tilt_val is None and name in pri_tilt: + try: + tilt_val = _norm_tilt(float(txt)) + except Exception: + pass + + # 2) ТОЛЬКО если не нашли — пробуем pan/tilt как fallback + if pan_val is None or tilt_val is None: + for el in root.iter(): + name = local(el.tag).lower() + if el.text is None: + continue + txt = el.text.strip() + if not txt: + continue + + if pan_val is None and name == "pan": + try: + pan_val = _norm_pan(float(txt)) + except Exception: + pass + if tilt_val is None and name == "tilt": + try: + tilt_val = _norm_tilt(float(txt)) + except Exception: + pass + + return pan_val, tilt_val + except Exception: + return None, None + + +def get_ptz_status(cam_idx: int) -> Tuple[Optional[float], Optional[float]]: + """ + Возвращает (azimuth_deg_normalized_0_360 | None, tilt | None). + Пытаемся по http, затем по https (если http дал 2xx, второй не трогаем). + """ + base, ch, auth, verify, timeout, _ = _base(cam_idx) + + # Попытка HTTP + url_http = f"{base}/ISAPI/PTZCtrl/channels/{ch}/status" + r, _e = _ptz_get(url_http, auth=auth, timeout=timeout, verify=False if base.startswith("http://") else verify) + if r is not None and (200 <= r.status_code < 300): + pan, tilt = _parse_ptz_status(r.text) + if pan is not None or tilt is not None: + logger.info( + "[ISAPI] cam=%s OK status: pan=%s tilt=%s via %s", + cam_idx, + f"{normalize360(pan):.2f}" if pan is not None else "None", + f"{tilt:.2f}" if tilt is not None else "None", + url_http, + ) + return normalize360(pan) if pan is not None else None, tilt + + # Попытка HTTPS (если http не дал валидные числа) + host = config.CAMERA_CONFIG[cam_idx]["ip"] + url_https = f"https://{host}:443/ISAPI/PTZCtrl/channels/{ch}/status" + r2, _e2 = _ptz_get(url_https, auth=auth, timeout=timeout, verify=verify) + if r2 is not None and (200 <= r2.status_code < 300): + pan, tilt = _parse_ptz_status(r2.text) + if pan is not None or tilt is not None: + logger.info( + "[ISAPI] cam=%s OK status: pan=%s tilt=%s via %s", + cam_idx, + f"{normalize360(pan):.2f}" if pan is not None else "None", + f"{tilt:.2f}" if tilt is not None else "None", + url_https, + ) + return normalize360(pan) if pan is not None else None, tilt + + return None, None + + +def read_ptz_azimuth_deg(cam_idx: int) -> Optional[float]: + pan_deg, _ = get_ptz_status(cam_idx) + return pan_deg + + +# ----------------------------------------------------------------------------- +# Умный фолбэк автофокуса (с кэшем и кулдауном) +# ----------------------------------------------------------------------------- +def _ptz_try_autofocus(base: str, auth, timeout: float, verify: bool, cam_idx: int) -> bool: + st = state.ptz_states[cam_idx] + caps = _af_caps(st) + + if caps.get("has") and caps.get("url"): + ok, _code, _body_txt = _ptz_put(caps["url"], data="", headers=PTZ_HEADERS, auth=auth, timeout=timeout, verify=verify) + if ok: + logger.info("[ISAPI] cam=%s autofocus OK via cached %s", cam_idx, caps["url"]) + return True + + now = time.monotonic() + cd = float(getattr(config, "AUTOFOCUS_COOLDOWN_SEC", 600.0)) + if caps.get("has") is False and (now - caps.get("last_probe_fail", 0.0) < cd): + return False + + candidates = [ + ("PUT", f"{base}/ISAPI/PTZCtrl/channels/1/focus", + "autotrigger"), + ("PUT", f"{base}/ISAPI/PTZCtrl/channels/1/focus", ""), + ("PUT", f"{base}/ISAPI/PTZCtrl/channels/1/triggerFocus", ""), + ("PUT", f"{base}/ISAPI/Image/channels/1/focus", + "trigger"), + ("PUT", f"{base}/ISAPI/Image/channels/1/focus/auto", ""), + ("PUT", f"{base}/ISAPI/System/Video/inputs/channels/1/focus", + "true"), + ("PUT", f"{base}/ISAPI/System/Video/inputs/channels/1/focus/auto", ""), + ] + + first_fail_logged = False + for _method, url, body in candidates: + ok, code, _body_txt = _ptz_put(url, data=body, headers=PTZ_HEADERS, auth=auth, timeout=timeout, verify=verify) + if ok: + caps["has"] = True + caps["url"] = url + logger.info("[ISAPI] cam=%s autofocus OK via %s", cam_idx, url) + return True + else: + if not first_fail_logged: + logger.warning("[ISAPI] cam=%s autofocus try fail code=%s url=%s", cam_idx, code, url) + first_fail_logged = True + else: + logger.debug("[ISAPI] cam=%s autofocus try fail code=%s url=%s", cam_idx, code, url) + + caps["has"] = False + caps["url"] = None + caps["last_probe_fail"] = now + logger.info("[ISAPI] cam=%s autofocus unsupported — backoff %.0fs", cam_idx, cd) + return False + + +def _ptz_goto_preset_isapi( + cam_idx: int, + base: str, + ch: int, + auth, + verify: bool, + timeout: float, + token: str, +) -> bool: + tok = str(token).strip() + url = f"{base}/ISAPI/PTZCtrl/channels/{ch}/presets/{tok}/goto" + + headers_xml = {"Content-Type": "application/xml; charset=UTF-8", "Connection": "keep-alive"} + + ok, code, body = _ptz_put(url, data=b"", headers=headers_xml, auth=auth, timeout=timeout, verify=verify) + if ok: + logger.info("[ISAPI] cam=%s goto preset OK via PUT-empty url=%s", cam_idx, url) + return True + + xml = f'{tok}' + ok, code, body = _ptz_put(url, data=xml, headers=headers_xml, auth=auth, timeout=timeout, verify=verify) + if ok: + logger.info("[ISAPI] cam=%s goto preset OK via PUT-xml url=%s", cam_idx, url) + return True + + headers_xml2 = {"Content-Type": "application/xml", "Connection": "keep-alive"} + ok, code, body = _ptz_put(url, data=xml, headers=headers_xml2, auth=auth, timeout=timeout, verify=verify) + if ok: + logger.info("[ISAPI] cam=%s goto preset OK via PUT-xml2 url=%s", cam_idx, url) + return True + + try: + r = _sess.post(url, data=xml, headers=headers_xml, auth=auth, timeout=timeout, verify=verify) + body2 = (r.text or "")[:300] + ok2 = 200 <= r.status_code < 300 + if ok2: + logger.info("[ISAPI] cam=%s goto preset OK via POST-xml url=%s", cam_idx, url) + return True + else: + logger.warning("PTZ HTTP %s url=%s body=%r", r.status_code, url, body2) + except Exception as e: + logger.warning("PTZ HTTP EXC url=%s timeout=%.1f verify=%s: %s", url, timeout, verify, e) + + logger.error( + "[ISAPI] cam=%s goto preset FAILED token=%s last_http=%s last_body=%r", + cam_idx, tok, code, (body or "")[:200], + ) + return False + + +# ----------------------------------------------------------------------------- +# Воркер отправки PTZ-команд +# ----------------------------------------------------------------------------- +def _ptz_worker() -> None: + """ + Читает команды из state.ptz_cmd_q и выполняет HTTP-вызовы. + """ + while True: + try: + cmd: PTZCommand = state.ptz_cmd_q.get(timeout=0.5) # type: ignore + except Exception: + continue + + if cmd is None: + break + + try: + # === CONTINUOUS MOVE === + if isinstance(cmd, MoveTuple) and cmd.typ is PTZCmd.MOVE: + cam_idx, pan_i, tilt_i, zoom_i = cmd.cam_idx, cmd.pan_i, cmd.tilt_i, cmd.zoom_i + base, ch, auth, verify, timeout, cfg = _base(cam_idx) + st = state.ptz_states[cam_idx] + + # не мешаем переезду на пресет + now_wall = time.time() + until = float(st.get("goto_in_progress_until", 0.0) or 0.0) + if now_wall < until: + continue + + # overlay zoom reset (важно: работает даже если команда пришла не через move_ptz_bounded) + if now_wall < float(st.get("zoom_reset_until", 0.0) or 0.0): + strength = float(st.get("zoom_reset_strength", getattr(config, "ZOOM_RESET_STRENGTH", -0.35)) or -0.35) + zoom_i = min(int(strength * 100), zoom_i) + + az = st.get("az_deg") + dmin = cfg.get("sector_min_deg") + dmax = cfg.get("sector_max_deg") + mode = st.get("mode", "IDLE") + + # сектор-ограничение НЕ применяется в TRACK (там мы разрешаем выход "внутрь" и т.п.) + if dmin is not None and dmax is not None and mode != "TRACK": + if az is None: + # если нет азимута — не толкаем пан, кроме патруля в IDLE/SEARCH + if not (st.get("patrol_active") and mode in ("IDLE", "SEARCH")): + pan_i = 0 + else: + pan_sign = cfg.get("pan_sign", 1) or 1 + inside = sector_contains(float(az), float(dmin), float(dmax)) + + def _cmd_dir() -> int: + if pan_i == 0: + return 0 + return 1 if (pan_i * pan_sign) > 0 else -1 + + cmd_dir = _cmd_dir() + + if not inside: + # если вне сектора — запрещаем толкать ещё дальше наружу + if cmd_dir != 0: + to_min = ang_diff_signed(float(dmin), float(az)) + to_max = ang_diff_signed(float(dmax), float(az)) + if abs(to_min) <= abs(to_max): + need_dir = 1 if to_min > 0 else -1 + else: + need_dir = 1 if to_max > 0 else -1 + if cmd_dir != need_dir: + pan_i = 0 + else: + # внутри: останов у границы + мягкое замедление + to_right = ang_diff_signed(float(dmax), float(az)) + to_left = ang_diff_signed(float(dmin), float(az)) + + pushing_right = (pan_i * pan_sign) > 0 and to_right <= 0.0 + pushing_left = (pan_i * pan_sign) < 0 and to_left >= 0.0 + if pushing_right or pushing_left: + pan_i = 0 + else: + if pan_i != 0: + dist_r = abs(to_right) + dist_l = abs(to_left) + dist_min = min(dist_r, dist_l) + edge_slow = getattr(config, "TRACK_EDGE_SLOWDOWN_DEG", 6.0) + edge_min_k = getattr(config, "TRACK_EDGE_MIN_SCALE", 0.25) + if dist_min < edge_slow: + k = max(edge_min_k, dist_min / max(1e-6, edge_slow)) + pan_i = int(pan_i * k) + + logger.info( + "[PTZ SEND] cam=%s mode=%s az=%s pan_i=%s tilt_i=%s zoom_i=%s", + cam_idx, st.get("mode", "IDLE"), st.get("az_deg"), + pan_i, tilt_i, zoom_i + ) + + url = f"{base}/ISAPI/PTZCtrl/channels/{ch}/continuous" + xml = f"{pan_i}{tilt_i}{zoom_i}" + ok, code, body = _ptz_put( + url, data=xml, headers=PTZ_HEADERS, + auth=auth, timeout=timeout, verify=verify + ) + if not ok and code == 0: + logger.error("PTZ HTTP failed for camera %s: %s", cam_idx, body) + + # === PRESET GOTO === + elif isinstance(cmd, GotoTuple) and cmd.typ is PTZCmd.GOTO: + cam_idx, token = cmd.cam_idx, cmd.token + base, ch, auth, verify, timeout, _cfg = _base(cam_idx) + + # ВАЖНО: используем надёжный goto (твоя функция), а не PUT-empty + ok = _ptz_goto_preset_isapi(cam_idx, base, ch, auth, verify, timeout, token) + if not ok: + continue + + st = state.ptz_states[cam_idx] + now = time.time() + settle = float(getattr(config, "GOTO_SETTLE_SEC", 2.5)) + st["goto_in_progress_until"] = now + settle + + st["mode"] = "IDLE" + st["tracking_active"] = False + try: + from .notify import notify_detected + notify_detected(cam_idx, False, force=True) + except Exception: + pass + + # СБРОС ЗУМА после возврата на пресет (чтобы не патрулировать в зуме) + dur = float(getattr(config, "GOTO_ZOOM_RESET_SEC", 2.0)) + strength = float(getattr(config, "ZOOM_RESET_STRENGTH", -0.35)) + request_zoom_reset(cam_idx, duration_sec=dur, strength=strength) + + # === AUTOFOCUS === + elif isinstance(cmd, FocusTuple) and cmd.typ is PTZCmd.FOCUS: + cam_idx = cmd.cam_idx + if not getattr(config, "AUTOFOCUS_TRY_ISAPI", False): + continue + + base, _ch, auth, verify, timeout, _ = _base(cam_idx) + + try: + cfg = config.CAMERA_CONFIG.get(cam_idx, {}) + override = cfg.get("focus_override") + except Exception: + override = None + + if override: + url = f"{base}{override}" + ok, code, body = _ptz_put( + url, data="", headers=PTZ_HEADERS, + auth=auth, timeout=timeout, verify=verify + ) + if not ok: + logger.warning( + "[ISAPI] cam=%s autofocus override failed code=%s body=%r", + cam_idx, code, (body or "")[:160] + ) + _ptz_try_autofocus(base, auth, timeout, verify, cam_idx) + else: + logger.info("[ISAPI] cam=%s autofocus OK via override %s", cam_idx, url) + else: + _ptz_try_autofocus(base, auth, timeout, verify, cam_idx) + + except Exception as e: + logger.error("PTZ worker error: %s", e) diff --git a/ptz_tracker_modular/ptz_io.py b/ptz_tracker_modular/ptz_io.py new file mode 100644 index 0000000..8a94ce9 --- /dev/null +++ b/ptz_tracker_modular/ptz_io.py @@ -0,0 +1,666 @@ +from __future__ import annotations + +import logging +import time +from enum import Enum +from typing import NamedTuple, Union, Optional, Tuple + +import requests +from requests.auth import HTTPDigestAuth +from requests.adapters import HTTPAdapter + +try: + from urllib3.util.retry import Retry # type: ignore +except Exception: # на всякий случай + Retry = None # type: ignore + +from . import config, state +from .utils import clipf, quantf +from .geom import sector_contains, ang_diff_signed, normalize360 + +logger = logging.getLogger("PTZTracker.PTZ") + +_sess = requests.Session() +_sess.trust_env = False + +if Retry is not None: + _adapter = HTTPAdapter( + pool_connections=64, + pool_maxsize=64, + max_retries=Retry( + total=2, + backoff_factor=0.2, + status_forcelist=[500, 502, 503, 504], + raise_on_status=False, + ), + ) +else: + _adapter = HTTPAdapter(pool_connections=64, pool_maxsize=64, max_retries=0) + +_sess.mount("http://", _adapter) +_sess.mount("https://", _adapter) + +PTZ_HEADERS = {"Content-Type": "application/xml", "Connection": "keep-alive"} + +AF_CAP_KEY = "af_caps" # ключ в state.ptz_states[cam_idx] + + +def _af_caps(st: dict) -> dict: + caps = st.get(AF_CAP_KEY) + if not isinstance(caps, dict): + caps = {} + st[AF_CAP_KEY] = caps + return caps + + +def _base(cam_idx: int): + """ + Возвращает: + base(str), ch(int), auth, verify_tls(bool), timeout_sec(float), cfg(dict) + где base = "{scheme}://{host}:{port}" + """ + cfg = config.CAMERA_CONFIG[cam_idx] + scheme = cfg.get("scheme") or ("https" if cfg.get("https") else "http") + host = cfg["ip"] + port = int(cfg.get("port") or (443 if scheme == "https" else 80)) + ch = int(cfg.get("ptz_channel", 1)) + + user = cfg.get("username") or cfg.get("user") or cfg.get("login") or "admin" + pwd = cfg.get("password") or cfg.get("pass") or cfg.get("pwd") or "" + + auth = HTTPDigestAuth(user, pwd) + base = f"{scheme}://{host}:{port}" + verify = bool(cfg.get("verify_tls", False)) + timeout = float(cfg.get("ptz_timeout_sec", 3.0)) + + return base, ch, auth, verify, timeout, cfg + + +def _ptz_put( + url: str, + *, + data: bytes | str = b"", + headers=None, + auth=None, + timeout: float = 3.0, + verify: bool = False, +) -> Tuple[bool, int, str]: + """ + Унифицированный PUT с логом и отрезанным телом ответа. + Возвращает (ok, http_code, body[:300]). + """ + try: + r = _sess.put(url, data=data, headers=headers or {}, auth=auth, timeout=timeout, verify=verify) + body = (r.text or "")[:300] + ok = 200 <= r.status_code < 300 + if not ok: + logger.warning("PTZ HTTP %s url=%s body=%r", r.status_code, url, body) + return ok, r.status_code, body + except Exception as e: + logger.warning("PTZ HTTP EXC url=%s timeout=%.1f verify=%s: %s", url, timeout, verify, e) + return False, 0, str(e) + + +def _ptz_get(url: str, *, auth=None, timeout: float = 3.0, verify: bool = False): + try: + r = _sess.get(url, auth=auth, timeout=timeout, verify=verify) + return r, None + except Exception as e: + return None, e + + +class PTZCmd(Enum): + MOVE = "move" + GOTO = "goto" + FOCUS = "focus" + + +class MoveTuple(NamedTuple): + typ: PTZCmd + cam_idx: int + pan_i: int + tilt_i: int + zoom_i: int + + +class GotoTuple(NamedTuple): + typ: PTZCmd + cam_idx: int + token: str + + +class FocusTuple(NamedTuple): + typ: PTZCmd + cam_idx: int + + +PTZCommand = Union[MoveTuple, GotoTuple, FocusTuple, None] + + +# ------------------------------ +# Zoom reset helper (важно для патруля) +# ------------------------------ +def request_zoom_reset(cam_idx: int, duration_sec: float = 2.0, strength: float | None = None) -> None: + """ + Просим "мягко" сбросить зум в течение duration_sec через continuous zoom-out. + strength: отрицательное значение (например -0.35). Если None — берём из config. + """ + st = state.ptz_states[cam_idx] + now = time.time() + st["zoom_reset_until"] = now + float(max(0.2, duration_sec)) + if strength is None: + strength = float(getattr(config, "ZOOM_RESET_STRENGTH", -0.35)) + st["zoom_reset_strength"] = float(strength) + + +def _apply_zoom_reset_overlay(cam_idx: int, zoom: float) -> float: + """ + Если активен zoom_reset — принудительно добавляем zoom-out (отрицательное). + """ + st = state.ptz_states[cam_idx] + until = float(st.get("zoom_reset_until", 0.0) or 0.0) + if time.time() < until: + strength = float(st.get("zoom_reset_strength", getattr(config, "ZOOM_RESET_STRENGTH", -0.35)) or -0.35) + # гарантируем, что команда будет "наружу" минимум strength + return min(float(zoom), float(strength)) + return zoom + + +def move_ptz(cam_idx: int, pan: float, tilt: float, zoom: float = 0.0) -> None: + """ + Отправка относительного движения (continuous). Троттлинг + квантизация. + """ + st = state.ptz_states[cam_idx] + now = time.monotonic() + min_dt = getattr(config, "PTZ_MIN_INTERVAL_SEC", 0.12) + if now - st.get("last_ptz_send", 0.0) < min_dt: + return + + q = getattr(config, "PTZ_QUANT", 0.01) + pan_f = clipf(quantf(pan, q), -1.0, 1.0) + tilt_f = clipf(quantf(tilt, q), -1.0, 1.0) + zoom_f = clipf(quantf(zoom, q), -1.0, 1.0) + + lp, lt, lz = st.get("last_cmd", (0.0, 0.0, 0.0)) + eps = getattr(config, "PTZ_EPS", 0.01) + if (abs(pan_f - lp) < eps) and (abs(tilt_f - lt) < eps) and (abs(zoom_f - lz) < eps): + return + + st["last_ptz_send"] = now + st["last_cmd"] = (pan_f, tilt_f, zoom_f) + + pan_i, tilt_i, zoom_i = int(pan_f * 100), int(tilt_f * 100), int(zoom_f * 100) + if state.ptz_cmd_q is not None: + try: + state.ptz_cmd_q.put_nowait(MoveTuple(PTZCmd.MOVE, cam_idx, pan_i, tilt_i, zoom_i)) + except Exception: + pass + + +def stop_ptz(cam_idx: int) -> None: + move_ptz(cam_idx, 0.0, 0.0, 0.0) + + +def goto_preset_token(cam_idx: int, token: str) -> None: + if state.ptz_cmd_q is not None: + try: + state.ptz_cmd_q.put_nowait(GotoTuple(PTZCmd.GOTO, cam_idx, token)) + except Exception: + pass + + +def call_autofocus(cam_idx: int) -> None: + """ + Троттлинг + запуск автофокуса, если камера это поддерживает и не в кулдауне. + Управляется флагом config.AUTOFOCUS_TRY_ISAPI. + """ + if not getattr(config, "AUTOFOCUS_TRY_ISAPI", False): + return + + st = state.ptz_states[cam_idx] + now = time.monotonic() + + # не чаще 1 раза в 2с + if now - st.get("last_focus_time", 0.0) < 2.0: + return + st["last_focus_time"] = now + + caps = _af_caps(st) + cd = float(getattr(config, "AUTOFOCUS_COOLDOWN_SEC", 600.0)) + + # если знаем, что AF нет — уважаем кулдаун + if caps.get("has") is False and (now - caps.get("last_probe_fail", 0.0) < cd): + return + + if state.ptz_cmd_q is not None: + try: + state.ptz_cmd_q.put_nowait(FocusTuple(PTZCmd.FOCUS, cam_idx)) + except Exception: + pass + + +def move_ptz_bounded(cam_idx: int, pan_speed: float, tilt_speed: float, zoom: float = 0.0) -> None: + """ + Обёртка над move_ptz. + + ВАЖНО: у тебя ограничение сектора реализовано внутри _ptz_worker (по mode/az/sector). + Здесь добавляем только "overlay" сброса зума, чтобы патруль мог вернуть масштаб обратно. + """ + zoom = _apply_zoom_reset_overlay(cam_idx, zoom) + move_ptz(cam_idx, pan_speed, tilt_speed, zoom) + + +# ----------------------------------------------------------------------------- +# Статус PTZ +# ----------------------------------------------------------------------------- +def _parse_ptz_status(xml_text: str) -> Tuple[Optional[float], Optional[float]]: + import xml.etree.ElementTree as ET + + def _norm_pan(v: float) -> float: + av = abs(v) + # сотые или десятые градуса (часто бывает) + if av > 3600 and av <= 36000: + return v / 100.0 + if av > 360 and av <= 3600: + return v / 10.0 + return v + + def _norm_tilt(v: float) -> float: + av = abs(v) + if av > 900 and av <= 9000: + return v / 100.0 + if av > 90 and av <= 900: + return v / 10.0 + return v + + try: + root = ET.fromstring(xml_text) + + def local(tag: str) -> str: + return tag.split("}", 1)[-1] if "}" in tag else tag + + # 1) СНАЧАЛА ищем “правильные” теги + pri_pan = ("azimuth", "absolutepan") + pri_tilt = ("elevation", "absolutetilt") + + pan_val: Optional[float] = None + tilt_val: Optional[float] = None + + for el in root.iter(): + name = local(el.tag).lower() + if el.text is None: + continue + txt = el.text.strip() + if not txt: + continue + + if pan_val is None and name in pri_pan: + try: + pan_val = _norm_pan(float(txt)) + except Exception: + pass + + if tilt_val is None and name in pri_tilt: + try: + tilt_val = _norm_tilt(float(txt)) + except Exception: + pass + + # 2) ТОЛЬКО если не нашли — пробуем pan/tilt как fallback + if pan_val is None or tilt_val is None: + for el in root.iter(): + name = local(el.tag).lower() + if el.text is None: + continue + txt = el.text.strip() + if not txt: + continue + + if pan_val is None and name == "pan": + try: + pan_val = _norm_pan(float(txt)) + except Exception: + pass + if tilt_val is None and name == "tilt": + try: + tilt_val = _norm_tilt(float(txt)) + except Exception: + pass + + return pan_val, tilt_val + except Exception: + return None, None + + +def get_ptz_status(cam_idx: int) -> Tuple[Optional[float], Optional[float]]: + """ + Возвращает (azimuth_deg_normalized_0_360 | None, tilt | None). + Пытаемся по http, затем по https (если http дал 2xx, второй не трогаем). + """ + base, ch, auth, verify, timeout, _ = _base(cam_idx) + + # Попытка HTTP + url_http = f"{base}/ISAPI/PTZCtrl/channels/{ch}/status" + r, _e = _ptz_get(url_http, auth=auth, timeout=timeout, verify=False if base.startswith("http://") else verify) + if r is not None and (200 <= r.status_code < 300): + pan, tilt = _parse_ptz_status(r.text) + if pan is not None or tilt is not None: + logger.info( + "[ISAPI] cam=%s OK status: pan=%s tilt=%s via %s", + cam_idx, + f"{normalize360(pan):.2f}" if pan is not None else "None", + f"{tilt:.2f}" if tilt is not None else "None", + url_http, + ) + return normalize360(pan) if pan is not None else None, tilt + + # Попытка HTTPS (если http не дал валидные числа) + host = config.CAMERA_CONFIG[cam_idx]["ip"] + url_https = f"https://{host}:443/ISAPI/PTZCtrl/channels/{ch}/status" + r2, _e2 = _ptz_get(url_https, auth=auth, timeout=timeout, verify=verify) + if r2 is not None and (200 <= r2.status_code < 300): + pan, tilt = _parse_ptz_status(r2.text) + if pan is not None or tilt is not None: + logger.info( + "[ISAPI] cam=%s OK status: pan=%s tilt=%s via %s", + cam_idx, + f"{normalize360(pan):.2f}" if pan is not None else "None", + f"{tilt:.2f}" if tilt is not None else "None", + url_https, + ) + return normalize360(pan) if pan is not None else None, tilt + + return None, None + + +def read_ptz_azimuth_deg(cam_idx: int) -> Optional[float]: + pan_deg, _ = get_ptz_status(cam_idx) + return pan_deg + + +# ----------------------------------------------------------------------------- +# Умный фолбэк автофокуса (с кэшем и кулдауном) +# ----------------------------------------------------------------------------- +def _ptz_try_autofocus(base: str, auth, timeout: float, verify: bool, cam_idx: int) -> bool: + st = state.ptz_states[cam_idx] + caps = _af_caps(st) + + if caps.get("has") and caps.get("url"): + ok, _code, _body_txt = _ptz_put(caps["url"], data="", headers=PTZ_HEADERS, auth=auth, timeout=timeout, verify=verify) + if ok: + logger.info("[ISAPI] cam=%s autofocus OK via cached %s", cam_idx, caps["url"]) + return True + + now = time.monotonic() + cd = float(getattr(config, "AUTOFOCUS_COOLDOWN_SEC", 600.0)) + if caps.get("has") is False and (now - caps.get("last_probe_fail", 0.0) < cd): + return False + + candidates = [ + ("PUT", f"{base}/ISAPI/PTZCtrl/channels/1/focus", + "autotrigger"), + ("PUT", f"{base}/ISAPI/PTZCtrl/channels/1/focus", ""), + ("PUT", f"{base}/ISAPI/PTZCtrl/channels/1/triggerFocus", ""), + ("PUT", f"{base}/ISAPI/Image/channels/1/focus", + "trigger"), + ("PUT", f"{base}/ISAPI/Image/channels/1/focus/auto", ""), + ("PUT", f"{base}/ISAPI/System/Video/inputs/channels/1/focus", + "true"), + ("PUT", f"{base}/ISAPI/System/Video/inputs/channels/1/focus/auto", ""), + ] + + first_fail_logged = False + for _method, url, body in candidates: + ok, code, _body_txt = _ptz_put(url, data=body, headers=PTZ_HEADERS, auth=auth, timeout=timeout, verify=verify) + if ok: + caps["has"] = True + caps["url"] = url + logger.info("[ISAPI] cam=%s autofocus OK via %s", cam_idx, url) + return True + else: + if not first_fail_logged: + logger.warning("[ISAPI] cam=%s autofocus try fail code=%s url=%s", cam_idx, code, url) + first_fail_logged = True + else: + logger.debug("[ISAPI] cam=%s autofocus try fail code=%s url=%s", cam_idx, code, url) + + caps["has"] = False + caps["url"] = None + caps["last_probe_fail"] = now + logger.info("[ISAPI] cam=%s autofocus unsupported — backoff %.0fs", cam_idx, cd) + return False + + +def _ptz_goto_preset_isapi( + cam_idx: int, + base: str, + ch: int, + auth, + verify: bool, + timeout: float, + token: str, +) -> bool: + tok = str(token).strip() + url = f"{base}/ISAPI/PTZCtrl/channels/{ch}/presets/{tok}/goto" + + headers_xml = {"Content-Type": "application/xml; charset=UTF-8", "Connection": "keep-alive"} + + ok, code, body = _ptz_put(url, data=b"", headers=headers_xml, auth=auth, timeout=timeout, verify=verify) + if ok: + logger.info("[ISAPI] cam=%s goto preset OK via PUT-empty url=%s", cam_idx, url) + return True + + xml = f'{tok}' + ok, code, body = _ptz_put(url, data=xml, headers=headers_xml, auth=auth, timeout=timeout, verify=verify) + if ok: + logger.info("[ISAPI] cam=%s goto preset OK via PUT-xml url=%s", cam_idx, url) + return True + + headers_xml2 = {"Content-Type": "application/xml", "Connection": "keep-alive"} + ok, code, body = _ptz_put(url, data=xml, headers=headers_xml2, auth=auth, timeout=timeout, verify=verify) + if ok: + logger.info("[ISAPI] cam=%s goto preset OK via PUT-xml2 url=%s", cam_idx, url) + return True + + try: + r = _sess.post(url, data=xml, headers=headers_xml, auth=auth, timeout=timeout, verify=verify) + body2 = (r.text or "")[:300] + ok2 = 200 <= r.status_code < 300 + if ok2: + logger.info("[ISAPI] cam=%s goto preset OK via POST-xml url=%s", cam_idx, url) + return True + else: + logger.warning("PTZ HTTP %s url=%s body=%r", r.status_code, url, body2) + except Exception as e: + logger.warning("PTZ HTTP EXC url=%s timeout=%.1f verify=%s: %s", url, timeout, verify, e) + + logger.error( + "[ISAPI] cam=%s goto preset FAILED token=%s last_http=%s last_body=%r", + cam_idx, tok, code, (body or "")[:200], + ) + return False + + +# ----------------------------------------------------------------------------- +# Воркер отправки PTZ-команд +# ----------------------------------------------------------------------------- +def _ptz_worker() -> None: + """ + Читает команды из state.ptz_cmd_q и выполняет HTTP-вызовы. + """ + from collections import deque + pending = deque() + + while True: + try: + if pending: + cmd: PTZCommand = pending.popleft() + else: + cmd: PTZCommand = state.ptz_cmd_q.get(timeout=0.5) # type: ignore + except Exception: + continue + + if cmd is None: + break + + # "latest wins": сжимаем поток MOVE-команд, чтобы не накапливать задержку + if isinstance(cmd, MoveTuple) and cmd.typ is PTZCmd.MOVE: + latest = cmd + while True: + try: + nxt: PTZCommand = state.ptz_cmd_q.get_nowait() # type: ignore + except Exception: + break + if isinstance(nxt, MoveTuple) and nxt.typ is PTZCmd.MOVE and nxt.cam_idx == latest.cam_idx: + latest = nxt + else: + pending.append(nxt) + cmd = latest + + try: + # === CONTINUOUS MOVE === + if isinstance(cmd, MoveTuple) and cmd.typ is PTZCmd.MOVE: + cam_idx, pan_i, tilt_i, zoom_i = cmd.cam_idx, cmd.pan_i, cmd.tilt_i, cmd.zoom_i + base, ch, auth, verify, timeout, cfg = _base(cam_idx) + st = state.ptz_states[cam_idx] + + # не мешаем переезду на пресет + now_wall = time.time() + until = float(st.get("goto_in_progress_until", 0.0) or 0.0) + if now_wall < until: + continue + + # overlay zoom reset (важно: работает даже если команда пришла не через move_ptz_bounded) + if now_wall < float(st.get("zoom_reset_until", 0.0) or 0.0): + strength = float(st.get("zoom_reset_strength", getattr(config, "ZOOM_RESET_STRENGTH", -0.35)) or -0.35) + zoom_i = min(int(strength * 100), zoom_i) + + az = st.get("az_deg") + dmin = cfg.get("sector_min_deg") + dmax = cfg.get("sector_max_deg") + mode = st.get("mode", "IDLE") + + # сектор-ограничение НЕ применяется в TRACK (там мы разрешаем выход "внутрь" и т.п.) + if dmin is not None and dmax is not None and mode != "TRACK": + if az is None: + # если нет азимута — не толкаем пан, кроме патруля в IDLE/SEARCH + if not (st.get("patrol_active") and mode in ("IDLE", "SEARCH")): + pan_i = 0 + else: + pan_sign = cfg.get("pan_sign", 1) or 1 + inside = sector_contains(float(az), float(dmin), float(dmax)) + + def _cmd_dir() -> int: + if pan_i == 0: + return 0 + return 1 if (pan_i * pan_sign) > 0 else -1 + + cmd_dir = _cmd_dir() + + if not inside: + # если вне сектора — запрещаем толкать ещё дальше наружу + if cmd_dir != 0: + to_min = ang_diff_signed(float(dmin), float(az)) + to_max = ang_diff_signed(float(dmax), float(az)) + if abs(to_min) <= abs(to_max): + need_dir = 1 if to_min > 0 else -1 + else: + need_dir = 1 if to_max > 0 else -1 + if cmd_dir != need_dir: + pan_i = 0 + else: + # внутри: останов у границы + мягкое замедление + to_right = ang_diff_signed(float(dmax), float(az)) + to_left = ang_diff_signed(float(dmin), float(az)) + + pushing_right = (pan_i * pan_sign) > 0 and to_right <= 0.0 + pushing_left = (pan_i * pan_sign) < 0 and to_left >= 0.0 + if pushing_right or pushing_left: + pan_i = 0 + else: + if pan_i != 0: + dist_r = abs(to_right) + dist_l = abs(to_left) + dist_min = min(dist_r, dist_l) + edge_slow = getattr(config, "TRACK_EDGE_SLOWDOWN_DEG", 6.0) + edge_min_k = getattr(config, "TRACK_EDGE_MIN_SCALE", 0.25) + if dist_min < edge_slow: + k = max(edge_min_k, dist_min / max(1e-6, edge_slow)) + pan_i = int(pan_i * k) + + logger.info( + "[PTZ SEND] cam=%s mode=%s az=%s pan_i=%s tilt_i=%s zoom_i=%s", + cam_idx, st.get("mode", "IDLE"), st.get("az_deg"), + pan_i, tilt_i, zoom_i + ) + + url = f"{base}/ISAPI/PTZCtrl/channels/{ch}/continuous" + xml = f"{pan_i}{tilt_i}{zoom_i}" + ok, code, body = _ptz_put( + url, data=xml, headers=PTZ_HEADERS, + auth=auth, timeout=timeout, verify=verify + ) + if not ok and code == 0: + logger.error("PTZ HTTP failed for camera %s: %s", cam_idx, body) + + # === PRESET GOTO === + elif isinstance(cmd, GotoTuple) and cmd.typ is PTZCmd.GOTO: + cam_idx, token = cmd.cam_idx, cmd.token + base, ch, auth, verify, timeout, _cfg = _base(cam_idx) + + # ВАЖНО: используем надёжный goto (твоя функция), а не PUT-empty + ok = _ptz_goto_preset_isapi(cam_idx, base, ch, auth, verify, timeout, token) + if not ok: + continue + + st = state.ptz_states[cam_idx] + now = time.time() + settle = float(getattr(config, "GOTO_SETTLE_SEC", 2.5)) + st["goto_in_progress_until"] = now + settle + + st["mode"] = "IDLE" + st["tracking_active"] = False + try: + from .notify import notify_detected + notify_detected(cam_idx, False, force=True) + except Exception: + pass + + # СБРОС ЗУМА после возврата на пресет (чтобы не патрулировать в зуме) + dur = float(getattr(config, "GOTO_ZOOM_RESET_SEC", 2.0)) + strength = float(getattr(config, "ZOOM_RESET_STRENGTH", -0.35)) + request_zoom_reset(cam_idx, duration_sec=dur, strength=strength) + + # === AUTOFOCUS === + elif isinstance(cmd, FocusTuple) and cmd.typ is PTZCmd.FOCUS: + cam_idx = cmd.cam_idx + if not getattr(config, "AUTOFOCUS_TRY_ISAPI", False): + continue + + base, _ch, auth, verify, timeout, _ = _base(cam_idx) + + try: + cfg = config.CAMERA_CONFIG.get(cam_idx, {}) + override = cfg.get("focus_override") + except Exception: + override = None + + if override: + url = f"{base}{override}" + ok, code, body = _ptz_put( + url, data="", headers=PTZ_HEADERS, + auth=auth, timeout=timeout, verify=verify + ) + if not ok: + logger.warning( + "[ISAPI] cam=%s autofocus override failed code=%s body=%r", + cam_idx, code, (body or "")[:160] + ) + _ptz_try_autofocus(base, auth, timeout, verify, cam_idx) + else: + logger.info("[ISAPI] cam=%s autofocus OK via override %s", cam_idx, url) + else: + _ptz_try_autofocus(base, auth, timeout, verify, cam_idx) + + except Exception as e: + logger.error("PTZ worker error: %s", e) diff --git a/ptz_tracker_modular/sector.py b/ptz_tracker_modular/sector.py new file mode 100644 index 0000000..eba0e0c --- /dev/null +++ b/ptz_tracker_modular/sector.py @@ -0,0 +1,134 @@ +from __future__ import annotations +import asyncio +import logging +from typing import Optional, Tuple + +from . import config, state +from .geom import normalize360, ang_diff_signed +from .ptz_io import read_ptz_azimuth_deg, goto_preset_token +from .toml_persist import persist_geo_fields + +logger = logging.getLogger("PTZTracker") + +def sector_contains(az_deg: float, sector_min_deg: float, sector_max_deg: float) -> bool: + """Проверка попадания азимута в сектор. + + Сектор задаётся двумя границами по кругу [0..360): + - если sector_min_deg <= sector_max_deg: обычный интервал + - если sector_min_deg > sector_max_deg: сектор «пересекает 0» (wrap-around) + """ + az = normalize360(float(az_deg)) + a = normalize360(float(sector_min_deg)) + b = normalize360(float(sector_max_deg)) + if a <= b: + return a <= az <= b + # wrap через 0: например [345..147] + return az >= a or az <= b + + +def sector_clamp(az_deg: float, sector_min_deg: float, sector_max_deg: float) -> float: + """Если азимут вне сектора — возвращаем ближайшую границу, иначе az_deg.""" + az = normalize360(float(az_deg)) + a = normalize360(float(sector_min_deg)) + b = normalize360(float(sector_max_deg)) + if sector_contains(az, a, b): + return az + # выберем ближайшую границу по угловой дистанции + da = abs(ang_diff_signed(a, az)) + db = abs(ang_diff_signed(b, az)) + return a if da <= db else b + + +def _compute_sector_bounds_from_hfov(ptz_cam_id: int) -> Optional[Tuple[float, float, float]]: + pano_id = config.PTZ_TO_PAN.get(ptz_cam_id) + if pano_id is None: + return None + ptz_cfg = config.CAMERA_CONFIG[ptz_cam_id] + pano_cfg = config.CAMERA_CONFIG[pano_id] + center = normalize360(float(ptz_cfg.get("pan_offset_deg", 0.0))) + hfov = float(pano_cfg.get("bullet_hfov_deg", 89.0)) + half = max(0.0, hfov * 0.5 - config.SECTOR_MARGIN_DEG) + left = normalize360(center - half) + right = normalize360(center + half) + return (center, left, right) + +async def sector_autocal_from_presets() -> None: + if not config.USE_PRESET_EDGES_FOR_SECTOR: + return + for cam in config.PTZ_CAM_IDS: + cfg = config.CAMERA_CONFIG[cam] + p1, p2 = cfg.get("preset1"), cfg.get("preset2") + if not (p1 and p2): + continue + goto_preset_token(cam, p1) + await asyncio.sleep(0.8) + a = read_ptz_azimuth_deg(cam) + goto_preset_token(cam, p2) + await asyncio.sleep(0.8) + b = read_ptz_azimuth_deg(cam) + if a is None or b is None: + logger.warning("[SECTOR] cam %s: can't read azimuths", cam) + continue + span = abs(ang_diff_signed(b, a)) + center = normalize360(a + ang_diff_signed(b, a) * 0.5) + half = max(0.0, span * 0.5 - config.SECTOR_MARGIN_DEG) + left = normalize360(center - half) + right = normalize360(center + half) + + st = state.ptz_states[cam] + st["sector_center_deg"] = center + st["sector_left_deg"] = left + st["sector_right_deg"] = right + st["sector_min_deg"] = left + st["sector_max_deg"] = right + + cfg["pan_offset_deg"] = center + cfg["sector_min_deg"] = left + cfg["sector_max_deg"] = right + cfg["preset1_deg"] = a + cfg["preset2_deg"] = b + + # сохраняем значения в cameras.toml + persist_geo_fields(cam, { + "preset1_deg": a, + "preset2_deg": b, + "sector_min_deg": left, + "sector_max_deg": right, + "sector_left_deg": left, + "sector_right_deg": right, + "pan_offset_deg": center, + }, source="sector_autocal") + + # обновим HFOV у пушки, если можем + pano_id = config.PTZ_TO_PAN.get(cam) + if pano_id is not None: + config.CAMERA_CONFIG[pano_id]["bullet_hfov_deg"] = span + + logger.info("[SECTOR] cam %s: span=%.1f°, center=%.1f°, L=%.1f°, R=%.1f°", cam, span, center, left, right) + +def sector_init_on_startup() -> None: + if config.USE_PRESET_EDGES_FOR_SECTOR: + for cam in config.PTZ_CAM_IDS: + st = state.ptz_states[cam] + if st.get("sector_left_deg") is None: + b = _compute_sector_bounds_from_hfov(cam) + if b is None: + continue + center, left, right = b + st["sector_center_deg"] = center + st["sector_left_deg"] = left + st["sector_right_deg"] = right + st["sector_min_deg"] = left + st["sector_max_deg"] = right + return + for cam in config.PTZ_CAM_IDS: + b = _compute_sector_bounds_from_hfov(cam) + if b is None: + continue + center, left, right = b + st = state.ptz_states[cam] + st["sector_center_deg"] = center + st["sector_left_deg"] = left + st["sector_right_deg"] = right + st["sector_min_deg"] = left + st["sector_max_deg"] = right \ No newline at end of file diff --git a/ptz_tracker_modular/servers.py b/ptz_tracker_modular/servers.py new file mode 100644 index 0000000..60b2cda --- /dev/null +++ b/ptz_tracker_modular/servers.py @@ -0,0 +1,384 @@ +# ptz_tracker_modular/servers.py +from __future__ import annotations + +import asyncio +import logging +import struct +from typing import Any, Dict, Tuple, List + +import websockets + +# --- конфиг/утилы/состояние --- +try: + from . import config + from .utils import json_dumps, json_loads + from .state import ( + video_clients, + vue_control_clients, + latest_jpeg_by_cam, + detected_cameras, + detection_queue, + ptz_states, + ) + from .preview import publish_preview, maybe_downscale + from .cameras_io import add_or_update_camera_pair, get_all as get_all_cameras +except Exception: # запуск вне пакета (не рекомендуется) + import config # type: ignore + from utils import json_dumps, json_loads # type: ignore + from state import ( # type: ignore + video_clients, + vue_control_clients, + latest_jpeg_by_cam, + detected_cameras, + detection_queue, + ptz_states, + ) + from preview import publish_preview, maybe_downscale # type: ignore + from cameras_io import add_or_update_camera_pair, get_all as get_all_cameras # type: ignore + +# >>> импорт триггера дворника (ISAPI) +try: + from .wiper import trigger_wiper_once +except Exception: # запасной путь + try: + from wiper import trigger_wiper_once # type: ignore + except Exception: + trigger_wiper_once = None # type: ignore + +logger = logging.getLogger("PTZTracker") + +PACKER_HI = struct.Struct(">HI") # cam_id:uint16, frame_id:uint32 + + +# ===================== VIDEO WS ===================== + +async def video_ws_handler(ws, path=None) -> None: + addr = getattr(ws, "remote_address", None) + logger.info("[VIDEO WS] client connected: %s path=%s", addr, path) + video_clients.add(ws) + try: + await ws.wait_closed() + finally: + video_clients.discard(ws) + logger.info("[VIDEO WS] client disconnected: %s", addr) + + +async def video_broadcaster() -> None: + """ + Раздает JPEG-кадры всем подписчикам video WS. + Держит темп для каждого клиента отдельно и выкидывает тех, у кого буфер распух. + """ + client_state: Dict[Any, Dict[str, Any]] = {} + min_interval_fast = config.VIDEO_MIN_INTERVAL_FAST + min_interval_slow = config.VIDEO_MIN_INTERVAL_SLOW + send_slow_sec = 0.02 + buf_soft = 4 * 1024 * 1024 + buf_hard = 8 * 1024 * 1024 + loop_tick = 1.0 / 200.0 + + while True: + try: + if not video_clients or not latest_jpeg_by_cam: + await asyncio.sleep(loop_tick) + continue + + now = asyncio.get_running_loop().time() + dead: set[Any] = set() + + for ws in list(video_clients): + st = client_state.setdefault( + ws, {"last_fid": {}, "min_interval": min_interval_fast, "last_sent": 0.0} + ) + if (now - st["last_sent"]) < st["min_interval"]: + continue + + transport = getattr(ws, "transport", None) + buf_sz = transport.get_write_buffer_size() if transport else 0 + if buf_sz > buf_hard: + try: + await ws.close() + except Exception: + pass + dead.add(ws) + continue + + pending: List[Tuple[int, int, bytes]] = [] + last_fid: Dict[int, int] = st["last_fid"] + + for cam_id, (fid, jpeg) in latest_jpeg_by_cam.items(): + if last_fid.get(cam_id, -1) != fid: + payload = PACKER_HI.pack(int(cam_id), int(fid)) + jpeg + pending.append((cam_id, fid, payload)) + + if config.SEND_DUPLICATES_WHEN_IDLE and not pending: + for cam_id, (fid, jpeg) in latest_jpeg_by_cam.items(): + payload = PACKER_HI.pack(int(cam_id), int(fid)) + jpeg + pending.append((cam_id, fid, payload)) + + if not pending: + continue + + max_per_tick = 12 if st["min_interval"] <= (1.0 / 30.0) else 6 + if len(pending) > max_per_tick: + pending.sort(key=lambda x: x[1], reverse=True) + pending = pending[:max_per_tick] + + t0 = asyncio.get_running_loop().time() + try: + send = ws.send + for cam_id, fid, payload in pending: + await send(payload) + last_fid[cam_id] = fid + except Exception: + dead.add(ws) + continue + finally: + st["last_sent"] = asyncio.get_running_loop().time() + + send_time = st["last_sent"] - t0 + if (send_time > send_slow_sec) or (buf_sz > buf_soft): + st["min_interval"] = min(st["min_interval"] * 1.25, min_interval_slow) + else: + st["min_interval"] = max(st["min_interval"] * 0.90, min_interval_fast) + + for d in dead: + video_clients.discard(d) + client_state.pop(d, None) + + except Exception as exc: + logger.error("video_broadcaster error: %s", exc) + + await asyncio.sleep(loop_tick) + + +# ===================== CONTROL WS ===================== + +async def _ctrl_send(ws, payload: Dict[str, Any]) -> None: + try: + await ws.send(json_dumps(payload)) + except Exception: + try: + await ws.close() + except Exception: + pass + vue_control_clients.discard(ws) + + +async def handle_vue_control(ws, path=None) -> None: + addr = getattr(ws, "remote_address", None) + logger.info("[CTRL WS] client connected: %s path=%s", addr, path) + vue_control_clients.add(ws) + try: + # начальная инициализация фронта + await _ctrl_send(ws, {"type": "init", "data": [len(detected_cameras)] + sorted(detected_cameras)}) + + async for msg in ws: + try: + data = json_loads(msg) + except Exception: + logger.warning("[CTRL WS] bad JSON from %s: %r", addr, msg) + continue + + mtype = data.get("type") + + # === переключение стратегии авто/ручной + if mtype == "policy": + auto = data.get("auto") + logger.info("[CTRL WS] policy request from %s: auto=%r", addr, auto) + if isinstance(auto, bool): + try: + import importlib + import ptz_tracker_modular.config as _cfg # type: ignore + _cfg.AUTO_STRATEGY = auto + await _ctrl_send(ws, {"type": "policy_ack", "auto": _cfg.AUTO_STRATEGY}) + logger.info("[CTRL WS] policy set to auto=%r", _cfg.AUTO_STRATEGY) + except Exception as e: + await _ctrl_send(ws, {"type": "policy_ack", "ok": False, "error": str(e)}) + logger.exception("[CTRL WS] policy error: %s", e) + continue + + # === полноэкран / выбор камеры + if mtype == "numberCamera": + try: + payload_data = data.get("data", {}) + canvas_id = payload_data.get("canvasId", None) + if isinstance(canvas_id, str): + canvas_id = int(canvas_id.strip()) + elif isinstance(canvas_id, float): + canvas_id = int(canvas_id) + elif not isinstance(canvas_id, int): + raise ValueError("canvasId must be int/str/float") + + payload = {"type": "numberCamera", "data": {"canvasId": canvas_id}} + if detection_queue is not None: + detection_queue.put_nowait(payload) + await _ctrl_send(ws, {"type": "numberCamera_ack", "ok": True}) + logger.info("[CTRL WS] numberCamera -> %s", canvas_id) + else: + await _ctrl_send(ws, {"type": "numberCamera_ack", "ok": False, "error": "no_queue"}) + logger.warning("[CTRL WS] numberCamera: no_queue") + except Exception as e: + logger.exception("[CTRL WS] numberCamera error: %s", e) + await _ctrl_send(ws, {"type": "numberCamera_ack", "ok": False, "error": str(e)}) + continue + + # === дворник (ISAPI) + if mtype == "wiper": + if trigger_wiper_once is None: + await _ctrl_send(ws, {"type": "wiper_ack", "ok": False, "error": "wiper not available"}) + logger.warning("[CTRL WS] wiper requested but not available") + continue + try: + payload = data.get("data") or {} + cam = int(payload.get("cam")) + sec = int(payload.get("sec", 3)) + logger.info("[CTRL WS] WIPER request from %s -> cam=%s sec=%s", addr, cam, sec) + + ok = await trigger_wiper_once(cam, sec) + await _ctrl_send(ws, {"type": "wiper_ack", "ok": bool(ok), "cam": cam, "sec": sec}) + logger.info("[CTRL WS] WIPER ack ok=%r cam=%s sec=%s", bool(ok), cam, sec) + except Exception as e: + logger.exception("[CTRL WS] wiper error: %s", e) + await _ctrl_send(ws, {"type": "wiper_ack", "ok": False, "error": str(e)}) + continue + + # === добавить КАМЕРУ (создать пару PTZ+PAN по IP) + if mtype == "add_camera": + """ + Ожидаемый payload: + { + "type": "add_camera", + "data": { + "ip": "...", "username": "...", "password": "...", + "preset1": "1", "preset2": "2", # для PTZ + "ptz_channel": 1, # можно прислать 1 или 2 — игнорится, мы создаём пару + "sweep_sign": 1 + } + } + """ + try: + payload = data.get("data") or {} + logger.info("[CTRL WS] add_camera from %s: %s", addr, payload) + if "ip" not in payload: + raise ValueError("missing 'ip'") + # создаём/обновляем пару записей + ptz_id, cams = add_or_update_camera_pair(payload) + # пересобираем производные структуры + try: + config.reload_cameras() + except Exception as e: + logger.warning("reload_cameras failed: %s", e) + + await _ctrl_send(ws, { + "type": "add_camera_ack", + "ok": True, + "ptz_id": ptz_id, + "count": len(cams), + }) + logger.info("[CTRL WS] add_camera OK -> ptz_id=%s total=%s", ptz_id, len(cams)) + except Exception as e: + logger.exception("[CTRL WS] add_camera error: %s", e) + await _ctrl_send(ws, {"type": "add_camera_ack", "ok": False, "error": str(e)}) + continue + + # === список камер по запросу (опционально) + if mtype == "list_cameras": + try: + cams = get_all_cameras() + await _ctrl_send(ws, {"type": "list_cameras_ack", "ok": True, "cameras": cams}) + logger.info("[CTRL WS] list_cameras -> %s items", len(cams)) + except Exception as e: + await _ctrl_send(ws, {"type": "list_cameras_ack", "ok": False, "error": str(e)}) + logger.exception("[CTRL WS] list_cameras error: %s", e) + continue + + logger.debug("[CTRL WS] unknown message type: %s", mtype) + + except Exception as exc: + logger.warning("[CTRL WS] error: %s", exc) + finally: + vue_control_clients.discard(ws) + logger.info("[CTRL WS] client disconnected: %s", addr) + + +# ===================== CONTROL STATUS BROADCASTER ===================== + +async def control_broadcaster(interval_sec: float = 0.25) -> None: + """ + Периодически шлёт фронтенду агрегированный статус по всем камерам. + Формат сообщения: + { + "type": "camera_status_batch", + "items": [ + { + "cam": 0, + "az": 123.4, "tilt": -12.3, + "mode": "auto", "zoom_state": 0, + "bearing_geo": 278.1, + "delta_from_preset1_geo": -5.2, + "attack_bearing_geo": 260.0, + "attack_compass": "W", + "last_seen": 1699999999.12 + }, ... + ] + } + """ + while True: + try: + if not vue_control_clients: + await asyncio.sleep(interval_sec) + continue + + items: List[Dict[str, Any]] = [] + for cam, st in list(ptz_states.items()): + try: + items.append({ + "cam": cam, + "az": st.get("az_deg"), + "tilt": st.get("tilt_deg"), + "mode": st.get("mode"), + "zoom_state": st.get("zoom_state"), + "bearing_geo": st.get("bearing_curr_geo"), + "delta_from_preset1_geo": st.get("delta_from_preset1_geo"), + "attack_bearing_geo": st.get("attack_bearing_geo"), + "attack_compass": st.get("attack_compass"), + "last_seen": st.get("last_seen"), + }) + except Exception: + continue + + if items: + payload = {"type": "camera_status_batch", "items": items} + msg = json_dumps(payload) + dead: set[Any] = set() + for ws in list(vue_control_clients): + try: + await ws.send(msg) + except Exception: + dead.add(ws) + for d in dead: + vue_control_clients.discard(d) + + # (опционально) краткая сводка в лог + if getattr(config, "LOG_STATUS_BATCH", False): + ptz_ids = set(getattr(config, "PTZ_CAM_IDS", [])) + line_parts = [] + for it in items: + cid = it.get("cam") + if cid in ptz_ids: + az = it.get("az") + b = it.get("bearing_geo") + d = it.get("delta_from_preset1_geo") + a = it.get("attack_bearing_geo") + s_az = f"az={az:.1f}°" if isinstance(az, (int, float)) else "az=n/a" + s_b = f" geo={b:.1f}°" if isinstance(b, (int, float)) else "" + s_d = f" dP1={d:+.1f}°" if isinstance(d, (int, float)) else "" + s_a = f" atk={a:.1f}°" if isinstance(a, (int, float)) else "" + line_parts.append(f"cam{cid}: {s_az}{s_b}{s_d}{s_a}") + if line_parts: + logger.info("[STATUS] " + " | ".join(line_parts)) + + except Exception as e: + logger.debug("control_broadcaster error: %s", e) + + await asyncio.sleep(interval_sec) diff --git a/ptz_tracker_modular/state.py b/ptz_tracker_modular/state.py new file mode 100644 index 0000000..ba48bae --- /dev/null +++ b/ptz_tracker_modular/state.py @@ -0,0 +1,201 @@ +from __future__ import annotations +import time +from collections import defaultdict, deque +from typing import Any, Dict, Optional, Tuple, List, Set + +try: + from . import config # нормальный пакетный импорт +except Exception: # если файл импортнули как топ-левел + import config as config # type: ignore + +# ---------------- Clients / WS ---------------- +vue_control_clients: Set[Any] = set() +video_clients: Set[Any] = set() +detected_cameras: Set[int] = set() + +# Порядок и маппинг «плиток» (обновляется из streaming._broadcast_init_to_vue) +last_broadcast_ids: List[int] = [] +index_to_cam_id: Dict[int, int] = {} # 1-based: 1 -> cam_id_1, 2 -> cam_id_2, ... + +# ---------------- Queues/loop (init in main) ---------------- +alarm_queue: Optional[Any] = None +detection_queue: Optional[Any] = None +MAIN_LOOP: Optional[Any] = None + +# ---------------- PTZ thread + queue (init in main) ---------------- +ptz_cmd_q: Optional[Any] = None +_ptz_thread: Optional[Any] = None + +# ---------------- MicroBatcher (init in main) ---------------- +batcher: Optional[Any] = None + +# ---------------- Preview frames ---------------- +latest_jpeg_by_cam: Dict[int, Tuple[int, bytes]] = {} +frame_id_by_cam: Dict[int, int] = {} + +# ---------------- Detect stats ---------------- +detect_stats_global = {"last_ts": None, "ema_interval": None, "times": deque(maxlen=512)} +detect_stats_cam = defaultdict(lambda: {"last_ts": None, "ema_interval": None, "times": deque(maxlen=256)}) + +# ---------------- Sync runtime (cohorts) ---------------- +sync_state: Dict[str, Dict[str, Any]] = { + g: {"dir": +1, "leg_idx": 0, "leg_start": 0.0, "leg_end": 0.0, "next_start_at": 0.0} + for g in config.SYNC_COHORTS +} + +# ---------------- Default PTZ state factory ---------------- +def _default_ptz_state(cam_id: int) -> Dict[str, Any]: + """ + Полный набор ключей, чтобы все модули (postprocess/patrol/ptz_io/…) + не ловили KeyError при «горячем» добавлении камеры. + """ + now = time.time() + is_ptz = bool(cam_id in config.PTZ_CAM_IDS) + + return { + # Tracking / kinematics + "last_seen": now, + "returning": False, + "last_dx": 0.0, "last_dy": 0.0, + "ix": 0.0, "iy": 0.0, + "vx": 0.0, "vy": 0.0, + "prev_t": None, "prev_cx": None, "prev_cy": None, + "prev_w_frac": None, "w_growth": 0.0, "w_frac_ema": None, + "mode": "IDLE", + + # Image history + "prev_gray": None, "of_pts": None, "last_bbox": None, + + # Zoom/aim dynamics + "zoom_state": 0, + "zoom_int": 0.0, + "zoom_prev_cmd": 0.0, + "zoom_ramp_start": 0.0, + "zoom_last_change": 0.0, + "zoom_lock": False, + "lock_candidate_since": None, + "was_zoomed_in": False, + "zoom_reset_timer": None, # asyncio.TimerHandle | None + + # Preset timers + "preset_timer": None, # asyncio.TimerHandle | None + "preset_target": None, # str | int | None + + # Command/throttle bookkeeping + "proc_busy": False, + "last_focus_time": 0.0, + "last_ptz_send": 0.0, + "last_cmd": (0.0, 0.0, 0.0), # (pan, tilt, zoom) -1..+1 + "last_cmd_ts": 0.0, + "last_pan": 0.0, + "last_tilt": 0.0, + "last_zoom": 0.0, + "ptz_busy_until": 0.0, + "pan_sign": 1, + + # Modes / loss recovery + "mode": "IDLE", + "loss_since": None, + "recover_until": 0.0, + "fallback_done": False, + "center_lock": 0, + "scan_phase": 0, "next_scan_at": 0.0, + "zoom_hold_until": 0.0, + "az_none_since": None, + "last_return_at": 0.0, + + # Stats / UI + "track_history": deque(maxlen=12), + "stable_frames": 0, + "alarm_sent": False, + "miss_frames": 0, + + # Patrol + "patrol_dir": +1, + "patrol_active": False, + "patrol_idx": 0, # <- пригодится управлялке патруля + "patrol_pause_until": 0.0, # <- мягкие паузы между ногами + "leg_start": 0.0, + "leg_end": 0.0, + "endpoint_latch": 0, + + # Sweep + "sweep_active": False, + "sweep_pan": 0.0, + "sweep_start_at": 0.0, + + # Anti-smear / shots + "shot_next_at": 0.0, + "shot_pause_until": 0.0, + + # Absolute commands + "last_abs_send": 0.0, + "last_abs_deg": None, + + # PTZ status (telemetry) + # общие поля, которые раньше были: + "az_deg": None, # относительный пан от драйвера (если такой есть) + "tilt_deg": None, # относительный тилт от драйвера (если такой есть) + # добавленные поля для абсолютной геометрии/оптики: + "pan_deg_abs": None, # абсолютный пан 0..360, привязанный к северу + "tilt_deg_abs": None, # абсолютный наклон (если доступен) + "hfov_deg": None, # точный HFOV PTZ на текущем зуме (если знаем) + "abs_zoom": 0.0, # абсолютное значение зума (масштаб зависит от твоего статус-пуллера) + + # Sector (soft bounds) + "sector_left_deg": None, + "sector_right_deg": None, + "sector_center_deg": None, + + # Sync mode + "sync_wait": False, + + # Recording debounce (CPP CTRL channel) + "rec_active": False, + "rec_first_seen": None, + "rec_started_at": 0.0, + "rec_last_seen": 0.0, + "rec_last_off_at": 0.0, + "rec_last_sent": None, + + # Meta + "is_ptz": is_ptz, + } + +# ---------------- Runtime PTZ state map ---------------- +ptz_states: Dict[int, Dict[str, Any]] = {} + +# Инициализация для камер, которые уже были в конфиге на момент импорта +_now = time.time() +for _cid in config.CAMERA_CONFIG: + # Берём дефолты и при необходимости можно дополнять из существующего (если будет перезагрузка модуля) + ptz_states[_cid] = _default_ptz_state(_cid) + +# ---------------- Public helpers ---------------- +def ensure_cam_state(cam_id: int, *, is_ptz: Optional[bool] = None) -> Dict[str, Any]: + """ + Гарантирует наличие и полноту структуры состояния для cam_id. + Если камера уже есть — дольём недостающие ключи без перетирания. + is_ptz можно подсказать явно; если None — вычислим по config.PTZ_CAM_IDS. + """ + st = ptz_states.get(cam_id) + if st is None: + st = _default_ptz_state(cam_id) + if is_ptz is not None: + st["is_ptz"] = bool(is_ptz) + ptz_states[cam_id] = st + return st + + # Дополним недостающие ключи (на случай старого состояния) + defaults = _default_ptz_state(cam_id) + for k, v in defaults.items(): + if k not in st: + st[k] = v + + # Актуализируем флаг is_ptz, если указан явно + if is_ptz is not None: + st["is_ptz"] = bool(is_ptz) + else: + st["is_ptz"] = bool(cam_id in config.PTZ_CAM_IDS) + + return st diff --git a/ptz_tracker_modular/status.py b/ptz_tracker_modular/status.py new file mode 100644 index 0000000..3bdfd39 --- /dev/null +++ b/ptz_tracker_modular/status.py @@ -0,0 +1,173 @@ +# status.py +from __future__ import annotations + +import time +import asyncio +import logging +from typing import Optional, Tuple, List + +from . import config, state +from .utils import clipf +from .ptz_io import get_ptz_status # ожидает cam_idx: int, возвращает (pan_deg|None, tilt_deg|None) + +logger = logging.getLogger("PTZTracker") + + +# ---------------- Публичные утилиты статуса ---------------- +def get_cached_angles(cam_id: int) -> Tuple[Optional[float], Optional[float], Optional[float]]: + """ + Вернёт (pan_deg, tilt_deg, t_epoch) из кэша, если есть (иначе None). + Кэш наполняется ptz_status_poller(). + """ + cache = getattr(state, "ptz_status", {}).get(cam_id) + if not cache: + return None, None, None + return cache.get("pan"), cache.get("tilt"), cache.get("t") + + +def format_angles_for_log(pan: Optional[float], tilt: Optional[float]) -> str: + if pan is None and tilt is None: + return "pan=NA tilt=NA" + if pan is None: + return f"pan=NA tilt={tilt:.2f}" + if tilt is None: + return f"pan={pan:.2f} tilt=NA" + return f"pan={pan:.2f} tilt={tilt:.2f}" + + +# --------------- Внутренний телеметрический логгер --------------- +def _should_log_telemetry(cam_id: int, now: float, period: float) -> bool: + """ + Простая защита от спама: логируем статус камеры не чаще, чем раз в period секунд. + """ + key = f"_last_log_{cam_id}" + last = getattr(state, key, 0.0) + if now - last >= period: + setattr(state, key, now) + return True + return False + + +def _update_runtime_state(cam_id: int, pan: Optional[float], tilt: Optional[float], now: float) -> None: + """ + Проставляет дубли в state.ptz_states[cam_id] для использования остальным кодом, + включая воркер из ptz_io.py (он читает az_deg!) и постпроцессинг. + + Ставим: + - az_deg (совместимость с ptz_io._ptz_worker) + - pan_deg (то же значение, чтобы везде было одинаково) + - pan_deg_geo (если в конфиге есть north_offset_deg) + - tilt_deg (клип -90..+90) + - last_status_at + """ + st = state.ptz_states.setdefault(cam_id, {}) + cfg = config.CAMERA_CONFIG.get(cam_id, {}) + + if pan is not None: + pan_n = float(pan) % 360.0 + st["az_deg"] = pan_n # <-- это читает ptz_io._ptz_worker при bounded-патруле + st["pan_deg"] = pan_n # удобный синоним + + north_off = cfg.get("north_offset_deg") + if north_off is not None: + try: + st["pan_deg_geo"] = (pan_n + float(north_off)) % 360.0 + except Exception: + st["pan_deg_geo"] = pan_n + + if tilt is not None: + st["tilt_deg"] = clipf(float(tilt), -90.0, 90.0) + + st["last_status_at"] = now + + +# -------------------- Основной поллер статуса --------------------- +async def ptz_status_poller() -> None: + """ + Асинхронный опрос PTZ-камер из config.PTZ_CAM_IDS и публикация в state: + - state.ptz_status[cid] = {"pan": deg|None, "tilt": deg|None, "t": now} + - state.ptz_states[cid]["az_deg"/"pan_deg"/"pan_deg_geo"/"tilt_deg"/"last_status_at"] + + ВАЖНО: частота задаётся как PTZ_STATUS_HZ **на камеру**. + То есть при PTZ_STATUS_HZ=4.0 и 3 камерах каждая опрашивается 4 Гц, а не 4/3. + + Редкое логирование: config.PTZ_STATUS_LOG_EVERY_SEC (>0 чтобы включить). + """ + hz = float(getattr(config, "PTZ_STATUS_HZ", 4.0)) + cam_ids: List[int] = list(getattr(config, "PTZ_CAM_IDS", [])) + n_cams = max(1, len(cam_ids)) + + if not cam_ids: + logger.info("[STATUS] no PTZ cameras to poll (PTZ_CAM_IDS empty)") + return + + # Интервал между подряд идущими запросами к ОДНОЙ камере: + # хотим 'hz' опросов в секунду для каждой камеры. + per_step_interval = 1.0 / max(0.1, hz * n_cams) + + log_period = float(getattr(config, "PTZ_STATUS_LOG_EVERY_SEC", 0.0)) + + if not hasattr(state, "ptz_status"): + state.ptz_status = {} # type: ignore[attr-defined] + + logger.info( + "[STATUS] PTZ status poller: %d cam(s), target %.2f Hz per cam (step_interval=%.3fs)", + len(cam_ids), + hz, + per_step_interval, + ) + + i = 0 + try: + while not getattr(state, "shutdown", False): + cam_id = cam_ids[i % len(cam_ids)] + i += 1 + now = time.time() + + # Сетевой запрос делаем в thread pool, чтобы не блокировать event loop + try: + pan, tilt = await asyncio.to_thread(get_ptz_status, cam_id) + except Exception as e: + logger.warning("[STATUS] cam %d poll error: %s", cam_id, e) + pan = tilt = None + + # Обновляем кэш + cache = getattr(state, "ptz_status", {}).get(cam_id, {}) + cache.update({"pan": pan, "tilt": tilt, "t": now}) + state.ptz_status[cam_id] = cache # type: ignore[attr-defined] + + # Дублируем в ptz_states (для patrol/track/ptz_io и компаса) + _update_runtime_state(cam_id, pan, tilt, now) + + # Редкое логирование (если включено) + if log_period > 0 and _should_log_telemetry(cam_id, now, log_period): + logger.info("[PTZ STATUS] cam %d %s", cam_id, format_angles_for_log(pan, tilt)) + + await asyncio.sleep(per_step_interval) + + except asyncio.CancelledError: + logger.info("[STATUS] PTZ status poller cancelled") + raise + finally: + logger.info("[STATUS] PTZ status poller stopped") + + +# -------------------- Вспомогательные публичные -------------------- +def last_attack_heading_deg(cam_id: int) -> Optional[float]: + """ + Возвращает последний закэшированный attack_heading_deg (если постпроцессинг его писал). + Это используется для логов/отладки, не обязательно для патруля. + """ + st = state.ptz_states.get(cam_id) if hasattr(state, "ptz_states") else None + if not st: + return None + val = st.get("attack_heading_deg") + return float(val) if isinstance(val, (int, float)) else None + + +def get_realtime_angles_for_log(cam_id: int) -> str: + pan, tilt, t = get_cached_angles(cam_id) + if t is None: + return f"cam {cam_id} pan=NA tilt=NA" + age = max(0.0, time.time() - t) + return f"cam {cam_id} {format_angles_for_log(pan, tilt)} (t-{age:.2f}s)" diff --git a/ptz_tracker_modular/streaming.py b/ptz_tracker_modular/streaming.py new file mode 100644 index 0000000..c958cb2 --- /dev/null +++ b/ptz_tracker_modular/streaming.py @@ -0,0 +1,1075 @@ +from __future__ import annotations + +import asyncio +import base64 +import logging +import os +import re +import struct +import time +from typing import Any, Dict, List, Tuple, Optional + +import cv2 +import numpy as np +import websockets +from websockets import serve + +from . import config, state +from . import alarms # <-- используем для нормализации MAC и отправки тревог +from .config import VUE_CONTROL_WS_PORT, VUE_VIDEO_WS_PORT +from .model import yolo_forward +from .postprocess import postprocess_control +from .preview import maybe_downscale, publish_preview +from .utils import json_dumps +from .wiper import trigger_wiper_once # опционально + +# горячая калибровка/патруль +from .calibration import calibrate_presets, autocal_pan_sign +from .patrol import ensure_patrol_resume +from .sector import sector_init_on_startup, sector_autocal_from_presets + +logger = logging.getLogger("PTZTracker.CTRL") +log_main = logging.getLogger("PTZTracker") + +PACKER_HI = struct.Struct(">HI") + +# ---- ALARM bridge state (антидребезг по кадрам) ---- +_alarm_on_since: Dict[int, float] = {} +_alarm_off_since: Dict[int, float] = {} +_alarm_is_on: Dict[int, bool] = {} + +def _alarm_gate(cam_id: int, has_target_now: bool) -> bool | None: + """ + Возвращает True/False когда ПORA отправлять ON/OFF. + Возвращает None — если ждём подтверждения (антидребезг). + Источник has_target_now — state.ptz_states[cam_id]["rec_active"]. + """ + now = time.monotonic() + was_on = _alarm_is_on.get(cam_id, False) + + if has_target_now: + # Кандидат на включение + if not was_on: + t0 = _alarm_on_since.get(cam_id) + if t0 is None: + _alarm_on_since[cam_id] = now + log_main.debug("[ALARM][GATE] cam %s arm start", cam_id) + return None + if (now - t0) >= float(getattr(config, "REC_ON_CONFIRM_SEC", 0.35)): + _alarm_is_on[cam_id] = True + _alarm_off_since.pop(cam_id, None) + _alarm_on_since.pop(cam_id, None) + log_main.info("[ALARM][GATE] cam %s ARMED (≥%.2fs)", cam_id, getattr(config, "REC_ON_CONFIRM_SEC", 0.35)) + return True + return None + else: + # уже включена — подтверждаем присутствие + _alarm_off_since.pop(cam_id, None) + return None + else: + # Кандидат на выключение + if was_on: + t0 = _alarm_off_since.get(cam_id) + if t0 is None: + _alarm_off_since[cam_id] = now + log_main.debug("[ALARM][GATE] cam %s disarm start", cam_id) + return None + if (now - t0) >= float(getattr(config, "REC_OFF_GRACE_SEC", 1.2)): + _alarm_is_on[cam_id] = False + _alarm_on_since.pop(cam_id, None) + _alarm_off_since.pop(cam_id, None) + log_main.info("[ALARM][GATE] cam %s DISARMED (≥%.2fs)", cam_id, getattr(config, "REC_OFF_GRACE_SEC", 1.2)) + return False + return None + else: + # уже выключена + _alarm_on_since.pop(cam_id, None) + return None + +async def alarm_gate_loop(poll_sec: float = 0.05) -> None: + """ + 20 Гц опрос state.ptz_states[cam]['rec_active'] и отправка тревог. + Работает поверх уже существующей логики записи/детекции. + """ + log_main.info("[ALARM][BRIDGE] gate loop started (poll=%.3fs)", poll_sec) + while True: + try: + # Итерация по известным камерам (если приходят новые — они появятся в config.CAMERA_CONFIG) + for cam_id in list(config.CAMERA_CONFIG.keys()): + st = state.ptz_states.get(cam_id, {}) + has_target_now = bool(st.get("rec_active", False)) + + decision = _alarm_gate(cam_id, has_target_now) + if decision is None: + continue + + if decision: + log_main.info("[ALARM][DECIDE] cam %s -> ON (rec_active=True)", cam_id) + alarms.queue_alarm(cam_id, True) # MAC-only/Swagger формат решается в alarms.py + else: + log_main.info("[ALARM][DECIDE] cam %s -> OFF (rec_active=False)", cam_id) + alarms.queue_alarm(cam_id, False) + + except Exception as e: + log_main.error("[ALARM][BRIDGE] loop error: %s", e) + + await asyncio.sleep(poll_sec) + +def _cameras_toml_path() -> str: + for name in ("CAMERAS_TOML_PATH", "CAMERAS_TOML", "CONFIG_TOML"): + p = getattr(config, name, None) + if p: + return str(p) + return os.path.join(os.getcwd(), "cameras.toml") + +def _toml_load(path: str) -> dict: + try: + import tomllib + with open(path, "rb") as f: + return tomllib.load(f) + except Exception: + return {} + +def _next_free_id(used: List[int]) -> int: + cur = 0 + used_set = set(int(x) for x in used) + while cur in used_set: + cur += 1 + return cur + +def _append_camera_block(path: str, block: str) -> None: + if not os.path.exists(path): + open(path, "a", encoding="utf-8").close() + with open(path, "a", encoding="utf-8") as f: + f.write(("\n" if not block.startswith("\n") else "") + block.strip() + "\n") + +def _emit_cam_block_for_toml(c: dict) -> str: + """ + Единый эмиттер TOML-блока камеры с расширенными полями. + Пишет только те optional-поля, которые заданы и валидны по типу. + """ + # обязательные + базовые + lines = [ + "[[camera]]", + f"id = {int(c['id'])}", + f"ip = \"{c['ip']}\"", + f"username = \"{c['username']}\"", + f"password = \"{c['password']}\"", + f"ptz_channel = {int(c.get('ptz_channel', 1))}", + "wiper_channel = 1", + + ] + + # <<< MAC >>> + mac = c.get("mac") + if isinstance(mac, str) and mac: + lines.append(f"mac = \"{mac}\"") + + # PTZ признаки и пресеты + if c.get("is_ptz", False): + lines.append(f"preset1 = \"{c.get('preset1', '1')}\"") + lines.append(f"preset2 = \"{c.get('preset2', '2')}\"") + lines.append(f"sweep_sign = {int(c.get('sweep_sign', 1))}") + + # --- Дополнительные поля --- + opt_float_keys = [ + "north_offset_deg", "bullet_hfov_deg", "hfov_deg", + "preset1_deg", "preset2_deg", + "preset1_tilt_deg", "preset2_tilt_deg", + "sector_min_deg", "sector_max_deg", + "pan_offset_deg", "tilt_offset_deg", + ] + opt_int_keys = ["pan_sign", "tilt_sign", "port", "port_http", "port_https", "ptz_timeout_sec"] + opt_bool_keys = ["https", "verify_tls", "is_ptz"] + opt_str_keys = ["scheme", "focus_override"] + + for k in opt_float_keys: + v = c.get(k) + if isinstance(v, (int, float)): + lines.append(f"{k} = {float(v):.6f}") + for k in opt_int_keys: + v = c.get(k) + if isinstance(v, int): + lines.append(f"{k} = {v}") + for k in opt_bool_keys: + v = c.get(k) + if isinstance(v, bool): + lines.append(f"{k} = {'true' if v else 'false'}") + for k in opt_str_keys: + v = c.get(k) + if isinstance(v, str) and v: + lines.append(f"{k} = \"{v}\"") + + return "\n".join(lines) + "\n" + +def _write_or_update_camera(path: str, cam: dict) -> None: + """ + Добавляет новую камеру в TOML, если такой id ещё нет. + Пишет расширенные поля (север/HFOV/сектор/знаки/оверрайды), только если они заданы. + """ + data = _toml_load(path) + existing = data.get("camera", []) + ids = [c.get("id") for c in existing if isinstance(c, dict)] + if cam["id"] in ids: + return + block = _emit_cam_block_for_toml(cam) + _append_camera_block(path, "\n" + block) + +def _rewrite_all_cameras(path: str, cams: List[dict]) -> None: + """ + Полностью перезаписывает cameras.toml, сохраняя для каждой камеры все полезные поля. + """ + blocks = [] + for cam in sorted(cams, key=lambda c: int(c["id"])): + blocks.append(_emit_cam_block_for_toml(cam)) + text = ("\n".join(b.strip() for b in blocks if b.strip()) + "\n") if blocks else "" + tmp = path + ".tmp" + with open(tmp, "w", encoding="utf-8") as f: + f.write(text) + f.flush() + os.fsync(f.fileno()) + os.replace(tmp, path) + +def _reload_config_best_effort() -> bool: + for name in ("reload_cameras", "reload_config_from_toml", "reload_from_file", "load_cameras"): + fn = getattr(config, name, None) + if callable(fn): + try: + fn() + return True + except Exception as e: + log_main.warning("[CFG] reload failed via %s: %s", name, e) + return False + +# ------------- «Горячее» добавление камер ------------- +def maybe_reload_cameras_when_unknown(idx: int) -> bool: + from . import config as C + if idx in C.CAMERA_CONFIG: + return True + _reload_config_best_effort() + return idx in C.CAMERA_CONFIG + +async def _broadcast_init_to_vue() -> None: + try: + clients = getattr(state, "vue_control_clients", None) + if not clients: + return + ids = list(sorted(config.CAMERA_CONFIG.keys())) + if not ids and getattr(state, "detected_cameras", None): + ids = list(sorted(state.detected_cameras)) + state.last_broadcast_ids = ids[:] + state.index_to_cam_id = {i + 1: cid for i, cid in enumerate(ids)} # 1-based + msg = {"type": "init", "data": [len(ids)] + ids} + raw = json_dumps(msg) + for ws in list(clients): + try: + await ws.send(raw) + except Exception: + pass + except Exception as e: + logger.warning("[CTRL] broadcast init failed: %s", e) + +def ensure_runtime_state_for_camera(cam_id: int) -> None: + st = state.ptz_states.setdefault(cam_id, {}) + defaults = { + "proc_busy": False, + "prev_t": None, "prev_cx": 0.5, "prev_cy": 0.5, + "vx": 0.0, "vy": 0.0, + "w_frac_ema": None, "prev_w_frac": None, "w_growth": 0.0, + "zoom_state": 0, "zoom_last_change": 0.0, + "zoom_int": 0.0, "zoom_prev_cmd": 0.0, "zoom_ramp_start": 0.0, + "last_dx": 0.0, "last_dy": 0.0, "ix": 0.0, "iy": 0.0, + "last_seen": 0.0, "loss_since": None, "last_bbox": None, + "alarm_sent": False, "stable_frames": 0, + "fallback_done": False, "zoom_lock": False, + "lock_candidate_since": None, "last_return_at": 0.0, + "patrol_active": False, "patrol_dir": +1, + "leg_start": 0.0, "leg_end": 0.0, "endpoint_latch": 0, + "is_ptz": (cam_id in config.PTZ_CAM_IDS), + "last_ptz_send": 0.0, + "last_cmd": (0.0, 0.0, 0.0), + "last_cmd_ts": 0.0, + "last_pan": 0.0, "last_tilt": 0.0, "last_zoom": 0.0, + "rec_active": False, "was_zoomed_in": False, + "zoom_reset_timer": None, "preset_timer": None, "preset_target": None, + "ptz_busy_until": 0.0, "pan_sign": 1, "mode": "IDLE", + } + for k, v in defaults.items(): + st.setdefault(k, v) + +# ---- Горячая инициализация PTZ после добавления ---- +async def _hot_setup_ptz(cam_id: int) -> None: + """ + Ждём кадры, калибруем пресеты с ретраями, автокалибруем pan_sign/сектор + и только затем запускаем патруль. + """ + try: + ensure_runtime_state_for_camera(cam_id) + st = state.ptz_states[cam_id] + st["patrol_active"] = False + st["mode"] = "IDLE" + + # 1) дождаться первых кадров (до 3 сек) + t0 = time.time() + while cam_id not in state.detected_cameras and (time.time() - t0) < 3.0: + await asyncio.sleep(0.1) + + # 2) calibrate_presets с 3 попытками + ok = False + for attempt in range(3): + try: + logger.info("Calibrating presets for camera %s (attempt %d)", cam_id, attempt + 1) + ok = await calibrate_presets(cam_id) + if ok: + break + except Exception as e: + logger.warning("calibrate_presets cam %s failed (attempt %d): %s", cam_id, attempt + 1, e) + await asyncio.sleep(0.5) + if not ok: + logger.warning("Failed to calibrate camera %s, using fallback for sector clamp.", cam_id) + + # 3) pan_sign + сектор + try: + await autocal_pan_sign(cam_id) + except Exception as e: + logger.debug("autocal pan_sign cam %s: %s", cam_id, e) + + if getattr(config, "USE_PRESET_EDGES_FOR_SECTOR", False): + try: + await sector_autocal_from_presets() + except Exception as e: + logger.warning("[SECTOR] autocal from presets failed: %s", e) + + try: + sector_init_on_startup() + except Exception: + pass + + # 4) запуск патруля + try: + ensure_patrol_resume(cam_id, delay=1.0) + except Exception as e: + logger.warning("[PATROL] ensure_patrol_resume failed: %s", e) + + except Exception as e: + logger.warning("[HOTCAL] _hot_setup_ptz cam %s failed: %s", cam_id, e) + +# ---------------- VIDEO WS ---------------- +async def video_ws_handler(ws, path=None) -> None: + addr = getattr(ws, "remote_address", None) + logger.info("[VIDEO WS] client connected: %s path=%s", addr, path) + state.video_clients.add(ws) + try: + await ws.wait_closed() + finally: + state.video_clients.discard(ws) + logger.info("[VIDEO WS] client disconnected: %s", addr) + +async def video_broadcaster() -> None: + client_state: Dict[Any, Dict[str, Any]] = {} + min_interval_fast = getattr(config, "VIDEO_MIN_INTERVAL_FAST", 1.0 / 60.0) + min_interval_slow = getattr(config, "VIDEO_MIN_INTERVAL_SLOW", 1.0 / 15.0) + send_slow_sec = 0.02 + buf_soft = 4 * 1024 * 1024 + buf_hard = 8 * 1024 * 1024 + loop_tick = 1.0 / 200.0 + + while True: + try: + if not state.video_clients or not state.latest_jpeg_by_cam: + await asyncio.sleep(loop_tick) + continue + + now = time.perf_counter() + dead: set[Any] = set() + + for ws in list(state.video_clients): + st = client_state.setdefault( + ws, {"last_fid": {}, "min_interval": min_interval_fast, "last_sent": 0.0} + ) + if (now - st["last_sent"]) < st["min_interval"]: + continue + + transport = getattr(ws, "transport", None) + buf_sz = transport.get_write_buffer_size() if transport else 0 + if buf_sz > buf_hard: + try: + await ws.close() + except Exception: + pass + dead.add(ws) + continue + + pending: List[Tuple[int, int, bytes]] = [] + last_fid: Dict[int, int] = st["last_fid"] + for cam_id, (fid, jpeg) in state.latest_jpeg_by_cam.items(): + if last_fid.get(cam_id, -1) != fid: + payload = PACKER_HI.pack(int(cam_id), int(fid)) + jpeg + pending.append((cam_id, fid, payload)) + + if getattr(config, "SEND_DUPLICATES_WHEN_IDLE", False) and not pending: + for cam_id, (fid, jpeg) in state.latest_jpeg_by_cam.items(): + payload = PACKER_HI.pack(int(cam_id), int(fid)) + jpeg + pending.append((cam_id, fid, payload)) + + if not pending: + continue + + max_per_tick = 12 if st["min_interval"] <= (1.0 / 30.0) else 6 + if len(pending) > max_per_tick: + pending.sort(key=lambda x: x[1], reverse=True) + pending = pending[:max_per_tick] + + t0 = time.perf_counter() + try: + send = ws.send + for cam_id, fid, payload in pending: + await send(payload) + last_fid[cam_id] = fid + except Exception: + dead.add(ws) + continue + finally: + st["last_sent"] = time.perf_counter() + + send_time = st["last_sent"] - t0 + if (send_time > send_slow_sec) or (buf_sz > buf_soft): + st["min_interval"] = min(st["min_interval"] * 1.25, min_interval_slow) + else: + st["min_interval"] = max(st["min_interval"] * 0.90, min_interval_fast) + + for d in dead: + state.video_clients.discard(d) + client_state.pop(d, None) + + except Exception as exc: + logger.error("video_broadcaster error: %s", exc) + + await asyncio.sleep(loop_tick) + +# ---------------- CTRL WS ---------------- +def _parse_config_camera_str(s: str) -> Tuple[str, str, str]: + main = s.strip().split()[0] + m = re.match(r"^(?P[^:@\s]+):(?P

[^@\s]+)@(?P[^\s]+)$", main) + if not m: + raise ValueError("Bad config format (expected login:password@ip vendor)") + return m.group("u"), m.group("p"), m.group("ip") + +def _inmem_add_camera(cfg: dict) -> None: + cam_id = int(cfg["id"]) + # базовые поля + rec = { + "ip": cfg["ip"], + "username": cfg["username"], + "password": cfg["password"], + "ptz_channel": int(cfg.get("ptz_channel", 1)), + "wiper_channel": 1, + } + # <<< mac in-memory >>> + if isinstance(cfg.get("mac"), str) and cfg.get("mac"): + rec["mac"] = cfg["mac"] + + # для PTZ сразу сохраняем пресеты/направление — калибровке проще + if cfg.get("is_ptz", False): + rec["preset1"] = str(cfg.get("preset1", "1")) + rec["preset2"] = str(cfg.get("preset2", "2")) + rec["sweep_sign"] = int(cfg.get("sweep_sign", 1)) + config.CAMERA_CONFIG[cam_id] = rec + + if cfg.get("is_ptz", False) and cam_id not in config.PTZ_CAM_IDS: + config.PTZ_CAM_IDS.append(cam_id) + +def _find_ptz_for_same_ip(cam_id: int) -> Optional[int]: + cfg = config.CAMERA_CONFIG.get(cam_id) + if not cfg: + return None + ip = cfg.get("ip") + if not ip: + return None + for pid in config.PTZ_CAM_IDS: + c = config.CAMERA_CONFIG.get(pid) + if c and c.get("ip") == ip: + return pid + for cid, c in config.CAMERA_CONFIG.items(): + if c.get("ip") == ip and int(c.get("ptz_channel", 0)) == 1: + return cid + return None + +def _token_to_cam_id(token: int) -> Optional[int]: + if token in config.CAMERA_CONFIG: + return token + mapping = getattr(state, "index_to_cam_id", None) + if isinstance(mapping, dict) and token in mapping: + return mapping[token] + ids = list(sorted(config.CAMERA_CONFIG.keys())) + if 1 <= token <= len(ids): + return ids[token - 1] + if 0 <= token < len(ids): + return ids[token] + return None + +async def handle_vue_control(ws, path=None) -> None: + import json, asyncio + logger = logging.getLogger("PTZTracker.CTRL") + + async def _direct_wiper_resolved_cam(cam_id: int, sec: float): + import requests + from requests.auth import HTTPDigestAuth + cfg = config.CAMERA_CONFIG[cam_id] + scheme = cfg.get("scheme") or ("https" if cfg.get("https") else "http") + host = cfg["ip"] + port = int(cfg.get("port") or (443 if scheme == "https" else 80)) + ch = int(cfg.get("ptz_channel", 1)) + user = cfg.get("username") or cfg.get("user") or "admin" + pwd = cfg.get("password") or cfg.get("pass") or "" + verify = bool(cfg.get("verify_tls", False)) + timeout = int(cfg.get("ptz_timeout_sec", 6)) + base = f"{scheme}://{host}:{port}" + url = f"{base}/ISAPI/PTZCtrl/channels/{ch}/manualWiper" + auth = HTTPDigestAuth(user, pwd) + def _do(): + s = requests.Session(); s.trust_env = False + r = s.put(url, auth=auth, timeout=timeout, verify=verify) + body = (r.text or "")[:300] + ok = 200 <= r.status_code < 300 + return ok, "manualWiper", r.status_code, body, cam_id + return await asyncio.get_running_loop().run_in_executor(None, _do) + + _wiper_fn = None + try: + from .wiper import trigger_wiper_once as _wiper_fn + logger.info("[CTRL] wiper module loaded") + except Exception as e: + logger.warning("[CTRL] no wiper module, using direct fallback: %s", e) + + def _resolve_cam_for_wiper(token: int) -> Optional[int]: + real_id = _token_to_cam_id(token) + if real_id is None: + return None + if real_id in config.PTZ_CAM_IDS: + return real_id + return _find_ptz_for_same_ip(real_id) + + addr = getattr(ws, "remote_address", None) + logger.info("[CTRL] client connected: %s path=%s", addr, path) + state.vue_control_clients.add(ws) + + try: + streams = getattr(state, "detected_cameras", None) or getattr(state, "streams", None) or getattr(state, "stream_indices", None) or [] + if isinstance(streams, dict): streams = list(streams.keys()) + elif isinstance(streams, set): streams = sorted(streams) + else: streams = list(streams) + init_msg = {"type": "init", "data": [len(streams)] + streams} + try: + await ws.send(json_dumps(init_msg)) + except Exception: + logger.exception("[CTRL] init send failed") + + async for raw in ws: + try: + data = json.loads(raw) + except Exception: + logger.warning("[CTRL] bad json: %r", raw) + continue + + mtype = data.get("type") + payload = data.get("data") or {} + + if mtype == "policy": + auto = data.get("auto") + config.AUTO_STRATEGY = bool(auto) + await ws.send(json_dumps({"type": "policy_ack", "auto": config.AUTO_STRATEGY})) + continue + + if mtype == "numberCamera": + try: + cnum = payload.get("canvasId") + if isinstance(cnum, str): cnum = int(cnum.strip()) + elif isinstance(cnum, float): cnum = int(cnum) + await ws.send(json_dumps({"type": "numberCamera_ack", "ok": True, "canvasId": cnum})) + except Exception as e: + await ws.send(json_dumps({"type": "numberCamera_ack", "ok": False, "error": str(e)})) + continue + + if mtype == "configCamera": + try: + cfg_str = str(payload.get("config", "")).strip() + is_ptz = bool(payload.get("ptz", False)) + # <<< MAC из клиента >>> + mac_raw = (payload.get("mac") or "").strip() if isinstance(payload.get("mac"), str) else "" + user, pwd, ip = _parse_config_camera_str(cfg_str) + + path = _cameras_toml_path() + data_toml = _toml_load(path) + current = data_toml.get("camera", []) + used_ids = [int(c.get("id")) for c in current if isinstance(c, dict) and "id" in c] + new_id = _next_free_id(used_ids) + ptz_ch = 1 if is_ptz else 2 + + cam_rec = { + "id": new_id, + "ip": ip, + "username": user, + "password": pwd, + "ptz_channel": ptz_ch, + "is_ptz": is_ptz, + } + + # нормализуем и сохраняем MAC, если передан + if mac_raw: + try: + cam_rec["mac"] = alarms._norm_mac(mac_raw) + except Exception: + await ws.send(json_dumps({"type": "configCamera_ack", "ok": False, "error": "bad_mac"})) + continue + + _write_or_update_camera(path, cam_rec) + logger.info("[CFG] appended camera to %s: %s", path, cam_rec) + + _inmem_add_camera(cam_rec) + ensure_runtime_state_for_camera(new_id) + + # Не запускаем патруль напрямую — делаем горячую калибровку в фоне + if is_ptz: + asyncio.create_task(_hot_setup_ptz(new_id)) + + await _broadcast_init_to_vue() + await ws.send(json_dumps({ + "type": "configCamera_ack", "ok": True, "id": new_id, "mac": cam_rec.get("mac") + })) + except Exception as e: + logger.exception("[CTRL] configCamera error: %s", e) + try: + await ws.send(json_dumps({"type": "configCamera_ack", "ok": False, "error": str(e)})) + except Exception: + pass + continue + + if mtype == "deleteCamera": + try: + token = payload.get("canvasId") + if token is None: + token = payload.get("CameraID") + if token is None: + raise ValueError("canvasId is required") + token_int = int(token) if not isinstance(token, int) else token + + real_id = _token_to_cam_id(token_int) + if real_id is None or real_id not in config.CAMERA_CONFIG: + await ws.send(json_dumps({"type": "deleteCamera_ack", "ok": False, "error": "unknown_canvasId"})) + continue + + logger.info("[DELETE] canvasId=%s -> cam_id=%s", token_int, real_id) + + # останов патруля/таймеров/PTZ + from .ptz_io import stop_ptz # безопасно + try: + stop_ptz(real_id) + except Exception: + pass + st = state.ptz_states.get(real_id) + if st: + st["patrol_active"] = False + st["endpoint_latch"] = 0 + st["mode"] = "IDLE" + for t in ("zoom_reset_timer", "preset_timer"): + try: + if st.get(t): + st[t].cancel() + except Exception: + pass + try: + from . import ptz_io as _ptz_mod + if hasattr(_ptz_mod, "PTZ_AUTH"): + _ptz_mod.PTZ_AUTH.pop(real_id, None) + except Exception: + pass + + # вычищаем рантайм и конфиг (in-memory) + was_ptz = (real_id in config.PTZ_CAM_IDS) + config.CAMERA_CONFIG.pop(real_id, None) + if was_ptz: + try: + config.PTZ_CAM_IDS.remove(real_id) + except ValueError: + pass + state.ptz_states.pop(real_id, None) + state.frame_id_by_cam.pop(real_id, None) + state.latest_jpeg_by_cam.pop(real_id, None) + state.detected_cameras.discard(real_id) + + # переписываем cameras.toml из текущего in-memory (с расшир. полями) + path = _cameras_toml_path() + cams_left: List[dict] = [] + for cid in sorted(config.CAMERA_CONFIG.keys()): + cfg = config.CAMERA_CONFIG[cid] + rec = { + "id": cid, + "ip": cfg["ip"], + "username": cfg["username"], + "password": cfg["password"], + "ptz_channel": int(cfg.get("ptz_channel", 1)), + "is_ptz": (cid in config.PTZ_CAM_IDS), + } + # переносим MAC, если есть + if isinstance(cfg.get("mac"), str) and cfg.get("mac"): + rec["mac"] = cfg["mac"] + if rec["is_ptz"]: + rec["preset1"] = str(cfg.get("preset1", "1")) + rec["preset2"] = str(cfg.get("preset2", "2")) + rec["sweep_sign"] = int(cfg.get("sweep_sign", 1)) + + # скопируем любые расширенные поля, если есть + for k in ( + "north_offset_deg","bullet_hfov_deg","hfov_deg", + "preset1_deg","preset2_deg","preset1_tilt_deg","preset2_tilt_deg", + "sector_min_deg","sector_max_deg","pan_offset_deg","tilt_offset_deg", + "pan_sign","tilt_sign","port","port_http","port_https", + "ptz_timeout_sec","https","verify_tls","scheme","focus_override", + ): + if k in cfg: + rec[k] = cfg[k] + + cams_left.append(rec) + + _rewrite_all_cameras(path, cams_left) + logger.info("[DELETE] cameras.toml rewritten at %s (left=%d)", path, len(cams_left)) + + _reload_config_best_effort() + await _broadcast_init_to_vue() + await ws.send(json_dumps({"type": "deleteCamera_ack", "ok": True, "id": real_id})) + except Exception as e: + logger.exception("[CTRL] deleteCamera error: %s", e) + try: + await ws.send(json_dumps({"type": "deleteCamera_ack", "ok": False, "error": str(e)})) + except Exception: + pass + continue + + if mtype == "wiper": + try: + cam_token_raw = payload.get("cam") + sec = float(payload.get("sec", 3)) + cam_token = int(cam_token_raw) if not isinstance(cam_token_raw, int) else cam_token_raw + logger.info("[CTRL] WIPER <- cam_token=%s sec=%.2f", cam_token, sec) + + cam_id = _resolve_cam_for_wiper(cam_token) + if cam_id is None: + ack = {"type": "wiper_ack", "ok": False, "cam": cam_token, "error": "no_ptz_for_camera"} + await ws.send(json_dumps(ack)) + continue + + if _wiper_fn is not None: + ok, endpoint, http_status, detail, _cam_back = await _wiper_fn(cam_id, sec) + else: + ok, endpoint, http_status, detail, _cam_back = await _direct_wiper_resolved_cam(cam_id, sec) + + ack = { + "type": "wiper_ack", + "ok": bool(ok), + "cam": cam_token, + "cam_id": cam_id, + "sec": int(round(sec)), + "endpoint": endpoint, + "status": http_status, + "detail": (detail[:200] if isinstance(detail, str) else detail), + } + logger.info("[CTRL] WIPER -> %s", ack) + await ws.send(json_dumps(ack)) + except Exception as e: + logger.exception("[CTRL] WIPER error: %s", e) + try: + await ws.send(json_dumps({"type": "wiper_ack", "ok": False, "error": str(e)})) + except Exception: + pass + continue + + except Exception as e: + logger.warning("[CTRL] ws loop error: %s", e) + finally: + state.vue_control_clients.discard(ws) + logger.info("[CTRL] client disconnected: %s", addr) + +# ------------- Микро-батчер инференса ------------- +class MicroBatcher: + __slots__ = ("max_batch","max_wait","_buf","_cond","_task","adaptive","bmin","bmax","target_ms","ema_ms") + + def __init__( + self, + max_batch: int = 12, + max_wait_ms: float = 2.0, + adaptive: bool = config.ADAPTIVE_BATCH_ENABLE, + bmin: int = config.BATCH_MIN, + bmax: int = config.BATCH_MAX, + target_ms: float = config.BATCH_TARGET_MS, + ) -> None: + self.max_batch = max(1, int(max_batch)) + self._buf: List[Tuple[int, np.ndarray, bool, asyncio.Future]] = [] + self._cond = asyncio.Condition() + self.max_wait = max_wait_ms / 1000.0 + self._task = asyncio.create_task(self._loop()) + self.adaptive = adaptive + self.bmin = max(1, int(bmin)) + self.bmax = max(self.bmin, int(bmax)) + self.target_ms = float(target_ms) + self.ema_ms: float | None = None + + async def submit(self, cam_idx: int, frame_bgr: np.ndarray, is_ptz: bool) -> None: + loop = asyncio.get_running_loop() + fut: asyncio.Future = loop.create_future() + async with self._cond: + self._buf.append((cam_idx, frame_bgr, is_ptz, fut)) + self._cond.notify() + return await fut + + async def _loop(self) -> None: + run = yolo_forward + while True: + async with self._cond: + while not self._buf: + await self._cond.wait() + + start_wait = time.perf_counter() + while len(self._buf) < self.max_batch: + remaining = self.max_wait - (time.perf_counter() - start_wait) + if remaining <= 0: + break + try: + await asyncio.wait_for(self._cond.wait(), timeout=remaining) + except asyncio.TimeoutError: + break + + batch = self._buf[: self.max_batch] + del self._buf[: self.max_batch] + + try: + frames = [b[1] for b in batch] + loop = asyncio.get_running_loop() + + infer_t0 = time.perf_counter() + results = await loop.run_in_executor(None, run, frames) + infer_ms = (time.perf_counter() - infer_t0) * 1000.0 + self.ema_ms = infer_ms if self.ema_ms is None else (0.8 * self.ema_ms + 0.2 * infer_ms) + + if self.adaptive: + if self.ema_ms < self.target_ms and len(self._buf) > 0 and self.max_batch < self.bmax: + self.max_batch += 1 + elif self.ema_ms > self.target_ms * config.BATCH_DOWN_SCALE and self.max_batch > self.bmin: + self.max_batch -= 1 + + post_out = await asyncio.gather( + *[ + loop.run_in_executor( + None, postprocess_control, cam_idx, frame_bgr, res, is_ptz + ) + for (cam_idx, frame_bgr, is_ptz, _), res in zip(batch, results) + ], + return_exceptions=True, + ) + + for (_, _, _, fut), po in zip(batch, post_out): + if isinstance(po, Exception): + logger.error("postprocess error: %s", po) + if not fut.done(): + fut.set_result(None) + + except Exception as exc: + logger.error("batch forward error: %s", exc) + for _, _, _, fut in batch: + if not fut.done(): + fut.set_result(None) + +# ------------- Поток кадров от C++ ------------- +async def cpp_ws_loop() -> None: + frames_total = 0 + skipped_total = 0 + last_log = time.time() + + while True: + try: + logger.info("[CPP WS] connecting to %s ...", config.CPP_WS_URI) + async with websockets.connect( + config.CPP_WS_URI, + max_size=None, + compression=None, + ping_interval=None, + close_timeout=1.0, + ) as ws: + logger.info("[CPP WS] connected]") + async for raw in ws: + if isinstance(raw, (bytes, bytearray)): + buf = memoryview(raw) + if len(buf) < 3 or buf[0] != 1: + continue + idx = int.from_bytes(buf[1:3], "big") + known_before = idx in config.CAMERA_CONFIG + if not known_before: + maybe_reload_cameras_when_unknown(idx) + arr = np.frombuffer(buf[3:], dtype=np.uint8) + img = cv2.imdecode(arr, cv2.IMREAD_COLOR) + if img is None: + continue + else: + try: + import json as _json + msg = _json.loads(raw) + except Exception: + continue + if msg.get("type") != "image": + continue + idx_str, b64 = msg["data"].split("|", 1) + idx = int(idx_str) + known_before = idx in config.CAMERA_CONFIG + if not known_before: + maybe_reload_cameras_when_unknown(idx) + arr = np.frombuffer(base64.b64decode(b64), dtype=np.uint8) + img = cv2.imdecode(arr, cv2.IMREAD_COLOR) + if img is None: + continue + + frames_total += 1 + h, w = img.shape[:2] + + now = time.time() + if now - last_log >= 1.0: + logger.info( + "[CPP WS] recv: %d fps, skipped=%d | last cam=%s | %dx%d | clients=%d", + frames_total, skipped_total, idx, w, h, len(state.video_clients), + ) + frames_total = 0 + skipped_total = 0 + last_log = now + + first_seen = idx not in state.detected_cameras + state.detected_cameras.add(idx) + + ensure_runtime_state_for_camera(idx) + + if first_seen or (not known_before and idx in config.CAMERA_CONFIG): + try: + await _broadcast_init_to_vue() + except Exception as e: + logger.warning("[CPP WS] broadcast init failed: %s", e) + + busy = state.ptz_states[idx]["proc_busy"] + no_clients = getattr(config, "PUBLISH_ONLY_IF_CLIENTS", False) and not state.video_clients + if getattr(config, "DROP_DECODE_WHEN_BUSY", False) and busy and no_clients: + skipped_total += 1 + continue + + if ( + getattr(config, "PUBLISH_RAW_BEFORE_INFER", False) + and (not getattr(config, "PUBLISH_ONLY_IF_CLIENTS", False) or state.video_clients) + ): + down, _, _ = maybe_downscale(img, getattr(config, "PREVIEW_TARGET_W", 640)) + publish_preview(idx, down) + + pst = state.ptz_states[idx] + if pst["proc_busy"]: + continue + pst["proc_busy"] = True + + async def process_frame(cam_idx: int, frame) -> None: + try: + await state.batcher.submit( + cam_idx, + frame, + cam_idx in config.PTZ_CAM_IDS, # type: ignore[arg-type] + ) + except Exception as exc: + logger.error("Error processing frame for camera %s: %s", cam_idx, exc) + finally: + state.ptz_states[cam_idx]["proc_busy"] = False + + asyncio.create_task(process_frame(idx, img)) + + except Exception as exc: + logger.error("[CPP WS] error: %s", exc) + await asyncio.sleep(0.5) + +# ------------- Управляющий канал C++ (детекции) ------------- +async def cpp_detection_loop() -> None: + assert state.detection_queue is not None + RESYNC_EVERY_SEC = 10.0 + + def _snapshot_states() -> List[Dict[str, Any]]: + snap: List[Dict[str, Any]] = [] + for cid in config.CAMERA_CONFIG.keys(): + st = state.ptz_states.get(cid, {}) + snap.append({"IdCamera": int(cid), "detection": bool(st.get("rec_active", False))}) + return snap + + while True: + try: + logger.info("[CPP CTRL] connecting to %s ...", config.CPP_CTRL_WS_URI) + async with websockets.connect( + config.CPP_CTRL_WS_URI, + max_size=None, + compression=None, + ping_interval=None, + close_timeout=1.0, + ) as ws: + logger.info("[CPP CTRL] connected") + + try: + for item in _snapshot_states(): + await ws.send(json_dumps({"type": "detection", "data": item})) + except Exception as e: + logger.error("[CPP CTRL] initial resync failed: %s", e) + raise + + last_resync = time.time() + while True: + now = time.time() + + if now - last_resync >= RESYNC_EVERY_SEC and state.detection_queue.empty(): + try: + for item in _snapshot_states(): + await ws.send(json_dumps({"type": "detection", "data": item})) + except Exception as e: + logger.error("[CPP CTRL] periodic resync failed: %s", e) + break + last_resync = now + + try: + payload = await asyncio.wait_for(state.detection_queue.get(), timeout=1.0) + except asyncio.TimeoutError: + continue + + try: + await ws.send(json_dumps(payload)) + last_resync = time.time() + except Exception as exc: + logger.error("[CPP CTRL] send failed: %s", exc) + try: + state.detection_queue.put_nowait(payload) + except Exception: + pass + break + finally: + state.detection_queue.task_done() + + except Exception as exc: + logger.error("[CPP CTRL] error: %s", exc) + + await asyncio.sleep(1.0) + +# ------------- Старт WS-серверов ------------- +async def build_servers(): + ctrl_server = await serve( + handle_vue_control, + "0.0.0.0", + VUE_CONTROL_WS_PORT, + max_size=None, + ping_interval=None, + compression=None, + ) + video_server = await serve( + video_ws_handler, + "0.0.0.0", + VUE_VIDEO_WS_PORT, + max_size=None, + ping_interval=None, + compression=None, + ) + return ctrl_server, video_server diff --git a/ptz_tracker_modular/toml_persist.py b/ptz_tracker_modular/toml_persist.py new file mode 100644 index 0000000..bad2134 --- /dev/null +++ b/ptz_tracker_modular/toml_persist.py @@ -0,0 +1,426 @@ +# toml_persist.py +from __future__ import annotations +import os +import time +import logging +from pathlib import Path +from typing import Any, Dict, List, Optional, Tuple + +from . import config +from .geom import ang_diff_signed, normalize360 +from .ptz_io import read_ptz_azimuth_deg, goto_preset_token + +logger = logging.getLogger("PTZTracker.PERSIST") + +def _cameras_toml_path() -> Path: + """ + Должны писать в тот же файл, который читает config.reload_cameras(). + Приоритет: + 1) env CAMERAS_FILE + 2) cameras.toml рядом с config.py + """ + env = os.getenv("CAMERAS_FILE") + if env: + return Path(env).resolve() + + # рядом с config.py (и тем самым совместимо с config._load_cameras_from_file) + cfg_dir = Path(getattr(config, "__file__", __file__)).resolve().parent + return (cfg_dir / "cameras.toml").resolve() + + +def _load_toml(path: Path) -> Dict[str, Any]: + if not path.exists(): + return {} + try: + import tomllib + with open(path, "rb") as f: + return tomllib.load(f) + except Exception as e: + logger.error("[PERSIST] TOML read failed: %s", e) + return {} + + +def _atomic_write(path: Path, text: str) -> None: + tmp = path.with_suffix(path.suffix + ".tmp") + with open(tmp, "w", encoding="utf-8") as f: + f.write(text) + f.flush() + os.fsync(f.fileno()) + os.replace(tmp, path) + + +def _to_float(val: Any) -> Optional[float]: + try: + return float(val) + except Exception: + return None + + +def _render_cameras_table(cams: List[Dict[str, Any]]) -> str: + lines: List[str] = [] + for c in sorted(cams, key=lambda x: int(x.get("id", 0))): + lines.append("[[camera]]") + lines.append(f"id = {int(c['id'])}") + lines.append(f"ip = \"{c.get('ip','')}\"") + lines.append(f"username = \"{c.get('username','')}\"") + lines.append(f"password = \"{c.get('password','')}\"") + lines.append(f"ptz_channel = {int(c.get('ptz_channel', 1))}") + if "wiper_channel" in c: + lines.append(f"wiper_channel = {int(c.get('wiper_channel', 1))}") + if "preset1" in c and c.get("preset1") not in (None, "None"): + lines.append(f"preset1 = \"{c.get('preset1')}\"") + if "preset2" in c and c.get("preset2") not in (None, "None"): + lines.append(f"preset2 = \"{c.get('preset2')}\"") + if "sweep_sign" in c: + lines.append(f"sweep_sign = {int(c.get('sweep_sign', 1))}") + if "is_ptz" in c: + lines.append(f"is_ptz = {str(bool(c.get('is_ptz'))).lower()}") + if "mac" in c and isinstance(c["mac"], str): + lines.append(f"mac = \"{c['mac']}\"") + + for k in ("north_offset_deg", "preset1_ref_geo_deg", "preset2_ref_geo_deg"): + if isinstance(c.get(k), (int, float)): + lines.append(f"{k} = {float(c[k]):.6f}") + + for k in ("preset1_deg_geo", "preset2_deg_geo", "sector_min_deg_geo", "sector_max_deg_geo"): + if isinstance(c.get(k), (int, float)): + lines.append(f"{k} = {float(c[k]):.6f}") + + for k in ("preset1_deg", "preset2_deg", "preset1_tilt_deg", "preset2_tilt_deg", "sector_min_deg", "sector_max_deg"): + if isinstance(c.get(k), (int, float)): + lines.append(f"{k} = {float(c[k]):.6f}") + + for k in ("north_last_source", "geo_last_source"): + if isinstance(c.get(k), str): + lines.append(f"{k} = \"{c[k]}\"") + for k in ("north_last_at", "geo_last_at"): + if isinstance(c.get(k), int): + lines.append(f"{k} = {c[k]}") + + lines.append("") + return "\n".join(lines).strip() + "\n" + + +def save_all_cameras() -> None: + path = _cameras_toml_path() + cams = list(config.CAMERA_CONFIG.values()) + text = _render_cameras_table(cams) + try: + _atomic_write(path, text) + logger.info("[PERSIST] cameras.toml saved (%d cameras)", len(cams)) + try: + config.reload_cameras() + except Exception as e: + logger.warning("[PERSIST] reload_cameras failed: %s", e) + except Exception as e: + logger.error("[PERSIST] write failed: %s", e) + +# ---------- низкоуровневая запись одного поля ---------- +def _persist_field(cam_id: int, key: str, value) -> bool: + """ + Записать камере поле key=value в cameras.toml. True, если действительно изменилось. + """ + path = _cameras_toml_path() + data = _load_toml(path) + cams = data.get("camera") or data.get("cameras") or [] + if not isinstance(cams, list): + logger.error("[PERSIST] cameras.toml format unexpected: no [[camera]] array") + return False + + changed = False + found = False + for c in cams: + if not isinstance(c, dict) or "id" not in c: + continue + if int(c["id"]) == int(cam_id): + found = True + before = c.get(key) + if before != value: + c[key] = value + changed = True + break + + if not found or not changed: + return False + + text = _render_cameras_table(cams) + try: + _atomic_write(path, text) + try: + config.reload_cameras() + except Exception as e: + logger.warning("[PERSIST] reload_cameras failed: %s", e) + return True + except Exception as e: + logger.error("[PERSIST] write failed: %s", e) + return False + +# ---------- публичный API: north_offset ---------- +def persist_north_offset(cam_id: int, north_deg: float, source: str = "unknown") -> bool: + """ + Обновляет/добавляет north_offset_deg; ставит метаданные; перезагружает config. + """ + path = _cameras_toml_path() + data = _load_toml(path) + cams = data.get("camera") or data.get("cameras") or [] + if not isinstance(cams, list): + logger.error("[PERSIST] cameras.toml format unexpected: no [[camera]] array") + return False + + north_deg = normalize360(float(north_deg)) + changed = False + found = False + for c in cams: + if not isinstance(c, dict) or "id" not in c: + continue + if int(c["id"]) == int(cam_id): + found = True + old = _to_float(c.get("north_offset_deg")) + if old is None or abs(old - north_deg) > 1e-6: + c["north_offset_deg"] = float(north_deg) + c["north_last_source"] = source + c["north_last_at"] = int(time.time()) + changed = True + break + + if not found: + logger.warning("[PERSIST] camera id=%s not found in cameras.toml; will not add a new block", cam_id) + return False + if not changed: + return False + + text = _render_cameras_table(cams) + try: + _atomic_write(path, text) + logger.info("[PERSIST] cameras.toml updated (cam %s north=%.2f° via %s)", cam_id, north_deg, source) + try: + config.reload_cameras() + except Exception as e: + logger.warning("[PERSIST] reload_cameras failed: %s", e) + return True + except Exception as e: + logger.error("[PERSIST] write failed: %s", e) + return False + +# ---------- публичный API: массовая запись geo-полей ---------- +def persist_geo_fields(cam_id: int, fields: Dict[str, float], source: str = "unknown") -> bool: + path = _cameras_toml_path() + data = _load_toml(path) + cams = data.get("camera") or data.get("cameras") or [] + if not isinstance(cams, list): + logger.error("[PERSIST] cameras.toml format unexpected: no [[camera]] array") + return False + + changed = False + found = False + for c in cams: + if not isinstance(c, dict) or "id" not in c: + continue + if int(c["id"]) == int(cam_id): + found = True + for k, v in fields.items(): + if not isinstance(v, (int, float)): + continue + vv = normalize360(float(v)) if k == "north_offset_deg" else float(v) + old = _to_float(c.get(k)) + if old is None or abs(old - vv) > 1e-6: + c[k] = vv + changed = True + if changed: + c["geo_last_source"] = source + c["geo_last_at"] = int(time.time()) + break + + if not found or not changed: + return False + + text = _render_cameras_table(cams) + try: + _atomic_write(path, text) + logger.info("[PERSIST] cameras.toml updated (cam %s fields=%s via %s)", cam_id, list(fields.keys()), source) + try: + config.reload_cameras() + except Exception as e: + logger.warning("[PERSIST] reload_cameras failed: %s", e) + return True + except Exception as e: + logger.error("[PERSIST] write failed: %s", e) + return False + +# ---------- публичный API: preset1_ref_geo_deg ---------- +def persist_preset1_ref_geo(cam_id: int, ref_geo_deg: float, source: str = "auto") -> bool: + """ + Записывает preset1_ref_geo_deg (если изменился) + reload. + """ + ref_geo_deg = normalize360(float(ref_geo_deg)) + ok = _persist_field(cam_id, "preset1_ref_geo_deg", ref_geo_deg) + if ok: + logger.info("[PERSIST] cam %s preset1_ref_geo_deg=%.2f (%s)", cam_id, ref_geo_deg, source) + return ok + +# ---------- утилиты чтения статуса ---------- +async def _wait_az_stable(cam: int, timeout: float = 2.5, tol_deg: float = 0.3) -> Optional[float]: + """ + Ждём пока pan успокоится (изменение <= tol_deg). Возвращаем pan (0..360) или None. + """ + import asyncio + loop = asyncio.get_event_loop() + t0 = loop.time() + last = None + stable = 0 + while loop.time() - t0 < timeout: + a = read_ptz_azimuth_deg(cam) + if a is None: + await asyncio.sleep(0.12) + continue + a = normalize360(a) + # подписанная разница + diff = (a - (last if last is not None else a) + 540.0) % 360.0 - 180.0 + if last is None or abs(diff) > tol_deg: + stable = 0 + else: + stable += 1 + if stable >= 1: + return a + last = a + await asyncio.sleep(0.12) + return last + +# ---------- авторасчёт и запись preset1_ref_geo_deg (от самого PTZ) ---------- +async def compute_and_persist_preset1_ref_geo(cam_id: int) -> Optional[float]: + """ + Если у камеры задан `north_offset_deg` и есть `preset1`, + вычисляет preset1_ref_geo_deg = normalize360(pan_on_preset1 + north_offset_deg), + сохраняет в cameras.toml и возвращает значение. + """ + cfg = config.CAMERA_CONFIG.get(cam_id, {}) + north = cfg.get("north_offset_deg") + p1 = cfg.get("preset1") + if not isinstance(north, (int, float)) or not p1: + logger.info("[PERSIST] cam %s: cannot compute preset1_ref_geo_deg (north or preset1 missing)", cam_id) + return None + + try: + goto_preset_token(cam_id, str(p1)) + import asyncio + await asyncio.sleep(0.6) + pan = await _wait_az_stable(cam_id) + if pan is None: + logger.warning("[PERSIST] cam %s: cannot read stable pan on preset1", cam_id) + return None + + ref_geo = normalize360(float(pan) + float(north)) + + changed = persist_geo_fields( + cam_id, + {"preset1_ref_geo_deg": ref_geo}, + source="compute_and_persist_preset1_ref_geo", + ) + if changed: + logger.info("[PERSIST] cam %s: preset1_ref_geo_deg=%.2f written to cameras.toml", cam_id, ref_geo) + else: + logger.info("[PERSIST] cam %s: preset1_ref_geo_deg already up-to-date (%.2f)", cam_id, ref_geo) + + # обновим рантайм для удобства + cfg["preset1_ref_geo_deg"] = ref_geo + cfg["preset1_deg_geo"] = ref_geo + if isinstance(cfg.get("preset2_deg"), (int, float)): + cfg["preset2_deg_geo"] = normalize360(cfg["preset2_deg"] + float(north)) + span_geo = ang_diff_signed(cfg["preset2_deg_geo"], cfg["preset1_deg_geo"]) + if span_geo >= 0: + cfg["sector_min_deg_geo"] = normalize360(cfg["preset1_deg_geo"] - config.SECTOR_MARGIN_DEG) + cfg["sector_max_deg_geo"] = normalize360(cfg["preset2_deg_geo"] + config.SECTOR_MARGIN_DEG) + else: + cfg["sector_min_deg_geo"] = normalize360(cfg["preset2_deg_geo"] - config.SECTOR_MARGIN_DEG) + cfg["sector_max_deg_geo"] = normalize360(cfg["preset1_deg_geo"] + config.SECTOR_MARGIN_DEG) + _persist_field(cam_id, "preset1_deg_geo", float(cfg["preset1_deg_geo"])) + _persist_field(cam_id, "preset2_deg_geo", float(cfg["preset2_deg_geo"])) + _persist_field(cam_id, "sector_min_deg_geo", float(cfg["sector_min_deg_geo"])) + _persist_field(cam_id, "sector_max_deg_geo", float(cfg["sector_max_deg_geo"])) + + return ref_geo + except Exception as e: + logger.warning("[PERSIST] cam %s: compute_and_persist_preset1_ref_geo failed: %s", cam_id, e) + return None + +# ---------- авторасчёт рефов от кромок панорамы и запись ---------- +def _pano_edges_geo(pano_id: int) -> Optional[Tuple[float, float, float]]: + """ + Возвращает (center_geo, left_geo, right_geo) для панорамы. + Требует: north_offset_deg (пано) и bullet_hfov_deg/hfov_deg (пано). + center_geo берём из runtime, если там есть pan_deg; иначе считаем центр по north_offset. + """ + pcfg = config.CAMERA_CONFIG.get(pano_id, {}) + north = pcfg.get("north_offset_deg") + hfov = pcfg.get("bullet_hfov_deg") or pcfg.get("hfov_deg") or 90.0 + if not isinstance(north, (int, float)): + return None + pan_abs = None + try: + from . import state + st = getattr(state, "ptz_states", {}).get(pano_id) or {} + pan_abs = st.get("pan_deg") + except Exception: + pan_abs = None + if pan_abs is None: + pan_abs = 0.0 + center_geo = normalize360(float(pan_abs) + float(north)) + left_geo = normalize360(center_geo - float(hfov) * 0.5) + right_geo = normalize360(center_geo + float(hfov) * 0.5) + return center_geo, left_geo, right_geo + +async def compute_ref_from_pano_edges_and_persist(ptz_id: int, bind: str = "preset1=left,preset2=right") -> Optional[Tuple[float, float]]: + """ + Находит «спаренную» панораму (тот же IP, channel=2), берёт её north_offset_deg и hfov, + вычисляет гео-азимуты левой/правой кромки и сохраняет в cameras.toml: + preset1_ref_geo_deg и preset2_ref_geo_deg. + bind: схема привязки (default: preset1->left, preset2->right). + Возвращает (p1_ref, p2_ref) или None. + """ + # 1) попытка найти панораму по PTZ_TO_PAN + pano_id = config.PTZ_TO_PAN.get(ptz_id) + if pano_id is None: + # резервный поиск по IP/каналу + cfg = config.CAMERA_CONFIG.get(ptz_id, {}) + ip = cfg.get("ip") + if ip: + for cid, c in config.CAMERA_CONFIG.items(): + if c.get("ip") == ip and int(c.get("ptz_channel", 0)) == 2: + pano_id = cid + break + if pano_id is None: + logger.info("[PERSIST] cam %s: paired panorama not found", ptz_id) + return None + + edges = _pano_edges_geo(pano_id) + if edges is None: + logger.info("[PERSIST] cam %s: panorama %s has no north_offset/hfov — cannot compute", ptz_id, pano_id) + return None + _, left_geo, right_geo = edges + + ptz_cfg = config.CAMERA_CONFIG.get(ptz_id, {}) + p1 = ptz_cfg.get("preset1"); p2 = ptz_cfg.get("preset2") + if not (p1 and p2): + logger.info("[PERSIST] cam %s: no preset1/preset2 — skip", ptz_id) + return None + + b = bind.lower().replace(" ", "") + if "preset1=right" in b: + p1_ref, p2_ref = right_geo, left_geo + else: + p1_ref, p2_ref = left_geo, right_geo + + fields = { + "preset1_ref_geo_deg": float(p1_ref), + "preset2_ref_geo_deg": float(p2_ref), + } + changed = persist_geo_fields(ptz_id, fields, source="pano_edges_autobind") + if changed: + logger.info("[PERSIST] cam %s: wrote %s into cameras.toml", ptz_id, list(fields.keys())) + try: + ptz_cfg.update(fields) # обновим рантайм + except Exception: + pass + else: + logger.info("[PERSIST] cam %s: %s already up-to-date", ptz_id, list(fields.keys())) + return p1_ref, p2_ref diff --git a/ptz_tracker_modular/utils.py b/ptz_tracker_modular/utils.py new file mode 100644 index 0000000..47cb3d0 --- /dev/null +++ b/ptz_tracker_modular/utils.py @@ -0,0 +1,75 @@ +from __future__ import annotations +import logging +from math import isfinite + +# configure module logger (app sets handlers/level) +logger = logging.getLogger("PTZTracker") + +# ---------- fast json ---------- +try: + import orjson + def json_dumps(obj) -> str: + return orjson.dumps(obj).decode("utf-8") + json_loads = orjson.loads +except Exception: + import json + def json_dumps(obj) -> str: + return json.dumps(obj, separators=(",", ":"), ensure_ascii=False) + json_loads = json.loads + +# ---------- math helpers ---------- +def clipf(v: float, lo: float, hi: float) -> float: + return lo if v < lo else hi if v > hi else v + +def quantf(v: float, q: float) -> float: + return round(v / q) * q + +# ---------- angle / compass helpers ---------- +def wrap_deg(a: float) -> float: + """[-180, +180)""" + return (a + 180.0) % 360.0 - 180.0 + +def norm360(a: float) -> float: + """[0, 360)""" + return a % 360.0 + +def shortest_delta_deg(a: float, b: float) -> float: + """(b - a) в [-180, +180)""" + return wrap_deg(b - a) + +# --- компас 8 румбов (как на картинке) --- +def deg_to_compass(deg: float) -> str: + labels_ru_8 = ["С", "СВ", "В", "ЮВ", "Ю", "ЮЗ", "З", "СЗ"] + idx = int((norm360(deg) + 22.5) // 45) % 8 + return labels_ru_8[idx] + +def format_bearing_ru(deg: float) -> str: + return f"{deg_to_compass(deg)} ({norm360(deg):.1f}°)" + +# ---------- optics helpers ---------- +def hfov_ptz_from_abs_zoom(abs_zoom: float, + zmin: float = 0.0, zmax: float = 100.0, + hfov_wide: float = 59.0, + hfov_tele: float = 2.0) -> float: + """ + Аппроксимация HFOV от абсолютного зума (экспоненциальная интерполяция). + abs_zoom приводим к [0..1]. Под реальные значения можно подогнать константы. + """ + if not isfinite(abs_zoom): + return hfov_wide + if zmax == zmin: + return hfov_wide + t = clipf((abs_zoom - zmin) / (zmax - zmin), 0.0, 1.0) + r = hfov_tele / hfov_wide + return hfov_wide * (r ** t) + +def bearing_from_xcenter(x_center_norm: float, + hfov_deg: float, + base_axis_deg: float) -> float: + """ + Преобразует x-норм. центр бокса (0..1) в глобальный азимут: + bearing = base_axis_deg + (x-0.5)*HFOV + где base_axis_deg — азимут оптической оси (с учётом севера). + """ + delta = (x_center_norm - 0.5) * hfov_deg + return norm360(base_axis_deg + delta) diff --git a/ptz_tracker_modular/wiper.py b/ptz_tracker_modular/wiper.py new file mode 100644 index 0000000..a43ca78 --- /dev/null +++ b/ptz_tracker_modular/wiper.py @@ -0,0 +1,229 @@ +# ptz_tracker_modular/wiper.py +from __future__ import annotations + +import asyncio +import logging +from typing import Optional, Tuple + +import requests +from requests.auth import HTTPDigestAuth + +log = logging.getLogger("PTZTracker.WIPER") + +_sess = requests.Session() +_sess.trust_env = False +try: + import urllib3 + urllib3.disable_warnings(urllib3.exceptions.InsecureRequestWarning) +except Exception: + pass + +# Базовое значение; можно переопределить в config.py как WIPER_MIN_INTERVAL_SEC +_DEFAULT_MIN_INTERVAL_SEC = 1.5 +_last_call: dict[int, float] = {} + + +def _resolve_cam_id(cam_token: int) -> int: + """cam_token = реальный ID из cameras.toml ИЛИ индекс в PTZ_CAM_IDS.""" + from . import config + if cam_token in config.CAMERA_CONFIG: + return cam_token + if 0 <= cam_token < len(config.PTZ_CAM_IDS): + return config.PTZ_CAM_IDS[cam_token] + raise KeyError(f"Unknown camera token: {cam_token}") + + +def _base(cam_id: int): + """Вернёт (base_url, auth, verify, timeout, meta, channel) для указанной камеры.""" + from . import config as C + cfg = C.CAMERA_CONFIG[cam_id] + + scheme = cfg.get("scheme") or ("https" if cfg.get("https") else "http") + host = cfg["ip"] + port = int(cfg.get("port") or (443 if scheme == "https" else 80)) + + # ВАЖНО: сначала берём wiper_channel, потом ptz_channel, по умолчанию 1 + ch = int(cfg.get("wiper_channel", cfg.get("ptz_channel", 1))) + + user = cfg.get("username") or cfg.get("user") or cfg.get("login") or "admin" + pwd = cfg.get("password") or cfg.get("pass") or cfg.get("pwd") or "" + + auth = HTTPDigestAuth(user, pwd) + base = f"{scheme}://{host}:{port}" + verify = bool(cfg.get("verify_tls", False)) + timeout = int(cfg.get("ptz_timeout_sec", 6)) + meta = {"ip": host, "port": port, "scheme": scheme, "channel": ch, "user": user} + return base, auth, verify, timeout, meta, ch + + +def _log_resp(tag: str, r: requests.Response | None, exc: Exception | None = None): + """ + Единая точка логирования ответов. + Переведено в DEBUG, чтобы не шуметь в INFO. Никаких print(). + Возвращает кортеж (ok, http_code|None, body_str). + """ + if r is not None: + body = (r.text or "")[:300] + # детальный лог — только в DEBUG + if log.isEnabledFor(logging.DEBUG): + log.debug("%s -> HTTP %s | %r", tag, r.status_code, body) + return (200 <= r.status_code < 300), r.status_code, body + else: + log.warning("%s -> EXC %s", tag, exc) + return False, None, str(exc) if exc else "" + + +def _PUT(url, *, auth, verify, timeout, data=b"", headers=None): + try: + return _sess.put(url, data=data, headers=headers or {}, auth=auth, timeout=timeout, verify=verify), None + except Exception as e: + return None, e + + +def _GET(url, *, auth, verify, timeout): + try: + return _sess.get(url, auth=auth, timeout=timeout, verify=verify), None + except Exception as e: + return None, e + + +def _try_ptz(base, ch, auth, verify, timeout): + """ + Основная линейка эндпоинтов: + 1) manualWiper (длинная протирка — предпочтительный путь) + 2) Wiper(single) через XML + 3) auxiliary?name=wiper:single + 4) auxiliary?name=wiper:on + """ + # 1) manualWiper + r, e = _PUT(f"{base}/ISAPI/PTZCtrl/channels/{ch}/manualWiper", + auth=auth, verify=verify, timeout=timeout) + ok, code, body = _log_resp("manualWiper", r, e) + if ok: + return True, "manualWiper", code, body + + # 2) Wiper(single) XML + xml = b'single' + r, e = _PUT(f"{base}/ISAPI/PTZCtrl/channels/{ch}/Wiper", + auth=auth, verify=verify, timeout=timeout, + data=xml, headers={"Content-Type": "application/xml"}) + ok, code, body = _log_resp("Wiper(single)", r, e) + if ok: + return True, "Wiper(single)", code, body + + # 3) auxiliary wiper:single + r, e = _PUT(f"{base}/ISAPI/PTZCtrl/channels/{ch}/auxiliary?name=wiper:single", + auth=auth, verify=verify, timeout=timeout) + ok, code, body = _log_resp("auxiliary?wiper:single", r, e) + if ok: + return True, "auxiliary(wiper:single)", code, body + + # 4) auxiliary wiper:on + r, e = _PUT(f"{base}/ISAPI/PTZCtrl/channels/{ch}/auxiliary?name=wiper:on", + auth=auth, verify=verify, timeout=timeout) + ok, code, body = _log_resp("auxiliary?wiper:on", r, e) + if ok: + return True, "auxiliary(wiper:on)", code, body + + return False, "ptz_endpoints_failed", code, body + + +def _probe_io(base, auth, verify, timeout): + """Попытка найти дискретный выход, содержащий 'wiper' в имени.""" + r, e = _GET(f"{base}/ISAPI/System/IO/outputs", + auth=auth, verify=verify, timeout=timeout) + ok, code, body = _log_resp("GET IO/outputs", r, e) + if not ok or not body: + return None + + import re + ports = re.findall(r"(.*?)", body, flags=re.S) + for chunk in ports: + id_m = re.search(r"\s*(\d+)\s*", chunk) + name_m = re.search(r"\s*([^<]+)\s*", chunk) + if id_m and name_m and ("wiper" in name_m.group(1).strip().lower()): + out_id = int(id_m.group(1)) + log.debug("IO: found output id=%s", out_id) + return out_id + + log.debug("IO: no output named 'wiper' found") + return None + + +def _try_io(base, out_id, auth, verify, timeout, pulse_ms: int): + xmlA = f'high{pulse_ms}'.encode("utf-8") + r, e = _PUT(f"{base}/ISAPI/System/IO/outputs/{out_id}/status", + auth=auth, verify=verify, timeout=timeout, + data=xmlA, headers={"Content-Type": "application/xml"}) + ok, code, body = _log_resp(f"PUT IO/outputs/{out_id}/status", r, e) + if ok: + return True, "io_status", code, body + + xmlB = f'{pulse_ms}'.encode("utf-8") + r, e = _PUT(f"{base}/ISAPI/System/IO/outputs/{out_id}/trigger", + auth=auth, verify=verify, timeout=timeout, + data=xmlB, headers={"Content-Type": "application/xml"}) + ok, code, body = _log_resp(f"PUT IO/outputs/{out_id}/trigger", r, e) + if ok: + return True, "io_trigger", code, body + + return False, "io_failed", code, body + + +def run(cam_token: int, sec: int = 3) -> Tuple[bool, str, Optional[int], str, int]: + """ + Синхронный запуск дворника. + Возвращает: (ok, endpoint, http_status|None, detail, cam_id) + """ + cam_id = _resolve_cam_id(cam_token) + base, auth, verify, timeout, meta, ch = _base(cam_id) + + log.info("WIPER start: cam_token=%s cam_id=%s meta=%s", cam_token, cam_id, meta) + + # 1) Пытаемся через PTZ эндпоинты + ok, endpoint, code, body = _try_ptz(base, ch, auth, verify, timeout) + if ok: + return True, endpoint, code, body, cam_id + + # 2) Фоллбек через дискретный выход, если нашли + out_id = _probe_io(base, auth, verify, timeout) + if out_id is not None: + pulse_ms = max(500, int(sec * 1000)) + ok2, tag, code2, body2 = _try_io(base, out_id, auth, verify, timeout, pulse_ms) + return (ok2, tag, code2, body2, cam_id) + + # 3) Совсем не получилось + return False, endpoint, code, body, cam_id + + +async def trigger_wiper_once(cam_token: int, sec: float = 3.0) -> Tuple[bool, str, Optional[int], str, int]: + """ + Асинхронный вызов с анти-дребезгом. + Супрессия теперь возвращает ok=False (чтобы фронт видел «слишком часто»). + Интервал берём из config.WIPER_MIN_INTERVAL_SEC при наличии. + """ + from . import config # локальный импорт, чтобы не тянуть конфиг на уровне модуля + loop = asyncio.get_running_loop() + cam_id = _resolve_cam_id(cam_token) + + min_interval = float(getattr(config, "WIPER_MIN_INTERVAL_SEC", _DEFAULT_MIN_INTERVAL_SEC)) + + now = loop.time() + last = _last_call.get(cam_id, 0.0) + if (now - last) < min_interval: + delta = now - last + log.debug("WIPER suppressed: cam_id=%s delta=%.2f < %.2f", cam_id, delta, min_interval) + # ok=False, чтобы UI мог показать «слишком часто» + return False, "suppressed", None, f"too-frequent ({delta:.2f}s < {min_interval:.2f}s)", cam_id + + _last_call[cam_id] = now + + # нормализация sec + try: + sec_i = int(round(float(sec))) + except Exception: + sec_i = 3 + if sec_i <= 0: + sec_i = 1 + + return await asyncio.to_thread(run, cam_token, sec_i)