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