maneuversd: support interpolated breakpoints (#33936)

* breakpoints

* clean up

* fix

* simplify

* np fast

* might as well

tools/longitudinal_maneuvers/maneuversd.py

@@ -1,4 +1,5 @@
 #!/usr/bin/env python3
+import numpy as np
 from dataclasses import dataclass
 
 from cereal import messaging, car
@@ -10,8 +11,11 @@ from openpilot.common.swaglog import cloudlog
 
 @dataclass
 class Action:
-  accel: float      # m/s^2
+  accel_bp: list[float]  # m/s^2
-  duration: float   # seconds
+  time_bp: list[float]   # seconds
+
+  def __post_init__(self):
+    assert len(self.accel_bp) == len(self.time_bp)
 
 
 @dataclass
@@ -41,11 +45,12 @@ class Maneuver:
       return min(max(self.initial_speed - v_ego, -2.), 2.)
 
     action = self.actions[self._action_index]
+    action_accel = np.interp(self._action_frames * DT_MDL, action.time_bp, action.accel_bp)
 
     self._action_frames += 1
 
     # reached duration of action
-    if self._action_frames > (action.duration / DT_MDL):
+    if self._action_frames > (action.time_bp[-1] / DT_MDL):
       # next action
       if self._action_index < len(self.actions) - 1:
         self._action_index += 1
@@ -60,7 +65,7 @@ class Maneuver:
       else:
         self._finished = True
 
-    return action.accel
+    return float(action_accel)
 
   @property
   def finished(self):
@@ -74,47 +79,47 @@ class Maneuver:
 MANEUVERS = [
   Maneuver(
     "come to stop",
-    [Action(-0.5, 12)],
+    [Action([-0.5], [12])],
     repeat=2,
     initial_speed=5.,
   ),
   Maneuver(
     "start from stop",
-    [Action(1.5, 5)],
+    [Action([1.5], [5])],
     repeat=2,
     initial_speed=0.,
   ),
   Maneuver(
     "creep: alternate between +1m/s^2 and -1m/s^2",
     [
-      Action(1, 3), Action(-1, 3),
+      Action([1], [3]), Action([-1], [3]),
-      Action(1, 3), Action(-1, 3),
+      Action([1], [3]), Action([-1], [3]),
-      Action(1, 3), Action(-1, 3),
+      Action([1], [3]), Action([-1], [3]),
     ],
     repeat=2,
     initial_speed=0.,
   ),
   Maneuver(
     "brake step response: -1m/s^2 from 20mph",
-    [Action(-1, 3)],
+    [Action([-1], [3])],
     repeat=2,
     initial_speed=20. * CV.MPH_TO_MS,
   ),
   Maneuver(
     "brake step response: -4m/s^2 from 20mph",
-    [Action(-4, 3)],
+    [Action([-4], [3])],
     repeat=2,
     initial_speed=20. * CV.MPH_TO_MS,
   ),
   Maneuver(
     "gas step response: +1m/s^2 from 20mph",
-    [Action(1, 3)],
+    [Action([1], [3])],
     repeat=2,
     initial_speed=20. * CV.MPH_TO_MS,
   ),
   Maneuver(
     "gas step response: +4m/s^2 from 20mph",
-    [Action(4, 3)],
+    [Action([4], [3])],
     repeat=2,
     initial_speed=20. * CV.MPH_TO_MS,
   ),