Latencylogger optimization (#24166)

tools/latencylogger/latency_logger.py

@@ -57,14 +57,21 @@ def read_logs(lr):
         if continue_outer:
           continue
       mono_to_frame[msg.logMonoTime] = frame_id
-
       data['timestamp'][frame_id][service].append((msg.which()+" published", msg.logMonoTime))
+
+      next_service = SERVICES[SERVICES.index(service)+1]
+      if not data['start'][frame_id][next_service]:
+        data['start'][frame_id][next_service] = msg.logMonoTime
+      data['end'][frame_id][service] = msg.logMonoTime
+
       if service in SERVICE_TO_DURATIONS:
         for duration in SERVICE_TO_DURATIONS[service]:
           data['duration'][frame_id][service].append((msg.which()+"."+duration, getattr(msg_obj, duration)))
 
       if service == SERVICES[0]:
         data['timestamp'][frame_id][service].append((msg.which()+" start of frame", msg_obj.timestampSof))
+        if not data['start'][frame_id][service]:
+          data['start'][frame_id][service] = msg_obj.timestampSof
       elif msg.which() == 'controlsState':
         # Sendcan is published before controlsState, but the frameId is retrived in CS
         data['timestamp'][frame_id][service].append(("sendcan published", latest_sendcan_monotime))
@@ -78,31 +85,16 @@ def read_logs(lr):
   assert len(frame_mismatches) < 20, "Too many frame mismatches"
   return (data, frame_mismatches)
 
-def get_min_time(frame_id, service, timestamps, use_max=False):
-  return min(timestamps[frame_id][service], key=lambda x: x[1])[1] if not use_max else max(timestamps[frame_id][service], key=lambda x: x[1])[1]
-
-def get_interval(frame_id, service, timestamps):
-  try:
-    service_max = get_min_time(frame_id, service, timestamps, use_max=True) 
-    if service == SERVICES[0]:
-      service_min = get_min_time(frame_id, service, timestamps) 
-      return (service_min, service_max)
-    prev_service = SERVICES[SERVICES.index(service)-1]
-    prev_service_max = get_min_time(frame_id, prev_service, timestamps, use_max=True) 
-    return (prev_service_max, service_max)
-  except ValueError:
-    return (-1,-1)
-
-def find_frame_id(time, service, timestamps):
-  for frame_id in reversed(timestamps):
-    start, end = get_interval(frame_id, service, timestamps)
-    if start <= time <= end:
-      return frame_id
-  return -1
+
+def find_frame_id(time, service, start_times, end_times):
+  for frame_id in reversed(start_times):
+    if start_times[frame_id][service] and end_times[frame_id][service]:
+      if start_times[frame_id][service] <= time <= end_times[frame_id][service]:
+        yield frame_id
 
 ## ASSUMES THAT AT LEAST ONE CLOUDLOG IS MADE IN CONTROLSD
-def insert_cloudlogs(lr, timestamps):
-  t0 = get_min_time(min(timestamps.keys()), SERVICES[0], timestamps) 
+def insert_cloudlogs(lr, timestamps, start_times, end_times):
+  t0 = start_times[min(start_times.keys())][SERVICES[0]] 
   failed_inserts = 0
   latest_controls_frameid = 0
   for msg in lr:
@@ -116,22 +108,28 @@ def insert_cloudlogs(lr, timestamps):
           # Filter out controlsd messages which arrive before the camera loop 
           continue
 
-        frame_id = latest_controls_frameid if service == "boardd" else find_frame_id(time, service, timestamps)
-        if frame_id > -1:
-          timestamps[frame_id][service].append((event, time))
-          if service == 'controlsd':
-            latest_controls_frameid = frame_id
+        if service == "boardd":
+          timestamps[latest_controls_frameid][service].append((event, time))
+          end_times[latest_controls_frameid][service] = time
         else:
-          failed_inserts += 1
+          frame_id_gen = find_frame_id(time, service, start_times, end_times)
+          frame_id = next(frame_id_gen, False)
+          if frame_id:
+            if service == 'controlsd':
+              latest_controls_frameid = frame_id
+            timestamps[frame_id][service].append((event, time))
+          else:
+            failed_inserts += 1
+  assert latest_controls_frameid > 0, "No timestamp in controlsd"
   assert failed_inserts < len(timestamps), "Too many failed cloudlog inserts"
 
-def print_timestamps(timestamps, durations ,relative):
-  t0 = get_min_time(min(timestamps.keys()), SERVICES[0], timestamps) 
+def print_timestamps(timestamps, durations, start_times, relative):
+  t0 = start_times[min(start_times.keys())][SERVICES[0]]
   for frame_id in timestamps.keys():
     print('='*80)
     print("Frame ID:", frame_id)
     if relative:
-      t0 = get_min_time(frame_id, SERVICES[0], timestamps) 
+      t0 = start_times[frame_id][SERVICES[0]]
     for service in SERVICES:
       print("  "+service)  
       events = timestamps[frame_id][service]
@@ -140,8 +138,8 @@ def print_timestamps(timestamps, durations ,relative):
       for event, time in durations[frame_id][service]:
         print("    "+'%-53s%-53s' %(event, str(time*1000))) 
 
-def graph_timestamps(timestamps, relative):
-  t0 = get_min_time(min(timestamps.keys()), SERVICES[0], timestamps) 
+def graph_timestamps(timestamps, start_times, end_times, relative):
+  t0 = start_times[min(start_times.keys())][SERVICES[0]] 
   fig, ax = plt.subplots()
   ax.set_xlim(0, 150 if relative else 750)
   ax.set_ylim(0, 15)
@@ -153,19 +151,22 @@ def graph_timestamps(timestamps, relative):
   points = {"x": [], "y": [], "labels": []}
   for frame_id, services in timestamps.items():
     if relative:
-      t0 = get_min_time(frame_id, SERVICES[0], timestamps) 
+      t0 = start_times[frame_id][SERVICES[0]]
     service_bars = []
     for service, events in services.items():
-      start, end = get_interval(frame_id, service,timestamps)
-      service_bars.append(((start-t0)/1e6,(end-start)/1e6))
-      for event in events:
-        points['x'].append((event[1]-t0)/1e6)
-        points['y'].append(frame_id)
-        points['labels'].append(event[0])
+      if start_times[frame_id][service] and end_times[frame_id][service]:
+        start = start_times[frame_id][service]
+        end = end_times[frame_id][service]
+        service_bars.append(((start-t0)/1e6,(end-start)/1e6))
+        for event in events:
+          points['x'].append((event[1]-t0)/1e6)
+          points['y'].append(frame_id)
+          points['labels'].append(event[0])
     ax.broken_barh(service_bars, (frame_id-0.45, 0.9), facecolors=(colors), alpha=0.5)
 
   scatter = ax.scatter(points['x'], points['y'], marker='d', edgecolor='black')
   tooltip = mpld3.plugins.PointLabelTooltip(scatter, labels=points['labels'])
+
   mpld3.plugins.connect(fig, tooltip)
   plt.legend(handles=[mpatches.Patch(color=colors[i], label=SERVICES[i]) for i in range(len(SERVICES))])
   return fig
@@ -173,7 +174,7 @@ def graph_timestamps(timestamps, relative):
 def get_timestamps(lr):
   data, frame_mismatches = read_logs(lr)
   lr.reset()
-  insert_cloudlogs(lr, data['timestamp'])
+  insert_cloudlogs(lr, data['timestamp'], data['start'], data['end'])
   return data, frame_mismatches
 
 if __name__ == "__main__":
@@ -192,6 +193,6 @@ if __name__ == "__main__":
   r = DEMO_ROUTE if args.demo else args.route_or_segment_name.strip()
   lr = logreader_from_route_or_segment(r, sort_by_time=True)
   data, _ = get_timestamps(lr)
-  print_timestamps(data['timestamp'], data['duration'], args.relative)
+  print_timestamps(data['timestamp'], data['duration'], data['start'], args.relative)
   if args.plot:
-    mpld3.show(graph_timestamps(data['timestamp'], args.relative))
+    mpld3.show(graph_timestamps(data['timestamp'], data['start'], data['end'], args.relative))