Add offline cached-route telemetry and pacing inspection tools

roadscore/tools/demo_route_pacing.py

@@ -0,0 +1,37 @@
+"""Read-only narrative window ranking over offline event extraction (no runtime use)."""
+import argparse,json,math
+from pathlib import Path
+import numpy as np
+parser=argparse.ArgumentParser();parser.add_argument('output',type=Path);P=parser.parse_args().output
+rank=[]
+for label in 'ABCDEFGHIJK':
+ d=json.loads((P/f'{label}.telemetry.private.json').read_text());e=json.loads((P/f'{label}.events.private.json').read_text())['events'];car=np.array(d['samples']['car'],float)
+ curves=[]
+ for item in e['curves']:
+  if curves and item['direction']==curves[-1]['direction'] and item['start']-curves[-1]['end']<3:
+   prev=curves[-1];prev['end']=item['end']
+   if item['peak_lateral_proxy']>prev['peak_lateral_proxy']:prev.update(peak_time=item['peak_time'],peak_lateral_proxy=item['peak_lateral_proxy'])
+  else:curves.append(item.copy())
+ options=[]
+ for length in [90,120,60]:
+  for start in np.arange(math.ceil(e['observed_start']/5)*5,e['observed_end']-length+.01,5):
+   end=start+length
+   cv=[v for v in curves if start<v['peak_time']<end];ed=[v for v in e['engagement_edges'] if start+2<v['time']<end-2];sg=[v for v in e['signals'] if start<=v['start']<end];al=[v for v in e['alerts'] if start<v['start']<end]
+   firstcue=min([v['start'] for v in cv+sg+al]+[end]);opening=max(0,firstcue-start)
+   ix=(car[:,0]>=start)&(car[:,0]<min(start+20,end));moving=float(np.mean(car[ix,1]>3)) if np.any(ix) else 0
+   last=(car[:,0]>=end-5)&(car[:,0]<end);stopped=float(np.mean(car[last,1]<.5))>.7 if np.any(last)else False
+   stopsecs=sum(max(0,min(v['end'],end)-max(v['start'],start)) for v in e['stops'])
+   nav=[r for r in d['samples']['nav']if end-10<r[0]<end and r[1]=='arrive' and r[3]<50];arrival=bool(nav and stopped)
+   score=(14 if len(cv)in(1,2)else -10*abs(len(cv)-2))+min(opening,22)*.4+6*moving
+   score+=(8 if len(ed)in(1,2)else -5*max(1,len(ed)-2))
+   score+=3*min(len(sg),2)-5*max(0,len(sg)-2)-2*max(0,len(al)-3)
+   score+=4*any(v['preceding_straight_seconds']>=7 for v in cv)
+   score+=4*any(v['active'] and start+3<=v['time']<=start+25 for v in ed)
+   score+=5*stopped+8*arrival-max(0,stopsecs-15)*.3
+   score-=6*any(v['start']<start<v['end']for v in curves)
+   score-=8*any(b['time']-a['time']<5 for a,b in zip(ed,ed[1:]))
+   if label in 'BC' or d['decode_warnings']:score-=30
+   options.append(dict(label=label,start=float(start),end=float(end),duration=length,score=round(score,2),opening_cue_free_seconds=round(opening,1),first20s_moving_fraction=round(moving,2),curves=cv,engagement=ed,signals=sg,alerts=al,ending_stopped=stopped,arrival_evidence=arrival,standstill_seconds=round(stopsecs,1)))
+ options.sort(key=lambda x:x['score'],reverse=True);rank.append(options[0]);(P/f'{label}.pacing.private.json').write_text(json.dumps(options[:15],indent=2))
+rank.sort(key=lambda x:x['score'],reverse=True);(P/'pacing_ranking.private.json').write_text(json.dumps(rank,indent=2))
+for r in rank:print(r['label'],r['start'],r['end'],'score',r['score'],'opening',r['opening_cue_free_seconds'],'moving',r['first20s_moving_fraction'],'curves',len(r['curves']),'engage',[(round(x['time'],1),x['active'])for x in r['engagement']],'signal',len(r['signals']),'stopped',r['ending_stopped'],'arrival',r['arrival_evidence'])

roadscore/tools/demo_route_selection.py

