Long maneuver report markers (#33552)

* check for overrides as well

* only used twice

* longActive is false for gas pressed

* label cross time

* love pycharm

tools/longitudinal_maneuvers/generate_report.py

@@ -3,7 +3,7 @@ import argparse
 import base64
 import io
 import os
-import json
+import pprint
 from pathlib import Path
 import matplotlib.pyplot as plt
 
@@ -12,12 +12,12 @@ from openpilot.system.hardware.hw import Paths
 
 
 def format_car_params(CP):
-  return json.dumps({k: v for k, v in CP.to_dict().items() if not k.endswith('DEPRECATED')}, indent=2)
+  return pprint.pformat({k: v for k, v in CP.to_dict().items() if not k.endswith('DEPRECATED')}, indent=2)
 
 
 def report(platform, route, CP, maneuvers):
   output_path = Path(__file__).resolve().parent / "longitudinal_reports"
-  output_fn = output_path / f"{platform}_{route}.html"
+  output_fn = output_path / f"{platform}_{route.replace('/', '_')}.html"
   output_path.mkdir(exist_ok=True)
   with open(output_fn, "w") as f:
     f.write("<h1>Longitudinal maneuver report</h1>\n")
@@ -34,24 +34,36 @@ def report(platform, route, CP, maneuvers):
         t_carState, carState = zip(*[(m.logMonoTime, m.carState) for m in msgs if m.which() == 'carState'], strict=True)
         t_longitudinalPlan, longitudinalPlan = zip(*[(m.logMonoTime, m.longitudinalPlan) for m in msgs if m.which() == 'longitudinalPlan'], strict=True)
 
+        # make time relative seconds
         t_carControl = [(t - t_carControl[0]) / 1e9 for t in t_carControl]
         t_carOutput = [(t - t_carOutput[0]) / 1e9 for t in t_carOutput]
         t_carState = [(t - t_carState[0]) / 1e9 for t in t_carState]
         t_longitudinalPlan = [(t - t_longitudinalPlan[0]) / 1e9 for t in t_longitudinalPlan]
 
+        # maneuver validity
         longActive = [m.longActive for m in carControl]
-        gasPressed = [m.gasPressed for m in carState]
+        maneuver_valid = all(longActive)
-        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")
 
+        # get first acceleration target and first intersection
+        aTarget = longitudinalPlan[0].aTarget
+        target_cross_time = None
+        f.write(f'<h3 style="font-weight: normal">Initial aTarget: {aTarget} m/s^2')
+        for t, cs in zip(t_carState, carState, strict=True):
+          if (0 < aTarget < cs.aEgo) or (0 > aTarget > cs.aEgo):
+            f.write(f', <strong>crossed in {t:.3f}s</strong>')
+            target_cross_time = t
+            break
+        else:
+          f.write(', <strong>not crossed</strong>')
+        f.write('</h3>')
+
         plt.rcParams['font.size'] = 40
-        fig = plt.figure(figsize=(30, 25))
+        fig = plt.figure(figsize=(30, 26))
         ax = fig.subplots(4, 1, sharex=True, gridspec_kw={'height_ratios': [5, 3, 1, 1]})
 
         ax[0].grid(linewidth=4)
@@ -64,14 +76,17 @@ def report(platform, route, CP, maneuvers):
         #ax[0].set_ylim(-6.5, 6.5)
         ax[0].legend()
 
+        if target_cross_time is not None:
+          ax[0].plot(target_cross_time, aTarget, marker='o', markersize=50, markeredgewidth=7, markeredgecolor='black', markerfacecolor='None')
+
         ax[1].grid(linewidth=4)
         ax[1].plot(t_carState, [m.vEgo for m in carState], 'g', label='vEgo', linewidth=6)
         ax[1].set_ylabel('Velocity (m/s)')
         ax[1].legend()
 
         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, [m.gasPressed for m in carState], label='gasPressed', linewidth=6)
-        ax[3].plot(t_carState, brakePressed, label='brakePressed', linewidth=6)
+        ax[3].plot(t_carState, [m.brakePressed for m in carState], label='brakePressed', linewidth=6)
         for i in (2, 3):
           ax[i].set_yticks([0, 1], minor=False)
           ax[i].set_ylim(-1, 2)

tools/longitudinal_maneuvers/maneuversd.py

@@ -28,8 +28,8 @@ class Maneuver:
   _ready_cnt: int = 0
   _repeated: int = 0
 
-  def get_accel(self, v_ego: float, enabled: bool, standstill: bool) -> float:
+  def get_accel(self, v_ego: float, long_active: bool, standstill: bool) -> float:
-    ready = abs(v_ego - self.initial_speed) < 0.3 and enabled and not standstill
+    ready = abs(v_ego - self.initial_speed) < 0.3 and long_active and not standstill
     self._ready_cnt = (self._ready_cnt + 1) if ready else 0
 
     if self._ready_cnt > (3. / DT_MDL):
@@ -132,11 +132,10 @@ def main():
 
     longitudinalPlan = plan_send.longitudinalPlan
     accel = 0
-    cs = sm['carState']
+    v_ego = max(sm['carState'].vEgo, 0)
-    v_ego = max(cs.vEgo, 0)
 
     if maneuver is not None:
-      accel = maneuver.get_accel(v_ego, cs.cruiseState.enabled, cs.cruiseState.standstill)
+      accel = maneuver.get_accel(v_ego, sm['carControl'].longActive, sm['carState'].cruiseState.standstill)
 
       if maneuver.active:
         alert_msg.alertDebug.alertText1 = f'Maneuver Active: {accel:0.2f} m/s^2'