diff --git a/tools/longitudinal_maneuvers/maneuversd.py b/tools/longitudinal_maneuvers/maneuversd.py index 5e13737e854aba..9a3a745cb5cbaa 100755 --- a/tools/longitudinal_maneuvers/maneuversd.py +++ b/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 @@ @dataclass class Action: - accel: float # m/s^2 - duration: float # seconds + accel_bp: list[float] # m/s^2 + time_bp: list[float] # seconds + + def __post_init__(self): + assert len(self.accel_bp) == len(self.time_bp) @dataclass @@ -41,11 +45,12 @@ def get_accel(self, v_ego: float, long_active: bool, standstill: bool, cruise_st 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 @@ def get_accel(self, v_ego: float, long_active: bool, standstill: bool, cruise_st else: self._finished = True - return action.accel + return float(action_accel) @property def finished(self): @@ -74,47 +79,47 @@ def active(self): 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, ),