@@ -0,0 +1,142 @@
+"""Offline cached-log demo selection. Never loaded by runtime or generation.
+No network, replay, hardware, or model execution. Private manifest paths are inputs.
+"""
+import argparse
+from collections import Counter
+import hashlib
+import json
+import math
+from pathlib import Path
+import warnings
+
+import numpy as np
+from openpilot.tools.lib.logreader import _LogFileReader
+
+
+def runs(times, values, minimum=0., gap=.6):
+  found=[];start=None;last=None;current=None
+  for t,value in zip(times,values):
+    if value != current or (last is not None and t-last>gap):
+      if current and start is not None and last-start>=minimum:found.append({'start':float(start),'end':float(last),'value':current})
+      start=t;current=value
+    last=t
+  if current and start is not None and last-start>=minimum:found.append({'start':float(start),'end':float(last),'value':current})
+  return found
+
+
+def extract(row,base):
+  files={}
+  for entry in row['cache'].get('files',[]):
+    path=base/entry['path']
+    if path.is_file() and path.name.startswith(('rlog','qlog')):
+      segment=entry['segment']
+      if segment not in files or path.name.startswith('rlog'):files[segment]=(path,entry)
+  origin=None;first={};last={};counts=Counter();valid=Counter();gaps={};prior={};samples={k:[] for k in ['car','control','model','active','alerts','nav']};decodes=[];sourcefiles=[]
+  start=row['selection']['start_s'];end=row['selection']['end_s'];last_sample={};active_prior=None;active_prior_t=None;edges=[]
+  for segment,(path,entry) in sorted(files.items()):
+    if hashlib.sha256(path.read_bytes()).hexdigest()!=entry['sha256']:raise ValueError('Cache checksum mismatch')
+    sourcefiles.append({'segment':segment,'path':str(path),'sha256':entry['sha256']})
+    try:
+      with warnings.catch_warnings(record=True) as caught:
+        warnings.simplefilter('always')
+        for e in _LogFileReader(str(path),sort_by_time=False):
+          ns=int(e.logMonoTime)
+          if origin is None:origin=ns
+          t=(ns-origin)/1e9
+          if t<start or (end is not None and t>=end):continue
+          kind=e.which()
+          if kind not in ('carState','controlsState','modelV2','selfdriveState','roadCameraState','navInstruction'):continue
+          counts[kind]+=1;first.setdefault(kind,t);last[kind]=t
+          if kind in prior:gaps[kind]=max(gaps.get(kind,0),t-prior[kind])
+          prior[kind]=t
+          if not e.valid:continue
+          valid[kind]+=1
+          data=getattr(e,kind)
+          if kind=='selfdriveState':
+            active=bool(data.active)
+            if active_prior is not None and active!=active_prior and t-active_prior_t<1:
+              edges.append({'time':t,'active':active,'state':str(data.state),'source':'selfdriveState.active'})
+            active_prior=active;active_prior_t=t
+          if t-last_sample.get(kind,-1e9)<.099 and kind!='navInstruction':continue
+          last_sample[kind]=t
+          if kind=='carState':samples['car'].append([t,float(data.vEgo),float(data.steeringAngleDeg),bool(data.leftBlinker),bool(data.rightBlinker),bool(data.standstill)])
+          elif kind=='controlsState':samples['control'].append([t,float(data.curvature)])
+          elif kind=='selfdriveState':
+            samples['active'].append([t,active]);samples['alerts'].append([t,str(data.alertType),str(data.alertText1),str(data.alertText2),str(data.alertSize)])
+          elif kind=='modelV2':
+            age=(ns-int(data.timestampEof))/1e9
+            trajectory=[(float(tt)-age,float(yaw)*float(v)) for tt,yaw,v in zip(data.orientationRate.t,data.orientationRate.z,data.velocity.x) if 1<=float(tt)-age<=5 and float(v)>3]
+            strength=max((abs(lat) for tt,lat in trajectory),default=0)
+            samples['model'].append([t,strength])
+          elif kind=='navInstruction':samples['nav'].append([t,str(data.maneuverType),str(data.maneuverModifier),float(data.maneuverDistance)])
+        decodes.extend({'segment':segment,'warning':str(w.message)} for w in caught)
+    except Exception as error:decodes.append({'segment':segment,'error':str(error)})
+  coverage={k:{'count':counts[k],'valid':valid[k],'first':first[k],'last':last[k],'max_gap':gaps.get(k)} for k in counts}
+  return dict(label=row['label'],selection=row['selection'],origin_mono_ns=origin,files=sourcefiles,coverage=coverage,decode_warnings=decodes,samples=samples,engagement_edges=edges)
+
+
+def characterize(data):
+  s=data['samples'];car=np.asarray(s['car'],dtype=float);control=np.asarray(s['control'],dtype=float)
+  if len(car)<2 or len(control)<2:return {'error':'Insufficient car/control data'}
+  t=car[:,0];speed=car[:,1]
+  indices=np.searchsorted(control[:,0],t,side='right')-1;indices=np.clip(indices,0,len(control)-1)
+  lateral=abs(control[indices,1])*speed**2
+  fresh=(t-control[indices,0]>=0)&(t-control[indices,0]<.3)
+  smooth=np.convolve(np.where(fresh,lateral,0),np.ones(9)/9,mode='same')
+  moving=speed>4
+  curve=runs(t,((smooth>.65)&moving&fresh).tolist(),minimum=1.5)
+  straight=runs(t,((smooth<.22)&(speed>5)&fresh).tolist(),minimum=7)
+  for c in curve:
+    sel=(t>=c['start'])&(t<=c['end']);peak=np.argmax(smooth[sel]);ct=t[sel]
+    c.update(peak_time=float(ct[peak]),peak_lateral_proxy=float(smooth[sel][peak]),direction='left' if np.median(control[indices[sel],1])>0 else 'right')
+    previous=[v for v in straight if 0<=c['start']-v['end']<8]
+    c['preceding_straight_seconds']=max((v['end']-v['start'] for v in previous),default=0)
+  signals=runs(t,['both' if r[3] and r[4] else 'left' if r[3] else 'right' if r[4] else '' for r in car],minimum=.3)
+  merged=[]
+  for sig in signals:
+    if merged and sig['value']==merged[-1]['value'] and sig['start']-merged[-1]['end']<1.5:merged[-1]['end']=sig['end']
+    else:merged.append(sig.copy())
+  signals=[v for v in merged if v['end']-v['start']>=1.5]
+  alerts=runs([r[0] for r in s['alerts']],[('|'.join(r[1:]) if r[1] and r[4]!='none' else '') for r in s['alerts']],minimum=.2)
+  stops=runs(t,(speed<.5).tolist(),minimum=2)
+  return dict(curves=curve,straights=straight,signals=signals,alerts=alerts,stops=stops,engagement_edges=data['engagement_edges'],observed_start=float(t[0]),observed_end=float(t[-1]),curve_proxy='abs(controlsState.curvature)*carState.vEgo^2 smoothed0.9s; not measured road geometry',actual_selfdrive_active_coverage=len(s['active']))
+
+
+def candidates(data,events,full_logs):
+  if 'error'in events:return []
+  start=max(data['selection']['start_s'],events['observed_start']);end=min(data['selection']['end_s'] or events['observed_end'],events['observed_end']);result=[]
+  for length in [90,120,60]:
+    for begin in np.arange(math.ceil(start/5)*5,max(start,end-length)+.01,5):
+      finish=begin+length
+      curves=[e for e in events['curves'] if begin+5<=e['peak_time']<=finish-5]
+      edges=[e for e in events['engagement_edges'] if begin+3<=e['time']<=finish-3]
+      signals=[e for e in events['signals'] if begin<=e['start'] and e['end']<=finish]
+      alerts=[e for e in events['alerts'] if begin<=e['start']<finish]
+      build=sum(e['preceding_straight_seconds']>=7 for e in curves)
+      side_diversity=len(set(e['direction'] for e in curves))
+      both_edges=any(e['active'] for e in edges) and any(not e['active'] for e in edges)
+      excess=max(0,len(signals)-6)+max(0,len(edges)-5)+max(0,len(curves)-5)
+      score=6*min(build,2)+3*min(len(curves),3)+2*side_diversity+4*min(len(edges),3)+5*both_edges+2*min(len(signals),4)+min(len(alerts),2)-2*excess
+      # Reject windows crossing missing telemetry; never mistake absence for calm.
+      car=np.asarray(data['samples']['car']);times=car[(car[:,0]>=begin)&(car[:,0]<=finish),0];maxgap=float(np.max(np.diff(times))) if len(times)>1 else length
+      if maxgap>.5:score-=30
+      if not full_logs or data['decode_warnings']:score-=25
+      result.append(dict(label=data['label'],start=float(begin),end=float(finish),duration=length,score=score,straight_to_curve_count=build,curves=curves,active_transitions=edges,signals=signals,alerts=alerts,max_car_gap=maxgap,full_logs=full_logs))
+  return sorted(result,key=lambda v:v['score'],reverse=True)
+
+
+def main():
+  parser=argparse.ArgumentParser();parser.add_argument('manifest',type=Path);parser.add_argument('--output',type=Path,required=True);args=parser.parse_args()
+  args.output.mkdir(parents=True,exist_ok=True);m=json.loads(args.manifest.read_text());ranking=[]
+  for row in m['routes']:
+    path=args.output/(row['label']+'.telemetry.private.json')
+    if path.exists():data=json.loads(path.read_text())
+    else:data=extract(row,args.manifest.parent);path.write_text(json.dumps(data))
+    events=characterize(data)
+    analysis=json.loads((args.manifest.parent/'analysis'/f"{row['label']}.private.json").read_text())
+    ranked=candidates(data,events,analysis.get('complete_selected_logs',False))
+    (args.output/(row['label']+'.events.private.json')).write_text(json.dumps(dict(events=events,candidates=ranked[:12]),indent=2))
+    if ranked:ranking.append(ranked[0])
+    print(row['label'],'events',len(events.get('curves',[])),len(events.get('signals',[])),len(events.get('engagement_edges',[])),'best',ranked[0]['score'] if ranked else None,flush=True)
+  (args.output/'ranking.private.json').write_text(json.dumps(sorted(ranking,key=lambda v:v['score'],reverse=True),indent=2))
+if __name__=='__main__':main()