Measure fresh ISAPI status during TRACK on all six PTZ cameras
#12
Open
YaroslavKartashov
wants to merge 2 commits from feature/ptz-field-status-sampling into feature/ptz-offline-field-calibration
@ -0,0 +1,6 @@
|
|||||||
|
{
|
||||||
|
"status.py": {
|
||||||
|
"before": "0f0b91414e04eedc547a5e6931825974f9b5bc09d5795babe5c6ac6179f0882d",
|
||||||
|
"after": "815dee066d46031984b6bb77b0f6974c8b6c77b786f7a94a5ad2eb117bee039e"
|
||||||
|
}
|
||||||
|
}
|
||||||
@ -0,0 +1,129 @@
|
|||||||
|
--- a/status.py
|
||||||
|
+++ b/status.py
|
||||||
|
@@ -14,7 +14,7 @@
|
||||||
|
|
||||||
|
|
||||||
|
# ---------------- Публичные утилиты статуса ----------------
|
||||||
|
-from .zoom_controller import observe_calibration
|
||||||
|
+from .zoom_controller import _episode_telemetry, _owner, observe_calibration
|
||||||
|
|
||||||
|
|
||||||
|
def get_cached_angles(cam_id: int) -> Tuple[Optional[float], Optional[float], Optional[float]]:
|
||||||
|
@@ -97,6 +97,13 @@
|
||||||
|
|
||||||
|
rate_alpha = clipf(float(getattr(config, "PTZ_RATE_EMA_ALPHA", 0.40)), 0.02, 1.0)
|
||||||
|
|
||||||
|
+ pan_alpha = tilt_alpha = rate_alpha
|
||||||
|
+ if _track_status_period(cfg, st) is not None:
|
||||||
|
+ # Preserve the previous time constant at the faster TRACK cadence.
|
||||||
|
+ baseline_hz = max(0.1, float(getattr(config, "PTZ_STATUS_HZ", 4.0)))
|
||||||
|
+ pan_alpha = 1 - (1 - rate_alpha) ** (max(0.0, pan_sample_dt) * baseline_hz)
|
||||||
|
+ tilt_alpha = 1 - (1 - rate_alpha) ** (max(0.0, tilt_sample_dt) * baseline_hz)
|
||||||
|
+
|
||||||
|
if pan is not None:
|
||||||
|
pan_n = float(pan) % 360.0
|
||||||
|
if isinstance(previous_pan, (int, float)) and 0.03 <= pan_sample_dt <= 2.0:
|
||||||
|
@@ -104,7 +111,7 @@
|
||||||
|
pan_limit = float(cfg.get("ptz_pan_max_speed_deg_s", getattr(config, "AIM_PAN_SPEED_DEG_S", 160.0)))
|
||||||
|
if abs(raw_rate) <= max(1.0, pan_limit * 1.35):
|
||||||
|
old_rate = float(st.get("pan_rate_deg_s", 0.0) or 0.0)
|
||||||
|
- st["pan_rate_deg_s"] = (1.0 - rate_alpha) * old_rate + rate_alpha * raw_rate
|
||||||
|
+ st["pan_rate_deg_s"] = (1.0 - pan_alpha) * old_rate + pan_alpha * raw_rate
|
||||||
|
pan_rate_updated = True
|
||||||
|
st["az_deg"] = pan_n # <-- это читает ptz_io._ptz_worker при bounded-патруле
|
||||||
|
st["pan_deg"] = pan_n # удобный синоним
|
||||||
|
@@ -126,7 +133,7 @@
|
||||||
|
tilt_limit = float(cfg.get("ptz_tilt_max_speed_deg_s", getattr(config, "AIM_TILT_SPEED_DEG_S", 120.0)))
|
||||||
|
if abs(raw_rate) <= max(1.0, tilt_limit * 1.35):
|
||||||
|
old_rate = float(st.get("tilt_rate_deg_s", 0.0) or 0.0)
|
||||||
|
- st["tilt_rate_deg_s"] = (1.0 - rate_alpha) * old_rate + rate_alpha * raw_rate
|
||||||
|
+ st["tilt_rate_deg_s"] = (1.0 - tilt_alpha) * old_rate + tilt_alpha * raw_rate
|
||||||
|
tilt_rate_updated = True
|
||||||
|
st["tilt_deg"] = tilt_n
|
||||||
|
|
||||||
|
@@ -196,6 +203,14 @@
|
||||||
|
st["last_status_attempt_at"] = now
|
||||||
|
|
||||||
|
|
||||||
|
+def _track_status_period(camera_config, st):
|
||||||
|
+ rate = camera_config.get("ptz_status_track_hz")
|
||||||
|
+ if (type(rate) in (float, int) and 10 <= rate <= 15
|
||||||
|
+ and st.get("mode") == "TRACK" and st.get("tracking_active")):
|
||||||
|
+ return 1.0 / rate
|
||||||
|
+ return None
|
||||||
|
+
|
||||||
|
+
|
||||||
|
# -------------------- Основной поллер статуса ---------------------
|
||||||
|
async def _poll_camera_status_once(
|
||||||
|
cam_id: int, camera_config: dict, log_period: float,
|
||||||
|
@@ -204,6 +219,8 @@
|
||||||
|
"""Publish one result only while its camera configuration is still current."""
|
||||||
|
started_mono = time.monotonic()
|
||||||
|
config_snapshot = camera_config.copy()
|
||||||
|
+ st = state.ptz_states.get(cam_id, {})
|
||||||
|
+ sample_owner = _owner(st) if _track_status_period(camera_config, st) is not None else None
|
||||||
|
try:
|
||||||
|
full = await asyncio.to_thread(get_ptz_status_full, cam_id)
|
||||||
|
pan, tilt = full.pan, full.tilt
|
||||||
|
@@ -248,6 +265,17 @@
|
||||||
|
zoom_ratio=zoom_ratio, hfov_deg=hfov_deg, zoom_raw=zoom_raw,
|
||||||
|
zoom_tag=zoom_tag, zoom_age_sec=zoom_age_sec,
|
||||||
|
)
|
||||||
|
+ st = state.ptz_states.get(cam_id, {})
|
||||||
|
+ if (sample_owner is not None and sample_owner[1] > 0 and _owner(st) == sample_owner
|
||||||
|
+ and _track_status_period(camera_config, st) is not None):
|
||||||
|
+ _episode_telemetry(
|
||||||
|
+ config, cam_id, sample_owner, "status_sample", observed_at,
|
||||||
|
+ request_ms=request_ms, status_ok=(pan is not None or tilt is not None or hfov_deg is not None),
|
||||||
|
+ FOV_actual=hfov_deg,
|
||||||
|
+ FOV_status_utc=observed_at - zoom_age_sec
|
||||||
|
+ if hfov_deg is not None and zoom_age_sec is not None else None,
|
||||||
|
+ optics_fresh=hfov_deg is not None and zoom_age_sec == 0.0,
|
||||||
|
+ )
|
||||||
|
if log_period > 0 and _should_log_telemetry(cam_id, observed_at, log_period):
|
||||||
|
st = state.ptz_states.get(cam_id, {})
|
||||||
|
logger.info(
|
||||||
|
@@ -280,7 +308,7 @@
|
||||||
|
)
|
||||||
|
loop = asyncio.get_running_loop()
|
||||||
|
in_flight: dict[int, asyncio.Task] = {}
|
||||||
|
- next_due: dict[int, float] = {}
|
||||||
|
+ last_start: dict[int, float] = {}
|
||||||
|
session = {"active": True}
|
||||||
|
cancelling = False
|
||||||
|
try:
|
||||||
|
@@ -297,22 +325,29 @@
|
||||||
|
logger.exception("[STATUS] cam %d publication failed", cam_id)
|
||||||
|
# Do not cancel removed cameras' to_thread work and immediately
|
||||||
|
# launch another request for a reused ID. Retire it when done.
|
||||||
|
- for cam_id in list(next_due):
|
||||||
|
+ for cam_id in list(last_start):
|
||||||
|
if cam_id not in cam_ids and cam_id not in in_flight:
|
||||||
|
- del next_due[cam_id]
|
||||||
|
+ del last_start[cam_id]
|
||||||
|
now_mono = loop.time()
|
||||||
|
+ delay = min(0.05, interval)
|
||||||
|
for cam_id in sorted(cam_ids):
|
||||||
|
cfg = config.CAMERA_CONFIG.get(cam_id)
|
||||||
|
- if cfg is None or cam_id in in_flight or now_mono < next_due.get(cam_id, 0.0):
|
||||||
|
+ if cfg is None:
|
||||||
|
continue
|
||||||
|
- next_due[cam_id] = now_mono + interval
|
||||||
|
+ track_period = _track_status_period(cfg, state.ptz_states.get(cam_id, {}))
|
||||||
|
+ period = track_period if track_period is not None else interval
|
||||||
|
+ if track_period is not None:
|
||||||
|
+ delay = min(delay, period / 8)
|
||||||
|
+ if cam_id in in_flight or now_mono < last_start.get(cam_id, -float("inf")) + period:
|
||||||
|
+ continue
|
||||||
|
+ last_start[cam_id] = now_mono
|
||||||
|
in_flight[cam_id] = asyncio.create_task(
|
||||||
|
_poll_camera_status_once(cam_id, cfg, log_period, session),
|
||||||
|
name=f"ptz-status-{cam_id}",
|
||||||
|
)
|
||||||
|
# Bounded supervisor tick also notices camera add/delete and shutdown.
|
||||||
|
# Individual tasks publish as soon as ready, not at this tick.
|
||||||
|
- await asyncio.sleep(min(0.05, interval))
|
||||||
|
+ await asyncio.sleep(delay)
|
||||||
|
except asyncio.CancelledError:
|
||||||
|
logger.info("[STATUS] PTZ status poller cancelled")
|
||||||
|
cancelling = True
|
||||||
@ -0,0 +1,113 @@
|
|||||||
|
"""Test the additive patch against the pinned PR10 status.py without HTTP."""
|
||||||
|
|
||||||
|
import asyncio
|
||||||
|
from hashlib import sha256
|
||||||
|
import json
|
||||||
|
from pathlib import Path
|
||||||
|
import subprocess
|
||||||
|
import sys
|
||||||
|
from tempfile import TemporaryDirectory
|
||||||
|
from types import ModuleType, SimpleNamespace
|
||||||
|
import unittest
|
||||||
|
from unittest.mock import patch
|
||||||
|
|
||||||
|
|
||||||
|
HERE = Path(__file__).resolve().parent
|
||||||
|
SOURCE = Path(sys.argv.pop(1)) if __name__ == "__main__" else None
|
||||||
|
|
||||||
|
|
||||||
|
@unittest.skipIf(SOURCE is None, "Pass the path to materialized PR10 status.py")
|
||||||
|
class FieldStatusTests(unittest.IsolatedAsyncioTestCase):
|
||||||
|
def setUp(self):
|
||||||
|
data = SOURCE.read_bytes()
|
||||||
|
pins = json.loads((HERE / "pins.json").read_bytes())["status.py"]
|
||||||
|
self.assertEqual(sha256(data).hexdigest(), pins["before"])
|
||||||
|
with TemporaryDirectory() as root:
|
||||||
|
path = Path(root) / "status.py"
|
||||||
|
path.write_bytes(data)
|
||||||
|
git = ["git", "-c", "core.autocrlf=false", "apply"]
|
||||||
|
subprocess.run([*git, str(HERE / "status.patch")], cwd=root, check=True)
|
||||||
|
candidate = path.read_bytes()
|
||||||
|
self.assertEqual(sha256(candidate).hexdigest(), pins["after"])
|
||||||
|
subprocess.run([*git, "-R", str(HERE / "status.patch")], cwd=root, check=True)
|
||||||
|
self.assertEqual(path.read_bytes(), data)
|
||||||
|
self.config = SimpleNamespace(PTZ_STATUS_HZ=4., PTZ_CAM_IDS=[0, 2, 4, 6, 8, 10],
|
||||||
|
CAMERA_CONFIG={c: {"ptz_status_track_hz": 15.}
|
||||||
|
for c in (0, 2, 4, 6, 8, 10)})
|
||||||
|
self.state = SimpleNamespace(shutdown=False, ptz_states={}, ptz_status={})
|
||||||
|
self.events = []
|
||||||
|
package = ModuleType("pilot_test")
|
||||||
|
package.config, package.state = self.config, self.state
|
||||||
|
mod = ModuleType("pilot_test.status")
|
||||||
|
mod.__package__ = package.__name__
|
||||||
|
values = {"pilot_test": package,
|
||||||
|
"pilot_test.utils": SimpleNamespace(clipf=lambda x, low, high: max(low, min(high, x)),
|
||||||
|
wrap_deg=lambda x: x),
|
||||||
|
"pilot_test.ptz_io": SimpleNamespace(get_ptz_status_full=None,
|
||||||
|
confirm_zoom_step_optics_feedback=None, _zoom_command_lock=None),
|
||||||
|
"pilot_test.zoom_controller": SimpleNamespace(
|
||||||
|
observe_calibration=None, _episode_telemetry=lambda *a, **kw:self.events.append((a, kw)),
|
||||||
|
_owner=lambda s:(s.get("ptz_generation", 0), s.get("track_started_at", 0.)))}
|
||||||
|
with patch.dict(sys.modules, values):
|
||||||
|
exec(compile(candidate, "status.py", "exec"), vars(mod))
|
||||||
|
self.status = mod
|
||||||
|
|
||||||
|
def test_only_valid_opt_in_track_accelerates(self):
|
||||||
|
for cam in self.config.PTZ_CAM_IDS:
|
||||||
|
cfg = self.config.CAMERA_CONFIG[cam]
|
||||||
|
self.assertAlmostEqual(self.status._track_status_period(cfg, {"mode":"TRACK", "tracking_active":True}), 1/15)
|
||||||
|
self.assertIsNone(self.status._track_status_period(cfg, {"mode":"RETURN", "tracking_active":True}))
|
||||||
|
for invalid in (None, 9, 16, True, "15", float("nan")):
|
||||||
|
self.assertIsNone(self.status._track_status_period({"ptz_status_track_hz":invalid},
|
||||||
|
{"mode":"TRACK", "tracking_active":True}))
|
||||||
|
|
||||||
|
async def test_one_request_per_camera_and_all_six_can_accelerate(self):
|
||||||
|
calls = dict.fromkeys(self.config.PTZ_CAM_IDS, 0)
|
||||||
|
inflight = set()
|
||||||
|
for cam in self.config.PTZ_CAM_IDS:
|
||||||
|
self.state.ptz_states[cam] = dict(mode="TRACK", tracking_active=True)
|
||||||
|
|
||||||
|
async def poll(cam, *args):
|
||||||
|
self.assertNotIn(cam, inflight)
|
||||||
|
inflight.add(cam)
|
||||||
|
calls[cam] += 1
|
||||||
|
await asyncio.sleep(.105 if cam == 0 else .002)
|
||||||
|
inflight.remove(cam)
|
||||||
|
|
||||||
|
self.status._poll_camera_status_once = poll
|
||||||
|
task = asyncio.create_task(self.status.ptz_status_poller())
|
||||||
|
try:
|
||||||
|
await asyncio.sleep(.42)
|
||||||
|
finally:
|
||||||
|
self.state.shutdown = True
|
||||||
|
await asyncio.wait_for(task, .5)
|
||||||
|
self.assertGreaterEqual(min(calls.values()), 3)
|
||||||
|
self.assertGreater(calls[2], calls[0])
|
||||||
|
|
||||||
|
async def test_late_reply_is_not_logged_for_new_track_and_cache_is_marked(self):
|
||||||
|
from threading import RLock
|
||||||
|
self.status._zoom_command_lock = lambda _: RLock()
|
||||||
|
self.status._update_runtime_state = lambda *args, **kwargs: None
|
||||||
|
self.state.ptz_states[8] = dict(mode="TRACK", tracking_active=True,
|
||||||
|
ptz_generation=1, track_started_at=100.)
|
||||||
|
value = SimpleNamespace(pan=0., tilt=0., zoom_ratio=2., hfov_deg=20.,
|
||||||
|
zoom_raw=20., zoom_tag="test", zoom_age_sec=.05)
|
||||||
|
async def late(*_):
|
||||||
|
self.state.ptz_states[8]["ptz_generation"] = 2
|
||||||
|
return value
|
||||||
|
async def current(*_):
|
||||||
|
return value
|
||||||
|
cfg = self.config.CAMERA_CONFIG[8]
|
||||||
|
with patch.object(self.status.asyncio, "to_thread", late):
|
||||||
|
await self.status._poll_camera_status_once(8, cfg, 0)
|
||||||
|
self.assertEqual(self.events, [])
|
||||||
|
with patch.object(self.status.asyncio, "to_thread", current):
|
||||||
|
await self.status._poll_camera_status_once(8, cfg, 0)
|
||||||
|
args, fields = self.events[0]
|
||||||
|
self.assertEqual(args[1:4], (8, (2, 100.), "status_sample"))
|
||||||
|
self.assertTrue(fields["status_ok"])
|
||||||
|
self.assertFalse(fields["optics_fresh"])
|
||||||
|
|
||||||
|
|
||||||
|
if __name__ == "__main__":
|
||||||
|
unittest.main()
|
||||||
Loading…
Reference in New Issue