maneuver report: use livePose acceleration and add description

tools/longitudinal_maneuvers/generate_report.py

@@ -16,7 +16,7 @@ def format_car_params(CP):
   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):
+def report(platform, route, _description, CP, maneuvers):
   output_path = Path(__file__).resolve().parent / "longitudinal_reports"
   output_fn = output_path / f"{platform}_{route.replace('/', '_')}.html"
   output_path.mkdir(exist_ok=True)
@@ -25,6 +25,8 @@ def report(platform, route, CP, maneuvers):
     f.write("<h1>Longitudinal maneuver report</h1>\n")
     f.write(f"<h3>{platform}</h3>\n")
     f.write(f"<h3>{route}</h3>\n")
+    if _description is not None:
+      f.write(f"<h3>Description: {_description}</h3>\n")
     f.write(f"<details><summary><h3 style='display: inline-block;'>CarParams</h3></summary><pre>{format_car_params(CP)}</pre></details>\n")
     for description, runs in maneuvers:
       print(f'plotting maneuver: {description}, runs: {len(runs)}')
@@ -34,17 +36,19 @@ def report(platform, route, CP, maneuvers):
         t_carControl, carControl = zip(*[(m.logMonoTime, m.carControl) for m in msgs if m.which() == 'carControl'], strict=True)
         t_carOutput, carOutput = zip(*[(m.logMonoTime, m.carOutput) for m in msgs if m.which() == 'carOutput'], strict=True)
         t_carState, carState = zip(*[(m.logMonoTime, m.carState) for m in msgs if m.which() == 'carState'], strict=True)
+        t_livePose, livePose = zip(*[(m.logMonoTime, m.livePose) for m in msgs if m.which() == 'livePose'], 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_livePose = [(t - t_livePose[0]) / 1e9 for t in t_livePose]
         t_longitudinalPlan = [(t - t_longitudinalPlan[0]) / 1e9 for t in t_longitudinalPlan]
 
         # maneuver validity
         longActive = [m.longActive for m in carControl]
-        maneuver_valid = all(longActive)
+        maneuver_valid = all(longActive) and not any(cs.cruiseState.standstill for cs in carState)
 
         _open = 'open' if maneuver_valid else ''
         title = f'Run #{int(run)+1}' + (' <span style="color: red">(invalid maneuver!)</span>' if not maneuver_valid else '')
@@ -55,8 +59,8 @@ def report(platform, route, CP, maneuvers):
         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):
+        for t, lp in zip(t_livePose, livePose, strict=True):
+          if (0 < aTarget < lp.accelerationDevice.x) or (0 > aTarget > lp.accelerationDevice.x):
             f.write(f', <strong>crossed in {t:.3f}s</strong>')
             target_cross_time = t
             if maneuver_valid:
@@ -75,6 +79,7 @@ def report(platform, route, CP, maneuvers):
         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)
+        ax[0].plot(t_livePose, [m.accelerationDevice.x for m in livePose], label='livePose.accelerationDevice.x', linewidth=6)
         # TODO localizer accel
         ax[0].set_ylabel('Acceleration (m/s^2)')
         #ax[0].set_ylim(-6.5, 6.5)
@@ -119,6 +124,7 @@ def report(platform, route, CP, maneuvers):
 if __name__ == '__main__':
   parser = argparse.ArgumentParser(description='Generate longitudinal maneuver report from route')
   parser.add_argument('route', type=str, help='Route name (e.g. 00000000--5f742174be)')
+  parser.add_argument('description', type=str, default=None)
 
   args = parser.parse_args()
 
@@ -150,4 +156,4 @@ def report(platform, route, CP, maneuvers):
     if active_prev:
       maneuvers[-1][1][-1].append(msg)
 
-  report(platform, args.route, CP, maneuvers)
+  report(platform, args.route, args.description, CP, maneuvers)