"""Run PR7 and PR8 on identical trajectories reconstructed from logged AIM.""" from bisect import bisect_right from collections import Counter, deque from contextlib import ExitStack from hashlib import sha256 from pathlib import Path from statistics import median from types import ModuleType, SimpleNamespace from unittest.mock import patch import argparse import cv2 import json from inspect import unwrap import logging import math import os import socket import sys OUT = Path(os.environ.get( "PTZ_BROAD_TELE_REPLAY_DIR", Path(__file__).resolve().parents[3] / "ptz_broad_tele_20261111", )) DATA = OUT.parent / "ptz_native10_profiles_20261111" sys.path.insert(0, str(DATA)) from measure_native10 import FIELD, PULSES, PR7, FULL_SWEEP_LOG_RATE, ROOT, read, utc LAB = ROOT / "ptz_virtual_lab_20261111" sys.path.insert(0, str(LAB)) import simulator as sim CANDIDATE = ROOT / "ptz_predictive_zoom_candidate_20261111" BUNDLE = ROOT / "chameleon_predictive_zoom_git_20261111/integration/ptz_predictive_zoom" BASE_STATUS = sim.SOURCE / "status.py" NEW = OUT / "candidate" NEW_PINS = OUT / "pins.json" TRACE_KEYS = ("normal_zoom_quality_ok", "zoom_quality_ok", "zoom_rearm_ready", "zoom_safe", "zoom_angular_safe", "stable_ok", "near_edge", "edge_recovery_active", "followup_ready", "raw_zoom_needed", "normal_size_hold", "final_guard_active", "visual_control_stale", "visual_control_coast", "zoom_zero_reason", "zoom_cmd", "can_zoom_in", "predictive_decision") FLIGHTS = { "19": ("03:11:07", "04:54:08"), "21": ("04:13:33", "04:37:45"), } def angle(error, fov): return math.degrees(math.atan(2*error*math.tan(math.radians(fov/2)))) def trajectory(camera, episode, source): start, end = map(utc, (episode["started_at"], episode["ended_at"])) candidates = [r for r in source["cameras"][camera]["samples"] if start <= r["t"] <= min(end, start+25) and r["err"] and r["fov"] and r["gimbal"] and r["optics_age"] is not None and r["optics_age"] <= .65 and r["source"] in ("visual","detector","acquire")] groups = [] for r in candidates: if not groups or r["t"] - groups[-1][-1]["t"] > 1.65: groups.append([]) groups[-1].append(r) # Prefer the longest contiguous trace so neither controller bridges a gap. rows = max(groups, key=lambda g: g[-1]["t"]-g[0]["t"]) origin = rows[0]["t"] pose = [0.0, 0.0] samples = [] for i,r in enumerate(rows): if i: previous = rows[i-1] dt = r["t"] - previous["t"] for axis, sign in enumerate((1,-1)): pose[axis] += sign * dt * (r["gimbal"][axis]+previous["gimbal"][axis])/2 bearing = tuple(pose[k]+angle(r["err"][k], r["fov"][k]) for k in range(2)) samples.append((r["t"]-origin,*bearing)) duration = math.floor(samples[-1][0]*100)/100 if duration < 3: raise ValueError(f"Insufficient contiguous AIM trace: camera {camera} {episode['started_at']}") # Only a scalar filtered size is available. Use a projected square with that # geometric mean at initial FOV and hold angular extent constant. first_size = episode["size_first"] or .018 fov_x, fov_y = rows[0]["fov"] angular = tuple(2*angle(first_size/2, fov) for fov in (fov_x, fov_y)) return dict(camera=int(camera), episode=episode["started_at"], aim_start=origin, duration=duration, knots=samples, angular_extent=angular, source_rows=len(samples), observed_hfov=rows[0]["fov"]) class FlightCamera(sim.Camera): def __init__(self, parameters, pan_sign, tilt_sign, trace, initial_fov): super().__init__(parameters, pan_sign, tilt_sign) self.trace = trace self.times = [x[0] for x in trace["knots"]] self.magnification = sim.OPTICAL_ZOOM ** ( math.log(math.tan(math.radians(59/2))/math.tan(math.radians(initial_fov/2))) / math.log(math.tan(math.radians(59/2))/math.tan(math.radians(2/2))) ) if initial_fov < 20: _, az, el = trace["knots"][0] # Keep this synthetic narrow-angle stress start inside the aperture. self.pan, self.tilt = az-.10*initial_fov, el-.05*initial_fov def target(self, t): i = min(len(self.times)-2, max(0, bisect_right(self.times,t)-1)) a,b = self.trace["knots"][i:i+2] frac = min(1,max(0,(t-a[0])/(b[0]-a[0]))) az, el = (x+(y-x)*frac for x,y in zip(a[1:],b[1:])) if self.trace.get("edge_stress"): # Same finite lateral acceleration for both controllers. push = min(1.1,max(0.0,t-.90)) az += 2.8 * push*push / 1.21 return az,el def project(self, t, *, clipped=True): center = self.target(t) bounds = [] for bearing, pose, width, fov, pixels in zip( center,(self.pan,self.tilt),self.trace["angular_extent"],(self.hfov,self.vfov),(1280,720) ): # stress small angles with a tiny object that still asks for zoom if self.trace.get("stress"): width *= .22 deviation = bearing-pose if abs(deviation)+width/2 >= 85: return None focal = (pixels/2)/math.tan(math.radians(fov/2)) lo,hi = [round(pixels/2+focal*math.tan(math.radians(deviation+o*width/2))) for o in (-1,1)] if clipped: lo,hi = max(0,lo),min(pixels-1,hi) if hi <= lo: return None bounds.append((lo,hi)) return (bounds[0][0],bounds[1][0],bounds[0][1],bounds[1][1]) def capture_control(pp, function): row = {} code = unwrap(pp.postprocess_control).__code__ def trace(frame,event,arg): if event == "call" and frame.f_code is code: frame.f_trace_lines = False return trace if event == "return" and frame.f_code is code: row.update({k:frame.f_locals[k] for k in TRACE_KEYS if k in frame.f_locals}) row["return_line"] = frame.f_lineno return None original = sys.gettrace() sys.settrace(trace) try: function() finally: sys.settrace(original) return row def blocker(row, st): """Primary gate plus all simultaneous blockers. Locals come from live policy.""" if "zoom_cmd" not in row: return "track_or_frame_rejected", [] if row["zoom_cmd"] > 0: return None,[] reasons = [ (row.get("final_guard_active"), "gimbal_brake"), (row.get("near_edge") or row.get("edge_recovery_active"), "edge_margin_low"), (row.get("visual_control_stale") or row.get("visual_control_coast"), "stale_visual"), (not row.get("zoom_rearm_ready",True), "geometry_rearm"), (not row.get("raw_zoom_needed",True) or row.get("normal_size_hold"), "size_controller"), (not row.get("zoom_quality_ok",True), "quality_or_validation"), (not row.get("followup_ready",True), "fov_cooldown"), (not row.get("zoom_safe",True), "framing"), (not row.get("zoom_angular_safe",True), "angular_limit"), (not row.get("stable_ok",True), "not_centered_or_not_stable"), ] flags = [why for enabled,why in reasons if enabled] predictive = row.get("predictive_decision") if predictive and predictive.stop: return predictive.reason,flags return (flags[0] if flags else row.get("zoom_zero_reason") or st.get("zoom_useful_pulse_reason") or "unknown"),flags def run(trace, profile, steering, revision, video_ms, isapi_ms, initial_fov=59., plant_stress=False, put_rtt=False): import numpy import torch cam = trace["camera"]-11 cfg = sim.Parameters(camera=trace["camera"], seconds=trace["duration"], video_ms=video_ms, command_ms=isapi_ms, optics_report_ms=isapi_ms, frame_hz=10, status_hz=4, steering_exponent=2, pan_at_10=steering["pan"]["native10_equivalent_deg_s"], tilt_at_10=steering["tilt"]["native10_equivalent_deg_s"], zoom_at_10=profile["upper_gain" if plant_stress else "nominal_gain"] * FULL_SWEEP_LOG_RATE / math.log(math.tan(math.radians(59/2))/math.tan(math.radians(2/2))) * math.log(42)/math.log(2)) with ExitStack() as stack: for owner,name in ((socket.socket,"connect"),(socket.socket,"connect_ex"), (socket,"create_connection"),(torch,"load"),(torch.cuda,"_lazy_init")): stack.enter_context(patch.object(owner,name,sim.deny)) stack.enter_context(patch.object(torch.cuda,"is_available",return_value=False)) pkg = ModuleType(sim.PKG) pkg.__path__ = [str(sim.BASE/"geometry_candidate/ptz_tracker_modular"),str(sim.ROOT)] stale_modules = {k:m for k,m in sys.modules.items() if k.startswith(sim.PKG+".")} for k in stale_modules: sys.modules.pop(k) stack.enter_context(patch.dict(sys.modules,{sim.PKG:pkg})) model = ModuleType(sim.PKG+".model") model.model_shared,model.yolo_forward=object(),sim.deny sys.modules[model.__name__] = model sim.load(sim.PKG+".visual_control_probe",sim.SOURCE/"visual_control_probe.py") control = sim.load(sim.PKG+".visual_control",sim.SOURCE/"visual_control.py") pins = read(NEW_PINS if revision=="BT" else BUNDLE/"pins.json") bundle = NEW if revision=="BT" else BUNDLE candidate = NEW if revision=="BT" else CANDIDATE for filename, values in pins.items(): path = (bundle if filename == "zoom_controller.py" else candidate) / filename assert sha256(path.read_bytes()).hexdigest() == values["after"], filename sim.load(sim.PKG+".zoom_controller",bundle/"zoom_controller.py") io_path, pp_path, status_path = [candidate/f for f in ("ptz_io.py","postprocess.py","status.py")] io = sim.load(sim.PKG+".ptz_io",io_path) status = sim.load(sim.PKG+".status",status_path) pp = sim.load(sim.PKG+".postprocess",pp_path) from ptz_tracker_modular import config,state,visual_tracker values = read(sim.BASE/"zoom_continuation_candidate/runtime_policy_snapshot.json")["values"] for key,value in values.items(): stack.enter_context(patch.object(config,key,value,create=True)) stack.enter_context(patch.object(config,"PTZ_CAM_IDS",[cam,*[x for x in config.PTZ_CAM_IDS if x!=cam]])) transport = sim.load("native10_mock_transport",sim.ROOT/"tests/test_zoom_useful_transport.py") before = sim.load("native10_control_fixture",sim.REPLAY) before.np,before.torch,before.config,before.state = numpy,torch,config,state before.ptz_io,before.pp,before.tracker,before.control,before.transport = io,pp,visual_tracker,control,transport case = before.Replay() case.setUp() stack.callback(case.doCleanups) f,st = case.fixture,case.st cam_config = config.CAMERA_CONFIG[cam] settings = dict(ptz_hfov_wide_deg=59.,ptz_hfov_tele_deg=2.) settings["predictive_zoom"] = dict(profile) if revision=="BT": settings["predictive_zoom"].update( tele_pan_native10_deg_s=cfg.pan_at_10, tele_tilt_native10_deg_s=cfg.tilt_at_10, latency_horizon_ms=video_ms+isapi_ms/2, ) settings["predictive_zoom"].update(trace.get("tuning",{})) stack.enter_context(patch.object(config,"PREDICTIVE_ZOOM_TELEMETRY_DIR",None,create=True)) stack.enter_context(patch.dict(cam_config,settings)) for key in ("ZOOM_STEP_PULSE_SEC","ZOOM_STEP_HARD_STOP_SEC"): stack.enter_context(patch.object(config,key,values[key])) plant = FlightCamera(cfg,float(cam_config.get("pan_sign",1)),float(cam_config.get("tilt_sign",-1)),trace,initial_fov) initial = plant.project(0) if initial is None: raise ValueError("Invalid initial viewport") st.update(last_bbox=initial,prev_cx=(initial[0]+initial[2])/2560, prev_cy=(initial[1]+initial[3])/1440,stable_frames=0, track_started_at=100.,last_seen=100.,visual_tracker_last_detector_at=100., visual_tracker_last_zoom_detector_at=100.,visual_tracker_last_detector_conf=.8) status._update_runtime_state(cam,None,None,99.99,zoom_ratio=plant.magnification,hfov_deg=plant.hfov) st["vfov_deg"]=plant.vfov # Replace only the asynchronous STOP thread's scheduler so it runs at # this virtual tick; the actual priority STOP path still runs. if revision in ("PR8","BT"): class InlineThread: def __init__(self, *, target, **_): self.target=target def start(self): self.target() original_thread = io.threading.Thread def choose_thread(*args, **kwargs): if kwargs.get("name") == "ptz-predictive-stop": return InlineThread(**kwargs) return original_thread(*args, **kwargs) stack.enter_context(patch.object(io.threading,"Thread",choose_thread)) t = 0. commands, rows, decisions = [], [], [] statuses, frames = deque(),deque() request_starts, request_ids = [],set() prior_lens = 0 busy_until = 0.0 def send(_url, *, data=b"", **_kw): nonlocal prior_lens, busy_until axes = tuple(transport.xml_axis(str(data),name) for name in ("pan","tilt","zoom")) # Model physical propagation and GET response separately. PUT # success and STOP timers use the existing deterministic fixture. issue = max(t,f.wall-100) if put_rtt else t response = issue+isapi_ms/1000 if put_rtt else issue propagation = isapi_ms/(2000 if put_rtt else 1000) plant.commands.append((issue+propagation,axes)) commands.append((round(issue,4),axes)) if put_rtt: busy_until = max(busy_until,response) f.wall,f.mono=100+response,1000+response request = st.get("zoom_step_pending_id") if axes[2]>0 and prior_lens<=0 and request and request not in request_ids: request_starts.append(round(response,4)) request_ids.add(request) prior_lens=axes[2] return True,200,"" stack.enter_context(patch.object(io,"_ptz_put",side_effect=send)) stack.enter_context(patch.object(io,"_ptz_put_priority_stop",side_effect=send)) logger=logging.getLogger("PTZTracker") saved=logger.level logger.setLevel(logging.ERROR) stack.callback(logger.setLevel,saved) interval = video_ms//10 stop_step = round(trace["duration"]*100) optics_count=0 for step in range(stop_step+1): t=step/100 now = max(t,busy_until) f.wall,f.mono=100+now,1000+now if step: plant.step(t,.01) if step%25==0: statuses.append((step+isapi_ms//10,step,plant.magnification,plant.hfov,plant.vfov, plant.pan_rate*plant.pan_sign,plant.tilt_rate*plant.tilt_sign)) while statuses and statuses[0][0]<=step and t+1e-8>=busy_until: _,origin,mag,hfov,vfov,pan,tilt=statuses.popleft() measured_at=100+origin/100 st.update(status_rate_at=measured_at,pan_rate_at=measured_at,tilt_rate_at=measured_at, pan_rate_deg_s=pan,tilt_rate_deg_s=tilt,vfov_deg=vfov) st["status_rate_history"] = (st.get("status_rate_history", []) + [(measured_at,pan,tilt)])[-20:] status._update_runtime_state(cam,None,None,f.wall,zoom_ratio=mag,hfov_deg=hfov, zoom_age_sec=(step-origin)/100) timer=st.get("zoom_step_stop_timer") if (timer and timer.is_alive() and f.mono+1e-8>=st["zoom_step_stop_started_mono"]+timer.delay): timer.fire() if step%10==0: frames.append((step+interval,step,plant.project(t))) while frames and frames[0][0]<=step and t+1e-8>=busy_until: due,origin,box=frames.popleft() case.seq+=1 track=visual_tracker.VisualTrackResult(box is not None,box,.8 if box else 0.,0,.8, case.seq,"telemetry projection",case.seq) age=(step-origin)/100 captured_at=f.wall-age visual=control.VisualSample(control.owner(st),case.seq,captured_at,track) if origin%40==0 and box is not None: detection=SimpleNamespace(boxes=before.Boxes([(*box,.8,0)])) pp.postprocess_control(cam,case.frame,detection,True,case.seq,visual_sample=visual) frame=SimpleNamespace(boxes=before.Boxes([])) row=capture_control(pp,lambda: pp.postprocess_control( cam,case.frame,frame,True,case.seq,visual_sample=visual,visual_only=True)) reason,flags=blocker(row,st) decisions.append(dict(t=t, zoom_block_reason=reason, flags=flags, requested=row.get("zoom_cmd"), source_seq=case.seq, return_line=row.get("return_line"), actual_hfov=plant.hfov, forecast_reason=getattr(row.get("predictive_decision"),"reason",None))) if not state.ptz_cmd_q.empty(): state.ptz_cmd_q.put_nowait(None) io._ptz_worker() optics_count=max(optics_count,st.get("zoom_in_pulse_count",0)) if step%5==0 or step==stop_step: box=plant.project(t,clipped=False) if box is None: error,margin,px=None,-.5,None else: x0,y0,x1,y1=box error=max(abs((x0+x1)/2560-.5),abs((y0+y1)/1440-.5)) margin=min(x0/1280,1-x1/1280,y0/720,1-y1/720) px=(x1-x0,y1-y0) rows.append(dict(t=t,hfov=plant.hfov,margin=margin,center_err=error,bbox_px=px, mode=st.get("mode"),zoom=plant.active[2], tele_steering=st.get("tele_steering_active",False))) first=request_starts[0] if request_starts else None prefix=[row for row in decisions if first is None or row["t"]<=first] errors=[r["center_err"] for r in rows if r["center_err"] is not None] tele = [r for r in rows if r["hfov"] < 18] telemetry=dict( trace={k:val for k,val in trace.items() if k!="knots"}, revision=revision, tested_after_sha={file:row["after"] for file,row in pins.items()}, video_ms=video_ms,isapi_ms=isapi_ms,initial_fov=initial_fov,plant_stress=plant_stress, put_rtt_ms=isapi_ms if put_rtt else 0, observed_model=dict(profile=profile,pan_at_native10=cfg.pan_at_10,tilt_at_native10=cfg.tilt_at_10), metrics=dict(first_zoom_s=first,positive_starts=len(request_starts),optics_confirmed_max=optics_count, hfov_min=round(min(r["hfov"] for r in rows),3), tele_samples=len(tele), tele_center_pct=round(100*sum(r["center_err"] is not None and r["center_err"]<=.1 for r in tele)/len(tele),1) if tele else None, tele_steering_samples=sum(bool(r["tele_steering"]) for r in tele), fully_in_frame_pct=round(100*sum(r["margin"]>=0 for r in rows)/len(rows),1), inside_center_10pct=round(100*sum(r["center_err"] is not None and r["center_err"]<=.1 for r in rows)/len(rows),1), min_edge_margin=round(min(r["margin"] for r in rows),4), peak_center_error=round(max(errors),4) if errors else None, narrow_lens_active_samples=sum(r["hfov"]<=15 and r["zoom"]>0 for r in rows), narrowing_with_edge_clip_samples=sum(r["margin"]<0 and r["zoom"]>0 for r in rows), first_edge_clip_s=next((r["t"] for r in rows if r["margin"]<0),None), positive_native=dict(Counter(axes[2] for _,axes in commands if axes[2]>0)), max_bbox_short_px=max((min(r["bbox_px"]) for r in rows if r["bbox_px"]),default=0)), zoom_block_reason_before_first=dict(Counter(r["zoom_block_reason"] for r in prefix if r["zoom_block_reason"])), zoom_block_reason=dict(Counter(r["zoom_block_reason"] for r in decisions if r["zoom_block_reason"])), simultaneous_flags_before_first=dict(Counter(x for r in prefix for x in r["flags"])), request_starts=request_starts,commands=commands,rows=rows,decisions=decisions) return telemetry def main(): parser=argparse.ArgumentParser(description=__doc__) parser.add_argument("--camera",choices=sorted(FLIGHTS)) parser.add_argument("--tune",action="store_true") parser.add_argument("--problem",action="store_true") parser.add_argument("--tuning-json",default="{}") parser.add_argument("--smoke",action="store_true") parser.add_argument("--edge-stress",action="store_true") parser.add_argument("--put-rtt",action="store_true") parser.add_argument("--counters",action="store_true", help="repeat the first six seconds with per-tick controller diagnostics") args=parser.parse_args() import torch torch.set_num_threads(1) dest=OUT/"final" dest.mkdir(exist_ok=True) profiles=read(DATA/"profiles.json")["cameras"] field,pulses=read(FIELD),read(PULSES) steering=read(LAB/"field-calibration-report.json")["cameras"] jobs=[] for camera, keys in FLIGHTS.items(): if args.camera and args.camera!=camera: continue traces=[trajectory(camera,next(e for e in pulses["episodes"][camera] if e["started_at"][11:19]==key),field) for key in keys] if args.problem: if camera=="19": ep=next(e for e in read(PR7)["episodes"][camera] if "05:49:18" in e["started_at"] or "12:49:18" in e["started_at"]) trace=trajectory(camera,ep,field) for video,api in ((180,40),(220,50),(250,60)): jobs.append((trace,video,api,59.,False)) continue if args.tune: trace = {**traces[0],"stress":True,"edge_stress":True, "duration":min(5.,traces[0]["duration"])} jobs.extend(((traces[-1],220,50,59.,False),(trace,250,60,6.,True), ({**traces[0],"stress":True,"duration":7.},250,60,12.,True))) continue if args.put_rtt: for video,api in ((180,40),(250,60)): jobs.append(({**traces[1],"duration":min(7.,traces[1]["duration"])},video,api,59.,False)) jobs.append(({**traces[0],"stress":True,"edge_stress":True, "duration":min(5.,traces[0]["duration"])},video,api,6.,True)) continue if args.edge_stress: trace = {**traces[0],"stress":True,"edge_stress":True, "duration":min(5.,traces[0]["duration"])} for video,api in ((180,40),(250,60)): jobs.append((trace,video,api,6.,True)) continue if args.counters: for trace in traces: jobs.append(({**trace,"duration":min(6.,trace["duration"])},220,50,59.,False)) continue if args.smoke: jobs.append((traces[0],220,50,59.,False)) continue for trace in traces: for video,api in ((180,40),(220,50),(250,60)): jobs.append((trace,video,api,59.,False)) for fov in (12.,6.): for video,api in ((180,40),(250,60)): stress = {**traces[0],"stress":True,"duration":min(traces[0]["duration"],7.)} jobs.append((stress,video,api,fov,True)) summaries=[] for trace,video,api,fov,stress in jobs: cam=str(trace["camera"]) versions=[] for revision in ("PR8","BT"): configured_trace = {**trace,"tuning":json.loads(args.tuning_json)} result=run(configured_trace,profiles[cam]["replay_settings"],steering[cam]["fit"], revision,video,api,fov,stress,put_rtt=args.put_rtt) name=f"{cam}-{trace['episode'][11:19].replace(':','')}-{video}-{api}-{int(fov)}-{revision}{'-counters' if args.counters else ''}{'-edge' if args.edge_stress else ''}{'-put' if args.put_rtt else '-tune' if args.tune else '-problem' if args.problem else ''}.json" (dest/name).write_text(json.dumps(result,allow_nan=False,separators=(",",":"))+"\n",encoding="utf-8") versions.append({k:result[k] for k in ("revision","metrics","zoom_block_reason_before_first","tested_after_sha")}|{"trace_file":name}) summaries.append(dict(camera=int(cam),episode=trace["episode"],video_ms=video,isapi_ms=api, initial_fov=fov,stress=stress,put_rtt_ms=api if args.put_rtt else 0,runs=versions)) print(cam,trace["episode"][11:19],video,api,fov, [(x["metrics"]["first_zoom_s"],x["metrics"]["hfov_min"],x["metrics"]["tele_center_pct"], x["metrics"]["min_edge_margin"],x["metrics"]["tele_steering_samples"]) for x in versions], flush=True) name = f"comparison{('-'+args.camera) if args.camera else ''}{'-smoke' if args.smoke else ''}{'-counters' if args.counters else ''}{'-edge' if args.edge_stress else ''}{'-put' if args.put_rtt else '-tune' if args.tune else '-problem' if args.problem else ''}.json" (dest/name).write_text(json.dumps(summaries,indent=2,allow_nan=False)+"\n",encoding="utf-8") if __name__=="__main__": main()