Predictive TRACK zoom for calibrated PTZ cameras
#8
Open
YaroslavKartashov
wants to merge 4 commits from feature/ptz-predictive-zoom into feature/ptz-zoom-threshold-diagnostics
@ -0,0 +1,45 @@
|
|||||||
|
{
|
||||||
|
"scope": "offline_replay_only",
|
||||||
|
"production_enable": false,
|
||||||
|
"cameras": {
|
||||||
|
"19": {
|
||||||
|
"paired_pulses": 3,
|
||||||
|
"observed_effective_log_rate_range": [
|
||||||
|
0.26096,
|
||||||
|
0.51553
|
||||||
|
],
|
||||||
|
"replay_settings": {
|
||||||
|
"nominal_gain": 0.44648,
|
||||||
|
"upper_gain": 0.95109
|
||||||
|
},
|
||||||
|
"telephoto_rate_measured": false
|
||||||
|
},
|
||||||
|
"21": {
|
||||||
|
"paired_pulses": 2,
|
||||||
|
"observed_effective_log_rate_range": [
|
||||||
|
0.40197,
|
||||||
|
0.59657
|
||||||
|
],
|
||||||
|
"replay_settings": {
|
||||||
|
"nominal_gain": 0.73468,
|
||||||
|
"upper_gain": 1.09732
|
||||||
|
},
|
||||||
|
"telephoto_rate_measured": false
|
||||||
|
}
|
||||||
|
},
|
||||||
|
"validation": {
|
||||||
|
"wide_pairs": 12,
|
||||||
|
"narrow_pairs": 8,
|
||||||
|
"narrow_lateral_acceleration_pairs": 4,
|
||||||
|
"first_zoom_speedup_s_range": [
|
||||||
|
0.0,
|
||||||
|
2.7
|
||||||
|
],
|
||||||
|
"candidate_smallest_model_margin_narrow": 0.2164,
|
||||||
|
"candidate_smallest_model_margin_lateral": 0.1844,
|
||||||
|
"candidate_model_edge_clips": 0,
|
||||||
|
"confirmed_live_retention_or_speed": false,
|
||||||
|
"additional_put_rtt_pairs": 8,
|
||||||
|
"candidate_smallest_model_margin_put_rtt_narrow": 0.1852
|
||||||
|
}
|
||||||
|
}
|
||||||
@ -0,0 +1,17 @@
|
|||||||
|
{
|
||||||
|
"postprocess.py": {
|
||||||
|
"before": "776b5b337e5f898e69770fec517c4459560f4cc6adeb951af40ac8f0b2178534",
|
||||||
|
"after": "82ba632e7e2696e5b48daa99e77741482b98a02103eaf7fb416c58fd686e4327"
|
||||||
|
},
|
||||||
|
"ptz_io.py": {
|
||||||
|
"before": "13d31539193b1a773a5d1db2187cc2a0c8db67d0aacaad89082face64df07499",
|
||||||
|
"after": "0b0651b0e0344e4f78aeaf01f76c969d0043148a4ac92e4b7b6fd4a5f0d6a2e7"
|
||||||
|
},
|
||||||
|
"status.py": {
|
||||||
|
"before": "2fca9a2000c2cac9af42ad3f637416c66e1bb87f2f8166c7aefbbebbe5d07b06",
|
||||||
|
"after": "820db8615638c728d1017ea365f3d16b95edfe61dbb684ba0cda8de051ea8bd1"
|
||||||
|
},
|
||||||
|
"zoom_controller.py": {
|
||||||
|
"after": "7197baba29976215b05f6f86ae1563e8b5c28b314a80894600ede517de00f91e"
|
||||||
|
}
|
||||||
|
}
|
||||||
@ -0,0 +1,73 @@
|
|||||||
|
"""Render an episode JSONL as a static SVG without plotting dependencies."""
|
||||||
|
|
||||||
|
import argparse
|
||||||
|
import json
|
||||||
|
from pathlib import Path
|
||||||
|
from xml.sax.saxutils import escape
|
||||||
|
|
||||||
|
|
||||||
|
def render(rows):
|
||||||
|
samples = sorted((r for r in rows if r["event"] == "tick"), key=lambda r: r["elapsed_ms"])
|
||||||
|
if not samples:
|
||||||
|
raise ValueError("The episode has no TRACK ticks.")
|
||||||
|
last = max(1.0, samples[-1]["elapsed_ms"])
|
||||||
|
groups = (
|
||||||
|
("HFOV (deg)", 60.0, (
|
||||||
|
("ISAPI actual", "#2563eb", lambda r: r["FOV_actual"]),
|
||||||
|
("Estimated", "#c2410c", lambda r: r["FOV_estimated"]),
|
||||||
|
("Narrow bound", "#64748b", lambda r: r["FOV_min_estimated"]),
|
||||||
|
)),
|
||||||
|
("BBox (px)", max(48.0, max(max(r["bbox_px"]) for r in samples)), (
|
||||||
|
("Width", "#2563eb", lambda r: r["bbox_px"][0]),
|
||||||
|
("Height", "#c2410c", lambda r: r["bbox_px"][1]),
|
||||||
|
)),
|
||||||
|
("Confidence", 1.0, (
|
||||||
|
("YOLO last match", "#2563eb", lambda r: r["detector_conf"]),
|
||||||
|
("OSTrack score", "#c2410c", lambda r: r["tracker_score"]),
|
||||||
|
)),
|
||||||
|
)
|
||||||
|
content = [
|
||||||
|
'<svg xmlns="http://www.w3.org/2000/svg" width="980" height="680" viewBox="0 0 980 680">',
|
||||||
|
'<rect width="100%" height="100%" fill="white"/>',
|
||||||
|
'<g fill="#0f172a" font-family="sans-serif" font-size="15">',
|
||||||
|
f'<text x="68" y="27">PTZ {escape(str(samples[0]["cam"]))} · TRACK telemetry</text>',
|
||||||
|
]
|
||||||
|
first = next((r.get("t_to_first_zoom_ms") for r in rows if r["event"] == "first_zoom"), None)
|
||||||
|
for panel, (title, ymax, series) in enumerate(groups):
|
||||||
|
y0 = 76 + 190 * panel
|
||||||
|
plot_h = 133
|
||||||
|
content.append(f'<text x="68" y="{y0-12}">{title}</text>')
|
||||||
|
content.append(f'<path d="M68 {y0} v{plot_h} h850" fill="none" stroke="#94a3b8"/>')
|
||||||
|
for frac in (0, .5, 1):
|
||||||
|
content.append(f'<text x="17" y="{y0 + plot_h * (1-frac) + 5}">{ymax*frac:.1f}</text>')
|
||||||
|
for number, (label, color, extract) in enumerate(series):
|
||||||
|
content.append(f'<text fill="{color}" x="{265 + 170 * number}" y="{y0-12}">{label}</text>')
|
||||||
|
path = []
|
||||||
|
for row in samples:
|
||||||
|
value = extract(row)
|
||||||
|
if value is None:
|
||||||
|
if path and path[-1]:
|
||||||
|
path.append("")
|
||||||
|
continue
|
||||||
|
x = 68 + 850 * row["elapsed_ms"] / last
|
||||||
|
y = y0 + plot_h * (1 - value / ymax)
|
||||||
|
command = "L" if path and path[-1] else "M"
|
||||||
|
path.append(f"{command}{x:.1f} {y:.1f}")
|
||||||
|
content.append(f'<path d="{" ".join(path)}" stroke="{color}" stroke-width="2" fill="none"/>')
|
||||||
|
if first is not None and 0 <= first <= last:
|
||||||
|
x = 68 + 850 * first / last
|
||||||
|
content.append(f'<path d="M{x:.1f} {y0} v{plot_h}" stroke="#334155" stroke-dasharray="3 3"/>')
|
||||||
|
for k in range(6):
|
||||||
|
content.append(f'<text x="{68 + 170*k}" y="623">{last*k/5000:.2f}</text>')
|
||||||
|
content.append('<text x="68" y="654">Seconds since TRACK_START. Dashed marker = first acknowledged zoom.</text>')
|
||||||
|
content.append('</g></svg>')
|
||||||
|
return "\n".join(content) + "\n"
|
||||||
|
|
||||||
|
|
||||||
|
if __name__ == "__main__":
|
||||||
|
parser = argparse.ArgumentParser(description=__doc__)
|
||||||
|
parser.add_argument("episode", type=Path)
|
||||||
|
parser.add_argument("output", type=Path)
|
||||||
|
args = parser.parse_args()
|
||||||
|
rows = [json.loads(line) for line in args.episode.read_text(encoding="utf-8").splitlines() if line]
|
||||||
|
args.output.write_text(render(rows), encoding="utf-8")
|
||||||
@ -0,0 +1,42 @@
|
|||||||
|
from pathlib import Path
|
||||||
|
import json, os, sys
|
||||||
|
|
||||||
|
v,z,root,variant=map(Path,sys.argv[1:])
|
||||||
|
candidate=v/"ptz_predictive_zoom_candidate_20261111"
|
||||||
|
pr7=v/"ptz_zoom_threshold_diagnostics_20261111"
|
||||||
|
pins=json.loads((z/"pins.json").read_text())
|
||||||
|
dest=v/f"predictive_zoom_regression_{variant}.json"
|
||||||
|
before=str(variant)=="before"
|
||||||
|
p=(v/"ptz_zoom_threshold_maintenance_20261111" if before else candidate)/"postprocess.py"
|
||||||
|
i=(v/"ptz_zoom_zero_decision_20261109" if before else candidate)/"ptz_io.py"
|
||||||
|
s=(v/"ptz_monitor/server_unknown_source_20261109_043525" if before else candidate)/"status.py"
|
||||||
|
source=v/"ptz_monitor/server_unknown_source_20261109_043525"
|
||||||
|
key="before" if before else "after"
|
||||||
|
sys.path.insert(0,str(pr7))
|
||||||
|
if not before:
|
||||||
|
sys.path.insert(0,str(z))
|
||||||
|
sys.argv=[str(pr7/"replay.py"),str(source),str(i),
|
||||||
|
str(v/"ptz_zoom_stop_maintenance_20261109/ssh_7c7ae84464cc400dbc7dc1327124bbf7/telemetry.json"),
|
||||||
|
str(v/"cc1_cc2_control_replay_20261026/test_control_replay.py"),str(p),
|
||||||
|
pins["postprocess.py"][key],pins["ptz_io.py"][key],str(root),str(dest),
|
||||||
|
str(v/"ptz_zoom_zero_maintenance_20261109/ssh_8f0f9accd24b4f40b6466d0566cafbce/telemetry.json"),"fixed","--thresholds"]
|
||||||
|
r=(pr7/"replay.py").read_text(encoding="utf-8")
|
||||||
|
if not before:
|
||||||
|
r=r.replace(" ptz_io=load",f" load(PKG+'.zoom_controller', Path({str(z/'zoom_controller.py')!r}))\n ptz_io=load")
|
||||||
|
r=r.replace(" pp=load",f" status=load(PKG+'.status', Path({str(s)!r}))\n pp=load")
|
||||||
|
r=r.replace(" result=unittest.TextTestRunner", """ from track_integration import TRACE as zoom_trace, build_track_cases
|
||||||
|
integration = build_track_cases(before, pp, ptz_io, config, state, status)
|
||||||
|
for case in ("test_track_runs_burst_without_post_stop_status",
|
||||||
|
"test_track_same_flight_legacy_reference",
|
||||||
|
"test_track_starts_at_26_percent_of_half_frame",
|
||||||
|
"test_track_legacy_reference_at_26_percent_of_half_frame",
|
||||||
|
"test_priority_zoom_stop_honors_axis_policy",
|
||||||
|
"test_priority_zoom_stop_does_not_override_full_stop"):
|
||||||
|
suite.addTest(integration(case))
|
||||||
|
result=unittest.TextTestRunner""")
|
||||||
|
r=r.replace(" result_file=Path", f""" if result.wasSuccessful():
|
||||||
|
Path({str(v/"predictive_zoom_simulated_episode.jsonl")!r}).write_text(
|
||||||
|
"".join(json.dumps(row, allow_nan=False) + "\\n" for row in zoom_trace), encoding="utf-8")
|
||||||
|
result_file=Path""")
|
||||||
|
code=compile(r,str(pr7/"replay.py"),"exec")
|
||||||
|
exec(code,{"__file__":str(pr7/"replay.py"),"__name__":"__main__"})
|
||||||
@ -0,0 +1,377 @@
|
|||||||
|
--- a/postprocess.py
|
||||||
|
+++ b/postprocess.py
|
||||||
|
@@ -10,6 +10,7 @@
|
||||||
|
import cv2
|
||||||
|
|
||||||
|
from . import config, state, visual_control
|
||||||
|
+from .zoom_controller import ZoomFrame, controller_for_track, telemetry_tick
|
||||||
|
from . import motion_detector
|
||||||
|
from .utils import (
|
||||||
|
clipf,
|
||||||
|
@@ -30,6 +31,7 @@
|
||||||
|
_zoom_command_lock,
|
||||||
|
zoom_step_optics_delta,
|
||||||
|
_zoom_speed_to_int,
|
||||||
|
+ stop_predictive_zoom_async,
|
||||||
|
)
|
||||||
|
from .geom import ang_diff_signed, sector_contains
|
||||||
|
from .patrol import (
|
||||||
|
@@ -4597,6 +4599,7 @@
|
||||||
|
optics_observed_now: float | None = None,
|
||||||
|
stop_trace: list[str] | None = None,
|
||||||
|
continuation_trace: list[tuple[int, int, float, float, float, float, float]] | None = None,
|
||||||
|
+ predictive=None,
|
||||||
|
) -> float:
|
||||||
|
"""Break continuous zoom-in into short feedback-controlled steps."""
|
||||||
|
pulse_until = float(st.get("zoom_step_until", 0.0) or 0.0)
|
||||||
|
@@ -4706,18 +4709,23 @@
|
||||||
|
_finish_zoom_step(st, now, stable_key=stable_key)
|
||||||
|
return 0.0
|
||||||
|
if pulse_until <= 0.0:
|
||||||
|
- if (
|
||||||
|
+ observed = now if optics_observed_now is None else optics_observed_now
|
||||||
|
+ early_repeat = bool(predictive and predictive.repeat_after_stop(st, observed))
|
||||||
|
+ if not early_repeat and (
|
||||||
|
float(now) < cooldown_until
|
||||||
|
or float(now) < float(st.get("zoom_recenter_until", 0.0) or 0.0)
|
||||||
|
):
|
||||||
|
return 0.0
|
||||||
|
- if (
|
||||||
|
+ if early_repeat:
|
||||||
|
+ # The original transaction is already committed. Consume its single
|
||||||
|
+ # credit under the transport lock, before publishing a new request.
|
||||||
|
+ predictive.confirmed_stop = None
|
||||||
|
+ st["zoom_feedback_required_after"] = 0.0
|
||||||
|
+ elif (
|
||||||
|
int(st.get("zoom_in_pulse_count", 0) or 0) > 0
|
||||||
|
or float(st.get("zoom_feedback_required_after", 0.0) or 0.0) > 0.0
|
||||||
|
):
|
||||||
|
- feedback_ready, feedback_reason = _zoom_optics_feedback_state(
|
||||||
|
- st, now if optics_observed_now is None else optics_observed_now,
|
||||||
|
- )
|
||||||
|
+ feedback_ready, feedback_reason = _zoom_optics_feedback_state(st, observed)
|
||||||
|
if not feedback_ready:
|
||||||
|
st["zoom_followup_reason"] = str(feedback_reason)
|
||||||
|
return 0.0
|
||||||
|
@@ -8778,6 +8786,74 @@
|
||||||
|
react_margin = clipf(react_margin_base + react_margin_gain * vel, react_margin_base, react_margin_max)
|
||||||
|
near_react_edge = min_margin < react_margin
|
||||||
|
|
||||||
|
+ predictive = None
|
||||||
|
+ predictive_frame = None
|
||||||
|
+ predictive_decision = None
|
||||||
|
+ if cfg_ptz.get("predictive_zoom") is not None:
|
||||||
|
+ with _zoom_command_lock(cam_idx):
|
||||||
|
+ predictive = controller_for_track(st, cfg_ptz)
|
||||||
|
+ if predictive is not None:
|
||||||
|
+ decision_wall = time.time()
|
||||||
|
+ sample_is_current = bool(
|
||||||
|
+ visual_sample is not None
|
||||||
|
+ and visual_control.current(
|
||||||
|
+ cam_idx, visual_sample, decision_wall,
|
||||||
|
+ max_age=predictive.cfg.frame_age_s,
|
||||||
|
+ )
|
||||||
|
+ )
|
||||||
|
+ predictive_frame = ZoomFrame(
|
||||||
|
+ at=now, seq=int(source_seq or 0),
|
||||||
|
+ offset=(measured_dx, measured_dy),
|
||||||
|
+ bbox_frac=(w_frac, h_frac), bbox_px=(x2 - x1, y2 - y1),
|
||||||
|
+ angles=(los_pan_angle_deg, los_tilt_angle_deg),
|
||||||
|
+ rates=(los_pan_rate_deg_s, los_tilt_rate_deg_s),
|
||||||
|
+ aspect=h / w,
|
||||||
|
+ tracker_score=(
|
||||||
|
+ float(st.get("visual_tracker_score", 0.0) or 0.0)
|
||||||
|
+ if current_track_source == "visual" else None
|
||||||
|
+ ),
|
||||||
|
+ detector_at=(last_visual_zoom_validation if normal_zoom_quality_ok
|
||||||
|
+ and last_visual_zoom_validation == last_visual_control_validation else 0.0),
|
||||||
|
+ detector_conf=float(st.get("visual_tracker_last_detector_conf", 0.0) or 0.0),
|
||||||
|
+ geometry_ok=bool(
|
||||||
|
+ sample_is_current and not st.get("visual_tracker_mismatch_frames")
|
||||||
|
+ and _zoom_revalidation_ready(st)
|
||||||
|
+ ),
|
||||||
|
+ los_valid=bool(
|
||||||
|
+ st.get("aim_los_rate_valid")
|
||||||
|
+ or predictive.request is not None
|
||||||
|
+ or predictive.repeat_after_stop(st, decision_wall)
|
||||||
|
+ or (st.get("zoom_step_pending") and predictive.history)
|
||||||
|
+ ),
|
||||||
|
+ )
|
||||||
|
+ skip_wait = predictive.repeat_after_stop(st, decision_wall)
|
||||||
|
+ same_pending = bool(
|
||||||
|
+ st.get("zoom_step_pending")
|
||||||
|
+ and st.get("predictive_zoom_request") == st.get("zoom_step_pending_id")
|
||||||
|
+ and not st.get("zoom_step_cancel_requested")
|
||||||
|
+ and not st.get("zoom_step_discard_on_finalize")
|
||||||
|
+ )
|
||||||
|
+ predictive_decision = predictive.decide(
|
||||||
|
+ predictive_frame, decision_wall,
|
||||||
|
+ first=pulse_count == 0 and predictive.first_ack_at is None,
|
||||||
|
+ cooldown=not skip_wait and decision_wall < max(
|
||||||
|
+ float(st.get("zoom_step_cooldown_until", 0.0) or 0.0),
|
||||||
|
+ float(st.get("zoom_recenter_until", 0.0) or 0.0),
|
||||||
|
+ ),
|
||||||
|
+ transport_clear=bool(
|
||||||
|
+ same_pending or skip_wait or (pulse_count == 0
|
||||||
|
+ and not st.get("zoom_feedback_required_after")
|
||||||
|
+ and not st.get("zoom_step_feedback_pending"))
|
||||||
|
+ or _zoom_optics_feedback_state(st, decision_wall)[0]
|
||||||
|
+ ) and not (
|
||||||
|
+ st.get("zoom_stop_obligation_active")
|
||||||
|
+ or st.get("lens_move_stop_required")
|
||||||
|
+ or st.get("zoom_step_cancel_requested")
|
||||||
|
+ or st.get("zoom_no_effect_blocked")
|
||||||
|
+ or (st.get("zoom_step_pending") and not same_pending)
|
||||||
|
+ ),
|
||||||
|
+ )
|
||||||
|
+
|
||||||
|
zoom_quality_ok = bool(
|
||||||
|
normal_zoom_quality_ok
|
||||||
|
or probe_allowed
|
||||||
|
@@ -8819,6 +8895,21 @@
|
||||||
|
pan_rate_deg_s=float(los_pan_rate_deg_s),
|
||||||
|
tilt_rate_deg_s=float(los_tilt_rate_deg_s),
|
||||||
|
)
|
||||||
|
+ predictive_entry = bool(
|
||||||
|
+ predictive_decision and predictive_decision.window
|
||||||
|
+ and normal_zoom_quality_ok and not bounded_identification_request
|
||||||
|
+ and not st.get("zoom_probe_active", False)
|
||||||
|
+ and not _zoom_identify_matches_current_track(st)
|
||||||
|
+ and not st.get("zoom_stop_obligation_active")
|
||||||
|
+ and not st.get("zoom_step_cancel_requested")
|
||||||
|
+ )
|
||||||
|
+ predictive_continuing = bool(
|
||||||
|
+ predictive_decision and same_pending and not predictive_decision.stop
|
||||||
|
+ and (predictive_decision.window or predictive_decision.reason == "pulse_active")
|
||||||
|
+ and normal_zoom_quality_ok and not bounded_identification_request
|
||||||
|
+ )
|
||||||
|
+ if (predictive_entry and (skip_wait or same_pending)) or predictive_continuing:
|
||||||
|
+ followup_ready = True
|
||||||
|
angular_rejection = {} if pulse_active else None
|
||||||
|
zoom_angular_safe = followup_ready and _zoom_angular_motion_safe(
|
||||||
|
pan_error_deg=float(los_pan_angle_deg),
|
||||||
|
@@ -8832,6 +8923,9 @@
|
||||||
|
)
|
||||||
|
if approach_margin is not None and followup_ready:
|
||||||
|
zoom_angular_safe = True
|
||||||
|
+ if (predictive_entry or predictive_continuing) and followup_ready:
|
||||||
|
+ zoom_angular_safe = True
|
||||||
|
+ zoom_safe, zoom_safe_scale = True, 1.0
|
||||||
|
st["zoom_safe_pred_margin"] = float(zoom_pred_margin)
|
||||||
|
st["zoom_safe_center_limit"] = float(zoom_center_max)
|
||||||
|
st["zoom_quality_ok"] = bool(zoom_quality_ok)
|
||||||
|
@@ -8952,6 +9046,7 @@
|
||||||
|
or bounded_identification_request
|
||||||
|
)
|
||||||
|
zoom_rearm_ready = _zoom_revalidation_ready(st)
|
||||||
|
+ stable_ok = stable_ok or predictive_entry or predictive_continuing
|
||||||
|
can_zoom_in = (
|
||||||
|
zoom_safe
|
||||||
|
and zoom_angular_safe
|
||||||
|
@@ -9099,6 +9194,14 @@
|
||||||
|
zoom_zero_reason = "deadband"
|
||||||
|
zoom_cmd = 0.0
|
||||||
|
|
||||||
|
+ predictive_drive = bool(
|
||||||
|
+ (predictive_entry or predictive_continuing) and can_zoom_in and raw_zoom_needed
|
||||||
|
+ and prev_z >= 0.0 and not want_out
|
||||||
|
+ and not normal_size_hold and not edge_recovery_active
|
||||||
|
+ and float(getattr(config, "ZOOM_STEP_MAX_IN_CMD", 0.12)) >= 0.10
|
||||||
|
+ )
|
||||||
|
+ if predictive_drive:
|
||||||
|
+ zoom_cmd = 0.10
|
||||||
|
zoom_after_size_minimum = zoom_cmd
|
||||||
|
cmd_mag_est = max(
|
||||||
|
abs(float(pan_speed) + float(pan_rate_ff)),
|
||||||
|
@@ -9151,7 +9254,7 @@
|
||||||
|
center_err=float(center_err),
|
||||||
|
moving_out=bool(moving_out),
|
||||||
|
recent_zoom=bool(recent_zoom_steering),
|
||||||
|
- approach_margin=approach_margin,
|
||||||
|
+ approach_margin=(predictive_decision.margin if predictive_drive else approach_margin),
|
||||||
|
)
|
||||||
|
if before_recenter > 0.0 and zoom_cmd <= 0.0:
|
||||||
|
zoom_zero_reason = "recenter"
|
||||||
|
@@ -9272,6 +9375,25 @@
|
||||||
|
proposed_zoom_cmd = max(zoom_cmd, min(pre_ramp_zoom_cmd, pulse_cap, .12))
|
||||||
|
if _zoom_native_response_available(st, _zoom_speed_to_int(proposed_zoom_cmd), hfov_deg, shared_wide_hfov):
|
||||||
|
zoom_cmd = proposed_zoom_cmd
|
||||||
|
+ # A separately checked predictive window supplies the normal native-10
|
||||||
|
+ # shape. Preserve the original ticket and timing on subsequent frames.
|
||||||
|
+ predictive_active = bool(
|
||||||
|
+ predictive_continuing and predictive_drive and can_zoom_in and zoom_cmd > 0.0
|
||||||
|
+ and pulse_family == "normal" and st.get("zoom_step_pending")
|
||||||
|
+ and predictive.owns(st)
|
||||||
|
+ and st.get("zoom_step_pending_id") == decision_request
|
||||||
|
+ and st.get("predictive_zoom_request") == decision_request
|
||||||
|
+ and 0 <= zoom_gate_now - now <= predictive.cfg.frame_age_s
|
||||||
|
+ )
|
||||||
|
+ predictive_start = bool(
|
||||||
|
+ predictive_entry and predictive_drive and pulse_family == "normal"
|
||||||
|
+ and zoom_cmd > 0.0 and not st.get("zoom_step_pending") and predictive.owns(st)
|
||||||
|
+ and not st.get("zoom_stop_obligation_active") and not st.get("lens_move_stop_required")
|
||||||
|
+ and 0 <= zoom_gate_now - now <= predictive.cfg.frame_age_s
|
||||||
|
+ and 0 <= zoom_gate_now - predictive_frame.detector_at <= predictive.cfg.detector_age_s
|
||||||
|
+ and predictive.optics.fresh(zoom_gate_now)
|
||||||
|
+ and (not skip_wait or predictive.repeat_after_stop(st, zoom_gate_now))
|
||||||
|
+ )
|
||||||
|
before_shape = zoom_cmd
|
||||||
|
zoom_cmd, useful_reason = _shape_useful_zoom_pulse(
|
||||||
|
zoom_cmd,
|
||||||
|
@@ -9299,6 +9421,21 @@
|
||||||
|
requested_native=timing_native,
|
||||||
|
rejection=response_rejection,
|
||||||
|
)
|
||||||
|
+ if predictive_start or (predictive_active and not near_edge and not edge_recovery_active
|
||||||
|
+ and zoom_rearm_ready and not normal_size_hold and not want_out):
|
||||||
|
+ zoom_cmd = 0.10
|
||||||
|
+ useful_reason = "useful"
|
||||||
|
+ st["zoom_useful_pulse_reason"] = useful_reason
|
||||||
|
+ response_timing = predictive_decision.window if predictive_start else (
|
||||||
|
+ float(st["zoom_step_pending_pulse_sec"]),
|
||||||
|
+ float(st["zoom_step_pending_hard_sec"]),
|
||||||
|
+ )
|
||||||
|
+ if predictive_decision and (
|
||||||
|
+ predictive_decision.stop or (predictive_decision.reason == "size_reached"
|
||||||
|
+ and pulse_family == "normal")
|
||||||
|
+ ):
|
||||||
|
+ zoom_cmd = min(zoom_cmd, 0.0)
|
||||||
|
+ zoom_zero_reason = predictive_decision.reason
|
||||||
|
if useful_reason == "useful" and response_timing == (0.0, 0.0):
|
||||||
|
zoom_cmd = 0.0
|
||||||
|
st["zoom_useful_pulse_reason"] = "wait_response_framing"
|
||||||
|
@@ -9321,7 +9458,10 @@
|
||||||
|
optics_observed_now=zoom_gate_now,
|
||||||
|
stop_trace=gate_stop,
|
||||||
|
continuation_trace=continuation_trace,
|
||||||
|
- )
|
||||||
|
+ predictive=predictive if predictive_start else None,
|
||||||
|
+ )
|
||||||
|
+ if predictive_start and zoom_cmd > 0.0 and st["zoom_step_request_id"] > request_id_before_gate:
|
||||||
|
+ st["predictive_zoom_request"] = st["zoom_step_pending_id"]
|
||||||
|
if gate_stop:
|
||||||
|
zoom_zero_reason = gate_stop[-1]
|
||||||
|
decision_generation = st.get("ptz_generation")
|
||||||
|
@@ -9380,6 +9520,12 @@
|
||||||
|
|
||||||
|
adj_pan, adj_tilt = _shape_tracking_cmd(st, now, adj_pan, adj_tilt, center_err)
|
||||||
|
pan_feedback_cmd, tilt_feedback_cmd = float(adj_pan), float(adj_tilt)
|
||||||
|
+ if predictive_active and predictive.request is not None and zoom_cmd > 0.0:
|
||||||
|
+ pan_optical, tilt_optical = predictive_decision.ff_deg_s
|
||||||
|
+ pan_ref = max(1.0, float(cfg_ptz.get("ptz_pan_max_speed_deg_s", getattr(config, "AIM_PAN_SPEED_DEG_S", 160.0))))
|
||||||
|
+ tilt_ref = max(1.0, float(cfg_ptz.get("ptz_tilt_max_speed_deg_s", getattr(config, "AIM_TILT_SPEED_DEG_S", 120.0))))
|
||||||
|
+ pan_rate_ff += pan_optical / pan_ref
|
||||||
|
+ tilt_rate_ff += tilt_optical / tilt_ref
|
||||||
|
adj_pan, adj_tilt = _merge_gimbal_rate_feedforward(
|
||||||
|
st,
|
||||||
|
now,
|
||||||
|
@@ -9844,6 +9990,14 @@
|
||||||
|
("brake", final_guard_active),
|
||||||
|
) if blocked)
|
||||||
|
zoom_decision = (source_seq, decision_request, zoom_zero_reason, "+".join(blockers) or "-")
|
||||||
|
+ if predictive_decision is not None:
|
||||||
|
+ with _zoom_command_lock(cam_idx):
|
||||||
|
+ telemetry_tick(
|
||||||
|
+ config, cam_idx, predictive, predictive_frame, predictive_decision,
|
||||||
|
+ zoom_cmd, zoom_zero_reason, time.time(),
|
||||||
|
+ )
|
||||||
|
+ if predictive_decision.stop and predictive.request is not None:
|
||||||
|
+ stop_predictive_zoom_async(cam_idx, predictive, predictive_decision.reason)
|
||||||
|
dispatch_kwargs = {"force": force_track_ptz}
|
||||||
|
if zoom_decision is not None:
|
||||||
|
dispatch_kwargs["zoom_decision"] = zoom_decision
|
||||||
|
--- a/ptz_io.py
|
||||||
|
+++ b/ptz_io.py
|
||||||
|
@@ -14,6 +14,7 @@
|
||||||
|
from requests.adapters import HTTPAdapter
|
||||||
|
|
||||||
|
from . import config, state
|
||||||
|
+from .zoom_controller import transport_accepted, transport_confirmed, transport_stopped
|
||||||
|
from .utils import clipf, hfov_ptz_from_abs_zoom, hfov_ptz_from_zoom_ratio, quantf
|
||||||
|
from .geom import sector_contains, ang_diff_signed, normalize360
|
||||||
|
|
||||||
|
@@ -582,6 +583,10 @@
|
||||||
|
st["zoom_last_motion_at"] = float(now)
|
||||||
|
st["zoom_followup_reason"] = "physical_pulse"
|
||||||
|
st["zoom_step_last_delivery_result"] = "physical_started"
|
||||||
|
+ transport_accepted(
|
||||||
|
+ config, cam_idx, st, request_id, native_cmd, deadline_origin, now,
|
||||||
|
+ deadline_origin + hard_sec,
|
||||||
|
+ )
|
||||||
|
logger.info(
|
||||||
|
"[ZOOM DELIVERY] cam=%s request=%s kind=%s physical start seq=%s pulse=%.3fs hard=%.3fs",
|
||||||
|
cam_idx,
|
||||||
|
@@ -744,6 +749,8 @@
|
||||||
|
float(st.get("zoom_recenter_until", 0.0) or 0.0),
|
||||||
|
float(now) + float(getattr(config, "ZOOM_STEP_RECENTER_SEC", 0.14)),
|
||||||
|
)
|
||||||
|
+ if reason == "status_poll":
|
||||||
|
+ transport_confirmed(config, st, request_id, now)
|
||||||
|
logger.info(
|
||||||
|
"[ZOOM DELIVERY] cam=%s request=%s optics confirmed source=%s zstep=%s",
|
||||||
|
cam_idx,
|
||||||
|
@@ -922,6 +929,12 @@
|
||||||
|
+ max(0.0, float(getattr(config, "ZOOM_STEP_RETRY_COOLDOWN_SEC", 0.12))),
|
||||||
|
)
|
||||||
|
|
||||||
|
+ transport_stopped(
|
||||||
|
+ config, cam_idx, st, request_id, now, reason=reason,
|
||||||
|
+ confirmed=confirmed and not discard,
|
||||||
|
+ clean=bool(not discard and kind == "normal" and delivered + 1e-6 >= full_window
|
||||||
|
+ and stop_axes is not None and stop_axes[2] == 0),
|
||||||
|
+ )
|
||||||
|
st["zoom_step_last_delivered_sec"] = float(delivered)
|
||||||
|
st["zoom_step_pending"] = False
|
||||||
|
st["zoom_step_pending_id"] = 0
|
||||||
|
@@ -2119,6 +2132,35 @@
|
||||||
|
if zoom_decision is not None:
|
||||||
|
return cmd_source, cmd_generation, move_seq, zoom_i
|
||||||
|
return None
|
||||||
|
+
|
||||||
|
+
|
||||||
|
+def stop_predictive_zoom_async(cam_idx, controller, reason):
|
||||||
|
+ """Order an early lens STOP independently of the ordinary MOVE queue."""
|
||||||
|
+ with _zoom_command_lock(cam_idx):
|
||||||
|
+ st = state.ptz_states.get(cam_idx, {})
|
||||||
|
+ request = controller.request
|
||||||
|
+ if (not controller.owns(st) or request is None
|
||||||
|
+ or st.get("zoom_step_physical_request_id") != request
|
||||||
|
+ or st.get("predictive_zoom_early_stop") == request):
|
||||||
|
+ return
|
||||||
|
+ st["predictive_zoom_early_stop"] = request
|
||||||
|
+ st["zoom_step_cancel_requested"] = True
|
||||||
|
+
|
||||||
|
+ def stop_if_current():
|
||||||
|
+ with _zoom_command_lock(cam_idx):
|
||||||
|
+ current = state.ptz_states.get(cam_idx, {})
|
||||||
|
+ if (controller.owns(current)
|
||||||
|
+ and current.get("zoom_step_physical_request_id") == request):
|
||||||
|
+ _deliver_priority_lens_stop_locked(
|
||||||
|
+ cam_idx, current, reason=f"predictive:{reason}",
|
||||||
|
+ guard_inflight_after_put=_generic_put_has_lens_motion(current),
|
||||||
|
+ )
|
||||||
|
+
|
||||||
|
+ try:
|
||||||
|
+ threading.Thread(target=stop_if_current, name="ptz-predictive-stop", daemon=True).start()
|
||||||
|
+ except RuntimeError:
|
||||||
|
+ # The scheduled watchdog remains the independent hard STOP.
|
||||||
|
+ logger.exception("Unable to start early lens STOP cam=%s", cam_idx)
|
||||||
|
|
||||||
|
|
||||||
|
def stop_ptz(cam_idx: int, *, source: str = "stop", generation: Optional[int] = None) -> None:
|
||||||
|
--- a/status.py
|
||||||
|
+++ b/status.py
|
||||||
|
@@ -170,6 +170,13 @@
|
||||||
|
if isinstance(hfov_deg, (int, float)) and float(hfov_deg) > 0.0:
|
||||||
|
st["hfov_deg"] = float(hfov_deg)
|
||||||
|
st["zoom_status_at"] = zoom_observed_at
|
||||||
|
+ if config.CAMERA_CONFIG.get(int(cam_id), {}).get("predictive_zoom") is not None:
|
||||||
|
+ previous = st.get("predictive_zoom_hfov", (0.0, 0.0))
|
||||||
|
+ if zoom_observed_at > previous[0]:
|
||||||
|
+ st["predictive_zoom_hfov"] = (zoom_observed_at, float(hfov_deg))
|
||||||
|
+ controller = st.get("predictive_zoom_controller")
|
||||||
|
+ if controller is not None and controller.owns(st):
|
||||||
|
+ controller.optics.observe(zoom_observed_at, float(hfov_deg), float(now))
|
||||||
|
if isinstance(zoom_raw, (int, float)):
|
||||||
|
st["zoom_position_raw"] = float(zoom_raw)
|
||||||
|
st["zoom_position_tag"] = str(zoom_tag or "")
|
||||||
@ -0,0 +1,172 @@
|
|||||||
|
import math
|
||||||
|
from pathlib import Path
|
||||||
|
import tempfile
|
||||||
|
import unittest
|
||||||
|
|
||||||
|
from zoom_controller import EpisodeTelemetryLogger, OpticalStateEstimator, ZoomConfig, ZoomController, ZoomFrame
|
||||||
|
|
||||||
|
|
||||||
|
def frame(at, seq, *, offset=(0.13, -0.03), pixels=(20.0, 16.0),
|
||||||
|
detector_at=None, score=0.8, geometry=True):
|
||||||
|
return ZoomFrame(
|
||||||
|
at=at, seq=seq, offset=offset, bbox_frac=(pixels[0] / 1280, pixels[1] / 720),
|
||||||
|
bbox_px=pixels, angles=(0.0, 0.0), rates=(0.0, 0.0), aspect=720 / 1280,
|
||||||
|
tracker_score=score, detector_at=at if detector_at is None else detector_at,
|
||||||
|
detector_conf=0.71, geometry_ok=geometry, los_valid=True,
|
||||||
|
)
|
||||||
|
|
||||||
|
|
||||||
|
def tick(controller, sample, now=None, **kw):
|
||||||
|
flags = {"first": controller.first_ack_at is None, "cooldown": False, "transport_clear": True}
|
||||||
|
flags.update(kw)
|
||||||
|
return controller.decide(sample, sample.at if now is None else now, **flags)
|
||||||
|
|
||||||
|
|
||||||
|
class PredictiveZoomTests(unittest.TestCase):
|
||||||
|
def setUp(self):
|
||||||
|
self.cfg = ZoomConfig(nominal_gain=0.65, upper_gain=1.0)
|
||||||
|
self.c = ZoomController(self.cfg, (4, 100.0))
|
||||||
|
self.c.optics.observe(99.99, 59.0, 100.0)
|
||||||
|
|
||||||
|
def test_short_3_6_second_flight_with_delayed_yolo_and_status(self):
|
||||||
|
c = self.c
|
||||||
|
focal = math.tan(math.radians(59 / 2))
|
||||||
|
duration = 3.6
|
||||||
|
active = None
|
||||||
|
pulses, stops = [], []
|
||||||
|
actual_status = 99.99
|
||||||
|
between_without_poll = 0
|
||||||
|
last_stop = None
|
||||||
|
visible = 0
|
||||||
|
for k in range(181):
|
||||||
|
t = k * 0.02
|
||||||
|
now = 100.0 + t
|
||||||
|
if active is not None:
|
||||||
|
focal *= math.exp(-self.cfg.log_rate * 0.02)
|
||||||
|
actual = math.degrees(2 * math.atan(focal))
|
||||||
|
# One status response every 250 ms, no forced request after STOP.
|
||||||
|
if k % 13 == 8:
|
||||||
|
c.optics.observe(now, actual, now)
|
||||||
|
actual_status = now
|
||||||
|
if active and now >= active[1]:
|
||||||
|
pulse, _, started, status_at_start = active
|
||||||
|
confirmed = actual_status > status_at_start
|
||||||
|
c.stopped(pulse, now, confirmed=confirmed, clean=True)
|
||||||
|
stops.append(now)
|
||||||
|
active = None
|
||||||
|
last_stop = now
|
||||||
|
if k % 4 != 0:
|
||||||
|
continue
|
||||||
|
# Same aircraft, steady closing LOS and native-10 synthetic plant.
|
||||||
|
x = 0.129 - min(0.069, t * 0.12)
|
||||||
|
optical_scale = math.tan(math.radians(59 / 2)) / focal
|
||||||
|
px = (20 * optical_scale, 16 * optical_scale)
|
||||||
|
self.assertLess(abs(x) + px[0] / 2560, 0.5)
|
||||||
|
visible += 1
|
||||||
|
detector = 100.0 + math.floor((t + 1e-9) / 0.52) * 0.52
|
||||||
|
can_repeat = last_stop is None or c.can_reuse_feedback(now, last_stop) or actual_status > last_stop
|
||||||
|
decision = tick(c, frame(now, k + 1, offset=(x, -0.025), pixels=px,
|
||||||
|
detector_at=detector),
|
||||||
|
transport_clear=active is None and can_repeat)
|
||||||
|
if decision.window:
|
||||||
|
if last_stop is not None and actual_status <= last_stop:
|
||||||
|
between_without_poll += 1
|
||||||
|
self.assertIsNone(active)
|
||||||
|
pulse = len(pulses) + 1
|
||||||
|
nominal, hard = decision.window
|
||||||
|
c.accepted(pulse, now, now, now + hard)
|
||||||
|
active = pulse, now + nominal, now, actual_status
|
||||||
|
pulses.append(t)
|
||||||
|
else:
|
||||||
|
if decision.stop:
|
||||||
|
self.assertEqual(decision.reason, "size_reached")
|
||||||
|
c.stopped(active[0], now, clean=False, confirmed=False)
|
||||||
|
last_stop = now
|
||||||
|
stops.append(now)
|
||||||
|
active = None
|
||||||
|
self.assertEqual(visible, 46)
|
||||||
|
self.assertLess(pulses[0], 0.25)
|
||||||
|
self.assertGreaterEqual(len(pulses), 4)
|
||||||
|
self.assertGreaterEqual(between_without_poll, 2)
|
||||||
|
self.assertGreaterEqual(len(stops), 3)
|
||||||
|
self.assertLess(actual, 42.0)
|
||||||
|
|
||||||
|
def test_wide_entry_without_next_detector_and_narrow_entry_is_stricter(self):
|
||||||
|
for k in range(3):
|
||||||
|
self.assertEqual(tick(self.c, frame(100 + .08 * k, k + 1, detector_at=99.96)).reason,
|
||||||
|
"fast_start" if k == 2 else "low_tracker_score")
|
||||||
|
other = ZoomController(self.cfg, (4, 100))
|
||||||
|
other.optics.observe(99.99, 28.0, 100.0)
|
||||||
|
for k in range(3):
|
||||||
|
d = tick(other, frame(100 + .08 * k, k + 1, detector_at=99.96))
|
||||||
|
self.assertEqual(d.reason, "not_centered")
|
||||||
|
|
||||||
|
def test_reject_outward_low_score_and_expired_detector(self):
|
||||||
|
for k in range(3):
|
||||||
|
d = tick(self.c, frame(100 + .08 * k, k + 1, offset=(.075 + .003 * k, -.02)))
|
||||||
|
self.assertEqual(d.reason, "not_centered")
|
||||||
|
self.assertEqual(tick(self.c, frame(100.20, 4, score=.2)).reason, "low_tracker_score")
|
||||||
|
self.assertEqual(tick(self.c, frame(100.28, 5, detector_at=99.0)).reason, "waiting_yolo")
|
||||||
|
|
||||||
|
def test_positive_ff_and_edge_stop_preserve_request_deadline(self):
|
||||||
|
self.c.accepted(4, 100.0, 100.01, 100.4)
|
||||||
|
active = self.c.optics.active
|
||||||
|
d = tick(self.c, frame(100.05, 1, offset=(.08, -.05)))
|
||||||
|
self.assertGreater(d.ff_deg_s[0], 0)
|
||||||
|
self.assertLess(d.ff_deg_s[1], 0)
|
||||||
|
d = tick(self.c, frame(100.12, 2, offset=(.32, -.05)))
|
||||||
|
self.assertTrue(d.stop)
|
||||||
|
self.assertEqual(d.reason, "edge_margin_low")
|
||||||
|
self.assertEqual(d.ff_deg_s, (0.0, 0.0))
|
||||||
|
self.assertEqual(self.c.optics.active, active)
|
||||||
|
|
||||||
|
def test_edge_acceleration_can_stop_before_margin_crosses(self):
|
||||||
|
c = ZoomController(ZoomConfig(.28, .45, edge_accel=.1, edge_watch_margin=.35), (4, 100))
|
||||||
|
c.optics.observe(99.99, 59, 100)
|
||||||
|
c.accepted(2, 100, 100.01, 100.4)
|
||||||
|
for seq, off in enumerate((.145, .145, .153), 1):
|
||||||
|
decision = tick(c, frame(100 + seq * .06, seq, offset=(off, -.02)))
|
||||||
|
self.assertGreater(decision.margin, c.cfg.edge_margin)
|
||||||
|
self.assertEqual(decision.reason, "edge_acceleration")
|
||||||
|
self.assertTrue(decision.stop)
|
||||||
|
|
||||||
|
def test_causal_status_and_failed_or_ambiguous_stop_cannot_rearm(self):
|
||||||
|
c = self.c
|
||||||
|
c.accepted(4, 100.0, 100.01, 100.4)
|
||||||
|
before = c.optics.estimate(100.1)
|
||||||
|
c.optics.observe(100.18, 55, 100.2)
|
||||||
|
c.optics.observe(100.08, 57, 100.21)
|
||||||
|
self.assertEqual(c.optics.estimate(100.1), before)
|
||||||
|
self.assertEqual(c.optics.status_at, 100.18)
|
||||||
|
self.assertFalse(c.can_reuse_feedback(100.35, 100.34))
|
||||||
|
c.stopped(4, 100.34, confirmed=True, clean=False)
|
||||||
|
self.assertFalse(c.can_reuse_feedback(100.35, 100.34))
|
||||||
|
|
||||||
|
def test_fov_curves_and_watchdog_bounds(self):
|
||||||
|
self.assertEqual(self.cfg.window(59), (.34, .40))
|
||||||
|
self.assertEqual(self.cfg.window(10), (.12, .18))
|
||||||
|
self.assertGreater(self.cfg.window(21)[0], self.cfg.window(13)[0])
|
||||||
|
for invalid in (dict(wide_pulse_s=.5), dict(native=20), dict(frame_age_s=.26),
|
||||||
|
dict(upper_gain=.4), dict(nominal_gain=float("nan"))):
|
||||||
|
args = {"nominal_gain": .65, "upper_gain": 1.0}
|
||||||
|
args.update(invalid)
|
||||||
|
with self.assertRaises(ValueError):
|
||||||
|
ZoomConfig(**args)
|
||||||
|
|
||||||
|
def test_telemetry_writes_first_and_ticks_separately(self):
|
||||||
|
import json
|
||||||
|
with tempfile.TemporaryDirectory() as directory:
|
||||||
|
logger = EpisodeTelemetryLogger(directory)
|
||||||
|
logger.submit(8, (4, 100.0), "first_zoom", 100.2, t_to_first_zoom_ms=200)
|
||||||
|
logger.submit(8, (4, 100.0), "tick", 100.25, FOV_actual=59.0,
|
||||||
|
FOV_estimated=57.0, bbox_px=[20, 16], detector_conf=.71)
|
||||||
|
logger.queue.join()
|
||||||
|
path, = Path(directory).glob("*.jsonl")
|
||||||
|
first, second = map(json.loads, path.read_text().splitlines())
|
||||||
|
self.assertEqual(first["t_to_first_zoom_ms"], 200)
|
||||||
|
self.assertEqual(second["elapsed_ms"], 250)
|
||||||
|
self.assertEqual(second["FOV_estimated"], 57)
|
||||||
|
|
||||||
|
|
||||||
|
if __name__ == "__main__":
|
||||||
|
unittest.main()
|
||||||
@ -0,0 +1,156 @@
|
|||||||
|
"""Integration cases for the PR #7 mock transport/control replay fixture."""
|
||||||
|
|
||||||
|
import math
|
||||||
|
from types import SimpleNamespace
|
||||||
|
from unittest.mock import patch
|
||||||
|
|
||||||
|
TRACE = []
|
||||||
|
|
||||||
|
def build_track_cases(fixture, pp, ptz_io, config, state, status):
|
||||||
|
class PredictiveTrack(fixture.Replay):
|
||||||
|
def setUp(self):
|
||||||
|
cameras = [8, *[cam for cam in config.PTZ_CAM_IDS if cam != 8]]
|
||||||
|
with patch.object(config, 'PTZ_CAM_IDS', cameras):
|
||||||
|
super().setUp()
|
||||||
|
|
||||||
|
def flight(self, enabled, center_x=720, *, record=False):
|
||||||
|
f, st = self.fixture, self.st
|
||||||
|
if enabled:
|
||||||
|
self.stack.enter_context(patch.dict(config.CAMERA_CONFIG[self.cam], {
|
||||||
|
"predictive_zoom": {"nominal_gain": .45, "upper_gain": .8},
|
||||||
|
}))
|
||||||
|
self.stack.enter_context(patch.object(config, "PTZ_ZOOM_STOP_FORCE_AXES_ZERO", False, create=True))
|
||||||
|
self.stack.enter_context(patch.object(config, "PREDICTIVE_ZOOM_TELEMETRY_DIR", None, create=True))
|
||||||
|
decisions = []
|
||||||
|
capture = pp.telemetry_tick
|
||||||
|
def remember(config, cam, controller, frame, decision, zoom, why, at):
|
||||||
|
decisions.append((round(at-100, 2), decision.reason, round(zoom, 3), why,
|
||||||
|
st.get("zoom_followup_reason"), st.get("zoom_step_last_delivery_result")))
|
||||||
|
estimated, narrowest = controller.optics.estimate(at)
|
||||||
|
if record:
|
||||||
|
TRACE.append({
|
||||||
|
"event": "tick", "cam": cam + 11, "elapsed_ms": round((at-100)*1000, 1),
|
||||||
|
"FOV_actual": controller.optics.actual, "FOV_estimated": estimated,
|
||||||
|
"FOV_min_estimated": narrowest, "bbox_px": frame.bbox_px,
|
||||||
|
"detector_conf": frame.detector_conf, "tracker_score": frame.tracker_score,
|
||||||
|
"reason": decision.reason,
|
||||||
|
})
|
||||||
|
capture(config, cam, controller, frame, decision, zoom, why, at)
|
||||||
|
self.stack.enter_context(patch.object(pp, "telemetry_tick", side_effect=remember))
|
||||||
|
box = (center_x - 13, 347, center_x + 13, 371)
|
||||||
|
st.update(track_started_at=99.95, last_bbox=box,
|
||||||
|
prev_cx=center_x / 1280, prev_cy=359 / 720, stable_frames=0)
|
||||||
|
fov, zoom = 59.0, 1.0
|
||||||
|
status._update_runtime_state(self.cam, None, None, 99.99, zoom_ratio=zoom, hfov_deg=fov)
|
||||||
|
delivered = []
|
||||||
|
last_status = 99.99
|
||||||
|
completed = []
|
||||||
|
early = []
|
||||||
|
for k in range(46):
|
||||||
|
f._advance(.08 if k else 0)
|
||||||
|
now = f.wall
|
||||||
|
if k % 6 == 0:
|
||||||
|
st.update(visual_tracker_last_detector_at=now - .015,
|
||||||
|
visual_tracker_last_zoom_detector_at=now - .015,
|
||||||
|
visual_tracker_last_detector_conf=.8)
|
||||||
|
if k % 3 == 1:
|
||||||
|
if st.get("zoom_step_physical_request_id"):
|
||||||
|
fov *= .97
|
||||||
|
zoom *= 1.03
|
||||||
|
status._update_runtime_state(self.cam, None, None, now - .012,
|
||||||
|
zoom_ratio=zoom, hfov_deg=fov)
|
||||||
|
last_status = now - .012
|
||||||
|
st.update(status_rate_at=now - .01, pan_rate_at=now - .01,
|
||||||
|
tilt_rate_at=now - .01, pan_rate_deg_s=.4, tilt_rate_deg_s=0.0)
|
||||||
|
timer = st.get("zoom_step_stop_timer")
|
||||||
|
if (timer and timer.is_alive()
|
||||||
|
and f.mono + 1e-9 >= st["zoom_step_stop_started_mono"] + timer.delay):
|
||||||
|
timer.fire()
|
||||||
|
request_before = st.get("zoom_step_physical_request_id")
|
||||||
|
self.seq += 1
|
||||||
|
scale = math.tan(math.radians(59 / 2)) / math.tan(math.radians(fov / 2))
|
||||||
|
half_w, half_h = round(13 * scale), round(12 * scale)
|
||||||
|
box = (center_x - half_w, 359 - half_h, center_x + half_w, 359 + half_h)
|
||||||
|
self.assertTrue(0 < box[0] < box[2] < 1280 and 0 < box[1] < box[3] < 720)
|
||||||
|
track = fixture.tracker.VisualTrackResult(True, box, .8, 0, .8, 30,
|
||||||
|
"synthetic captured", self.seq)
|
||||||
|
sample = fixture.control.VisualSample(fixture.control.owner(st), self.seq, now-.01, track)
|
||||||
|
pp.postprocess_control(self.cam, self.frame, SimpleNamespace(boxes=fixture.Boxes([])),
|
||||||
|
True, self.seq, visual_sample=sample, visual_only=True)
|
||||||
|
if not state.ptz_cmd_q.empty():
|
||||||
|
f._flush()
|
||||||
|
request = st.get("zoom_step_physical_request_id")
|
||||||
|
if request and request != request_before:
|
||||||
|
delivered.append((round(now-100, 2), request, last_status))
|
||||||
|
if st.get("predictive_zoom_request") == request and st["zoom_step_finished_at"] > last_status:
|
||||||
|
early.append(request)
|
||||||
|
if request_before and not request:
|
||||||
|
completed.append(request_before)
|
||||||
|
self.assertEqual(st["mode"], "TRACK")
|
||||||
|
return delivered, early, fov, decisions, completed
|
||||||
|
|
||||||
|
def test_track_runs_burst_without_post_stop_status(self):
|
||||||
|
delivered, early, fov, decisions, completed = self.flight(True, record=True)
|
||||||
|
self.assertTrue(delivered, decisions)
|
||||||
|
TRACE.insert(0, {"event": "first_zoom", "t_to_first_zoom_ms": delivered[0][0]*1000})
|
||||||
|
self.assertGreaterEqual(len(delivered), 3, (delivered, decisions))
|
||||||
|
self.assertLess(delivered[0][0], .5, decisions)
|
||||||
|
self.assertGreaterEqual(len(early), 1, (early, delivered, decisions))
|
||||||
|
print(f"3.6s TRACK: {len(delivered)} starts, first {delivered[0][0]:.2f}s, "
|
||||||
|
f"{len(early)} before a post-STOP poll, terminal simulated HFOV {fov:.1f} deg")
|
||||||
|
self.assertTrue(any(row[1] == "burst" for row in decisions), decisions)
|
||||||
|
self.assertGreaterEqual(self.st["zoom_in_pulse_count"], 2, (completed, decisions))
|
||||||
|
self.assertLess(fov, 59.0)
|
||||||
|
def test_track_same_flight_legacy_reference(self):
|
||||||
|
delivered, early, fov, decisions, completed = self.flight(False)
|
||||||
|
first = delivered[0][0] if delivered else None
|
||||||
|
print(f"3.6s legacy TRACK: {len(delivered)} starts, first {first}s, terminal simulated HFOV {fov:.1f} deg")
|
||||||
|
self.assertLess(len(delivered), 7)
|
||||||
|
self.assertFalse(early)
|
||||||
|
|
||||||
|
def test_track_starts_at_26_percent_of_half_frame(self):
|
||||||
|
delivered, _, _, decisions, _ = self.flight(True, 806)
|
||||||
|
self.assertTrue(delivered, decisions)
|
||||||
|
self.assertLess(delivered[0][0], .5)
|
||||||
|
self.assertTrue(any(reason == "fast_start" and zoom > 0
|
||||||
|
for _, reason, zoom, *_ in decisions), decisions)
|
||||||
|
|
||||||
|
def test_track_legacy_reference_at_26_percent_of_half_frame(self):
|
||||||
|
delivered, _, _, _, _ = self.flight(False, 806)
|
||||||
|
print(f"3.6s legacy TRACK at 26% half-frame: {len(delivered)} starts")
|
||||||
|
self.assertFalse(delivered)
|
||||||
|
|
||||||
|
def assert_early_stop_axes(self, full_stop):
|
||||||
|
f, st = self.fixture, self.st
|
||||||
|
from ptz_tracker_modular.zoom_controller import controller_for_track
|
||||||
|
self.stack.enter_context(patch.dict(config.CAMERA_CONFIG[self.cam], {
|
||||||
|
"predictive_zoom": {"nominal_gain": .45, "upper_gain": .8},
|
||||||
|
}))
|
||||||
|
controller = controller_for_track(st, config.CAMERA_CONFIG[self.cam])
|
||||||
|
controller.optics.observe(99.95, 59.0, f.wall)
|
||||||
|
cmd = pp._gate_zoom_in_pulse(st, f.wall, .10, True, pulse_floor=.10, pulse_timing=(.34, .40))
|
||||||
|
st["predictive_zoom_request"] = st["zoom_step_pending_id"]
|
||||||
|
f._enqueue(cmd, pan=.12, tilt=-.08)
|
||||||
|
f._flush()
|
||||||
|
request = controller.request
|
||||||
|
self.assertIsNotNone(request)
|
||||||
|
class InlineThread:
|
||||||
|
def __init__(self, *, target, **_):
|
||||||
|
self.target = target
|
||||||
|
def start(self):
|
||||||
|
self.target()
|
||||||
|
self.stack.enter_context(patch.object(ptz_io.threading, "Thread", InlineThread))
|
||||||
|
self.stack.enter_context(patch.object(config, "PTZ_ZOOM_STOP_FORCE_AXES_ZERO", full_stop, create=True))
|
||||||
|
ptz_io.stop_predictive_zoom_async(self.cam, controller, "edge_acceleration")
|
||||||
|
axes = tuple(fixture.transport.xml_axis(f.sent[-1], axis) for axis in ("pan", "tilt", "zoom"))
|
||||||
|
self.assertEqual(axes, (0, 0, 0) if full_stop else (12, -8, 0))
|
||||||
|
self.assertIsNone(controller.request)
|
||||||
|
self.assertFalse(controller.repeat_after_stop(st, f.wall))
|
||||||
|
|
||||||
|
def test_priority_zoom_stop_honors_axis_policy(self):
|
||||||
|
self.assert_early_stop_axes(full_stop=False)
|
||||||
|
|
||||||
|
def test_priority_zoom_stop_does_not_override_full_stop(self):
|
||||||
|
self.assert_early_stop_axes(full_stop=True)
|
||||||
|
|
||||||
|
return PredictiveTrack
|
||||||
Loading…
Reference in New Issue