hide bad maneuvers

tools/longitudinal_maneuvers/generate_report.py

@@ -3,17 +3,13 @@ import argparse
 import base64
 import io
 import os
-import json
 import time
 from collections import defaultdict
 from pathlib import Path
-
 import matplotlib.pyplot as plt
-from openpilot.tools.lib.logreader import LogReader
-
 
-# TODO any import for this?
-REALDATA = Path('/home/batman/.comma/media/0/realdata')
+from openpilot.tools.lib.logreader import LogReader
+from openpilot.system.hardware.hw import Paths
 
 
 def report(platform, maneuvers):
@@ -26,23 +22,36 @@ def report(platform, maneuvers):
     # if args.desc:
     #   f.write(f"<h3>{args.desc}</h3>")
     for description, runs in maneuvers:
-      print('using description:', description)
+      print('plotting maneuver:', description, 'runs:', len(runs))
       f.write("<div style='border-top: 1px solid #000; margin: 20px 0;'></div>\n")
       f.write(f"<h2>{description}</h2>\n")
       for run, msgs in enumerate(runs):
         t_carControl, carControl = zip(*[(m.logMonoTime, m.carControl) for m in msgs if m.which() == 'carControl'])
+        t_carOutput, carOutput = zip(*[(m.logMonoTime, m.carOutput) for m in msgs if m.which() == 'carOutput'])
         t_carState, carState = zip(*[(m.logMonoTime, m.carState) for m in msgs if m.which() == 'carState'])
         t_longitudinalPlan, longitudinalPlan = zip(*[(m.logMonoTime, m.longitudinalPlan) for m in msgs if m.which() == 'longitudinalPlan'])
 
-        f.write(f"<h3>Run #{int(run)+1}</h3>\n")
+        longActive = [m.longActive for m in carControl]
+        gasPressed = [m.gasPressed for m in carState]
+        brakePressed = [m.brakePressed for m in carState]
+
+        maneuver_valid = all(longActive) and not (any(gasPressed) or any(brakePressed))
+
+        _open = 'open' if maneuver_valid else ''
+        title = f'Run #{int(run)+1}' + (' <span style="color: red">(invalid maneuver!)</span>' if not maneuver_valid else '')
+
+        f.write(f"<details {_open}><summary><h3 style='display: inline-block;'>{title}</h3></summary>\n")
+
         plt.rcParams['font.size'] = 40
         fig = plt.figure(figsize=(30, 25))
         ax = fig.subplots(4, 1, sharex=True, gridspec_kw={'hspace': 0, 'height_ratios': [5, 3, 1, 1]})
 
         ax[0].grid(linewidth=4)
-        ax[0].plot(t_carControl, [m.actuators.accel for m in carControl], label='accel command', linewidth=6)
-        ax[0].plot(t_longitudinalPlan, [m.aTarget for m in longitudinalPlan], label='accel target', linewidth=6)
-        ax[0].plot(t_carState, [m.aEgo for m in carState], label='aEgo', linewidth=6)
+        ax[0].plot(t_carControl, [m.actuators.accel for m in carControl], label='carControl.actuators.accel', linewidth=6)
+        ax[0].plot(t_carOutput, [m.actuatorsOutput.accel for m in carOutput], label='carOutput.actuatorsOutput.accel', linewidth=6)
+        ax[0].plot(t_longitudinalPlan, [m.aTarget for m in longitudinalPlan], label='longitudinalPlan.aTarget', linewidth=6)
+        ax[0].plot(t_carState, [m.aEgo for m in carState], label='carState.aEgo', linewidth=6)
+        # TODO localizer accel
         ax[0].set_ylabel('Acceleration (m/s^2)')
         #ax[0].set_ylim(-6.5, 6.5)
         ax[0].legend()
@@ -52,9 +61,9 @@ def report(platform, maneuvers):
         ax[1].set_ylabel('Velocity (m/s)')
         ax[1].legend()
 
-        ax[2].plot(t_carControl, [m.enabled for m in carControl], label='enabled', linewidth=6)
-        ax[3].plot(t_carState, [m.gasPressed for m in carState], label='gasPressed', linewidth=6)
-        ax[3].plot(t_carState, [m.brakePressed for m in carState], label='brakePressed', linewidth=6)
+        ax[2].plot(t_carControl, longActive, label='longActive', linewidth=6)
+        ax[3].plot(t_carState, gasPressed, label='gasPressed', linewidth=6)
+        ax[3].plot(t_carState, brakePressed, label='brakePressed', linewidth=6)
         for i in (2, 3):
           ax[i].set_yticks([0, 1], minor=False)
           ax[i].set_ylim(-1, 2)
@@ -67,8 +76,8 @@ def report(platform, maneuvers):
         fig.savefig(buffer, format='png')
         buffer.seek(0)
         f.write(f"<img src='data:image/png;base64,{base64.b64encode(buffer.getvalue()).decode()}' style='width:100%; max-width:800px;'>\n")
+        f.write("</details>\n")
 
-    # f.write(f"<p style='display: none'>{json.dumps(logs)}</p>")
   print(f"\nReport written to {output_fn}\n")
 
 
@@ -80,17 +89,15 @@ if __name__ == '__main__':
 
   logs = defaultdict(dict)
 
-  segs = []
-  for seg in os.listdir(REALDATA):
-    if args.route == seg[:20]:
-      print(seg)
-      segs.append(seg)
-
-  lr = LogReader([str(REALDATA / seg / 'rlog') for seg in segs])
+  if '/' in args.route or '|' in args.route:
+    lr = LogReader(args.route)
+  else:
+    segs = [seg for seg in os.listdir(Paths.log_root()) if args.route in seg]
+    lr = LogReader([os.path.join(Paths.log_root(), seg, 'rlog') for seg in segs])
 
   CP = lr.first('carParams')
   platform = CP.carFingerprint
-  print('got platform', platform)
+  print('processing report for', platform)
 
   maneuvers: list[tuple[str, list[list]]] = []
   active_prev = False
@@ -111,8 +118,4 @@ if __name__ == '__main__':
     if active_prev:
       maneuvers[-1][1][-1].append(msg)
 
-  # print(len(list(lr)))
-  for desc, msgs in maneuvers:
-    print(desc, len(msgs))
-
   report(platform, maneuvers)