add longitudinal maneuvers (#33527)

* add longitudinal profiles

* stash

* unfortunately even longitudinalPlan causes circle

* add to process config

* reach target speed smoothly

* stash

* works

* clean up

* debug alert

* rename

* fix

* better text

* toggle via exp button

* try coming to a stop better, smoother target reaching

* closer to target

* revert controlsd migration

* add description to alert

* generate report from local logs

* hide bad maneuvers

* pdflike

* Revert "pdflike"

This reverts commit 6d4af1bf9be2e9e0798eaecf026a53d860da7613.

* try this

* use alert manager

* fix that check

* wat

* Revert "wat"

This reverts commit 93d0d27ab838d3f580d06ff212f380e0b912d599.

* some clean up

* rm

* cleanup

* move

* fix test

* more fix

* clean up

* fix that

cereal/car.capnp

@@ -54,6 +54,7 @@ struct OnroadEvent @0x9b1657f34caf3ad3 {
     manualRestart @30;
     lowSpeedLockout @31;
     joystickDebug @34;
+    longitudinalManeuver @124;
     steerTempUnavailableSilent @35;
     resumeRequired @36;
     preDriverDistracted @37;

cereal/log.capnp

@@ -2304,6 +2304,11 @@ struct EncodeData {
   height @5 :UInt32;
 }
 
+struct DebugAlert {
+  alertText1 @0 :Text;
+  alertText2 @1 :Text;
+}
+
 struct UserFlag {
 }
 
@@ -2411,6 +2416,7 @@ struct Event {
     driverEncodeData @87 :EncodeData;
     wideRoadEncodeData @88 :EncodeData;
     qRoadEncodeData @89 :EncodeData;
+    alertDebug @133 :DebugAlert;
 
     livestreamRoadEncodeData @120 :EncodeData;
     livestreamWideRoadEncodeData @121 :EncodeData;

cereal/services.py

@@ -75,6 +75,7 @@ _services: dict[str, tuple] = {
 
   # debug
   "uiDebug": (True, 0., 1),
+  "alertDebug": (True, 0.),
   "roadEncodeData": (False, 20.),
   "driverEncodeData": (False, 20.),
   "wideRoadEncodeData": (False, 20.),

common/params.cc

@@ -158,6 +158,7 @@ std::unordered_map<std::string, uint32_t> keys = {
     {"LiveParameters", PERSISTENT},
     {"LiveTorqueParameters", PERSISTENT | DONT_LOG},
     {"LocationFilterInitialState", PERSISTENT},
+    {"LongitudinalManeuverMode", CLEAR_ON_MANAGER_START | CLEAR_ON_OFFROAD_TRANSITION},
     {"LongitudinalPersonality", PERSISTENT},
     {"NetworkMetered", PERSISTENT},
     {"ObdMultiplexingChanged", CLEAR_ON_MANAGER_START | CLEAR_ON_ONROAD_TRANSITION},

release/release_files.py

@@ -53,6 +53,7 @@ whitelist = [
   "tools/lib/",
   "tools/bodyteleop/",
   "tools/joystick/",
+  "tools/longitudinal_maneuvers/",
 
   "tinygrad_repo/openpilot/compile2.py",
   "tinygrad_repo/extra/onnx.py",

selfdrive/selfdrived/events.py

@@ -325,6 +325,17 @@ def joystick_alert(CP: car.CarParams, CS: car.CarState, sm: messaging.SubMaster,
   vals = f"Gas: {round(gb * 100.)}%, Steer: {round(steer * 100.)}%"
   return NormalPermanentAlert("Joystick Mode", vals)
 
+
+def longitudinal_maneuver_alert(CP: car.CarParams, CS: car.CarState, sm: messaging.SubMaster, metric: bool, soft_disable_time: int, personality) -> Alert:
+  ad = sm['alertDebug']
+  audible_alert = AudibleAlert.prompt if 'Active' in ad.alertText1 else AudibleAlert.none
+  alert_status = AlertStatus.userPrompt if 'Active' in ad.alertText1 else AlertStatus.normal
+  alert_size = AlertSize.mid if ad.alertText2 else AlertSize.small
+  return Alert(ad.alertText1, ad.alertText2,
+               alert_status, alert_size,
+               Priority.LOW, VisualAlert.none, audible_alert, 0.2)
+
+
 def personality_changed_alert(CP: car.CarParams, CS: car.CarState, sm: messaging.SubMaster, metric: bool, soft_disable_time: int, personality) -> Alert:
   personality = str(personality).title()
   return NormalPermanentAlert(f"Driving Personality: {personality}", duration=1.5)
@@ -344,6 +355,11 @@ EVENTS: dict[int, dict[str, Alert | AlertCallbackType]] = {
     ET.PERMANENT: NormalPermanentAlert("Joystick Mode"),
   },
 
+  EventName.longitudinalManeuver: {
+    ET.WARNING: longitudinal_maneuver_alert,
+    ET.PERMANENT: NormalPermanentAlert("Longitudinal Maneuver Mode"),
+  },
+
   EventName.selfdriveInitializing: {
     ET.NO_ENTRY: NoEntryAlert("System Initializing"),
   },

selfdrive/selfdrived/selfdrived.py

@@ -61,7 +61,7 @@ class SelfdriveD:
     # TODO: de-couple selfdrived with card/conflate on carState without introducing controls mismatches
     self.car_state_sock = messaging.sub_sock('carState', timeout=20)
 
-    ignore = self.sensor_packets + self.gps_packets
+    ignore = self.sensor_packets + self.gps_packets + ['alertDebug']
     if SIMULATION:
       ignore += ['driverCameraState', 'managerState']
     if REPLAY:
@@ -70,7 +70,7 @@ class SelfdriveD:
     self.sm = messaging.SubMaster(['deviceState', 'pandaStates', 'peripheralState', 'modelV2', 'liveCalibration',
                                    'carOutput', 'driverMonitoringState', 'longitudinalPlan', 'livePose',
                                    'managerState', 'liveParameters', 'radarState', 'liveTorqueParameters',
-                                   'controlsState', 'carControl', 'driverAssistance'] + \
+                                   'controlsState', 'carControl', 'driverAssistance', 'alertDebug'] + \
                                    self.camera_packets + self.sensor_packets + self.gps_packets,
                                   ignore_alive=ignore, ignore_avg_freq=ignore+['radarState',],
                                   ignore_valid=ignore, frequency=int(1/DT_CTRL))
@@ -135,6 +135,10 @@ class SelfdriveD:
       self.events.add(EventName.joystickDebug)
       self.startup_event = None
 
+    if self.sm.recv_frame['alertDebug'] > 0:
+      self.events.add(EventName.longitudinalManeuver)
+      self.startup_event = None
+
     # Add startup event
     if self.startup_event is not None:
       self.events.add(self.startup_event)

system/manager/process_config.py

@@ -35,6 +35,12 @@ def joystick(started: bool, params, CP: car.CarParams) -> bool:
 def not_joystick(started: bool, params, CP: car.CarParams) -> bool:
   return started and not params.get_bool("JoystickDebugMode")
 
+def long_maneuver(started: bool, params, CP: car.CarParams) -> bool:
+  return started and params.get_bool("LongitudinalManeuverMode")
+
+def not_long_maneuver(started: bool, params, CP: car.CarParams) -> bool:
+  return started and not params.get_bool("LongitudinalManeuverMode")
+
 def qcomgps(started, params, CP: car.CarParams) -> bool:
   return started and not ublox_available()
 
@@ -81,7 +87,8 @@ procs = [
   PythonProcess("paramsd", "selfdrive.locationd.paramsd", only_onroad),
   NativeProcess("ubloxd", "system/ubloxd", ["./ubloxd"], ublox, enabled=TICI),
   PythonProcess("pigeond", "system.ubloxd.pigeond", ublox, enabled=TICI),
-  PythonProcess("plannerd", "selfdrive.controls.plannerd", only_onroad),
+  PythonProcess("plannerd", "selfdrive.controls.plannerd", not_long_maneuver),
+  PythonProcess("maneuversd", "tools.longitudinal_maneuvers.maneuversd", long_maneuver),
   PythonProcess("radard", "selfdrive.controls.radard", only_onroad),
   PythonProcess("hardwared", "system.hardware.hardwared", always_run),
   PythonProcess("tombstoned", "system.tombstoned", always_run, enabled=not PC),

tools/longitudinal_maneuvers/generate_report.py

@@ -0,0 +1,115 @@
+#!/usr/bin/env python3
+import argparse
+import base64
+import io
+import os
+import time
+from pathlib import Path
+import matplotlib.pyplot as plt
+
+from openpilot.tools.lib.logreader import LogReader
+from openpilot.system.hardware.hw import Paths
+
+
+def report(platform, maneuvers):
+  output_path = Path(__file__).resolve().parent / "longitudinal_reports"
+  output_fn = output_path / f"{platform}_{time.strftime('%Y%m%d-%H_%M_%S')}.html"
+  output_path.mkdir(exist_ok=True)
+  with open(output_fn, "w") as f:
+    f.write("<h1>Longitudinal maneuver report</h1>\n")
+    f.write(f"<h3>{platform}</h3>\n")
+    for description, runs in maneuvers:
+      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'], 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_longitudinalPlan, longitudinalPlan = zip(*[(m.logMonoTime, m.longitudinalPlan) for m in msgs if m.which() == 'longitudinalPlan'], strict=True)
+
+        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='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()
+
+        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, brakePressed, label='brakePressed', linewidth=6)
+        for i in (2, 3):
+          ax[i].set_yticks([0, 1], minor=False)
+          ax[i].set_ylim(-1, 2)
+          ax[i].legend()
+
+        ax[-1].set_xlabel("Time (s)")
+        fig.tight_layout()
+
+        buffer = io.BytesIO()
+        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")
+
+  print(f"\nReport written to {output_fn}\n")
+
+
+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)')
+
+  args = parser.parse_args()
+
+  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('processing report for', platform)
+
+  maneuvers: list[tuple[str, list[list]]] = []
+  active_prev = False
+  description_prev = None
+
+  for msg in lr:
+    if msg.which() == 'alertDebug':
+      active = 'Maneuver Active' in msg.alertDebug.alertText1
+      if active and not active_prev:
+        if msg.alertDebug.alertText2 == description_prev:
+          maneuvers[-1][1].append([])
+        else:
+          maneuvers.append((msg.alertDebug.alertText2, [[]]))
+        description_prev = maneuvers[-1][0]
+      active_prev = active
+
+    if active_prev:
+      maneuvers[-1][1][-1].append(msg)
+
+  report(platform, maneuvers)

tools/longitudinal_maneuvers/maneuversd.py

@@ -0,0 +1,165 @@
+#!/usr/bin/env python3
+from dataclasses import dataclass
+
+from cereal import messaging, car
+from opendbc.car.common.conversions import Conversions as CV
+from openpilot.common.realtime import DT_MDL
+from openpilot.common.params import Params
+from openpilot.common.swaglog import cloudlog
+
+
+@dataclass
+class Action:
+  accel: float      # m/s^2
+  duration: float   # seconds
+
+
+@dataclass
+class Maneuver:
+  description: str
+  actions: list[Action]
+  repeat: int = 0
+  initial_speed: float = 0.  # m/s
+
+  _active: bool = False
+  _finished: bool = False
+  _action_index: int = 0
+  _action_frames: int = 0
+  _ready_cnt: int = 0
+  _repeated: int = 0
+
+  def get_accel(self, v_ego: float, enabled: bool, standstill: bool) -> float:
+    ready = abs(v_ego - self.initial_speed) < 0.3 and enabled and not standstill
+    self._ready_cnt = (self._ready_cnt + 1) if ready else 0
+
+    if self._ready_cnt > (3. / DT_MDL):
+      self._active = True
+
+    if not self._active:
+      return min(max(self.initial_speed - v_ego, -2.), 2.)
+
+    action = self.actions[self._action_index]
+
+    self._action_frames += 1
+
+    # reached duration of action
+    if self._action_frames > (action.duration / DT_MDL):
+      # next action
+      if self._action_index < len(self.actions) - 1:
+        self._action_index += 1
+        self._action_frames = 0
+      # repeat maneuver
+      elif self._repeated < self.repeat:
+        self._repeated += 1
+        self._action_index = 0
+        self._action_frames = 0
+        self._active = False
+      # finish maneuver
+      else:
+        self._finished = True
+
+    return action.accel
+
+  @property
+  def finished(self):
+    return self._finished
+
+  @property
+  def active(self):
+    return self._active
+
+
+MANEUVERS = [
+  Maneuver(
+   "creep: alternate between +1m/s^2 and -1m/s^2",
+   [
+     Action(1, 2), Action(-1, 2),
+     Action(1, 2), Action(-1, 2),
+     Action(1, 2), Action(-1, 2),
+   ],
+   repeat=1,
+   initial_speed=0.,
+  ),
+  Maneuver(
+    "brake step response: -1m/s^2 from 20mph",
+    [Action(-1, 3)],
+    repeat=2,
+    initial_speed=20. * CV.MPH_TO_MS,
+  ),
+  Maneuver(
+    "brake step response: -4m/s^2 from 20mph",
+    [Action(-4, 3)],
+    repeat=2,
+    initial_speed=20. * CV.MPH_TO_MS,
+  ),
+  Maneuver(
+    "gas step response: +1m/s^2 from 20mph",
+    [Action(1, 3)],
+    repeat=2,
+    initial_speed=20. * CV.MPH_TO_MS,
+  ),
+  Maneuver(
+    "gas step response: +4m/s^2 from 20mph",
+    [Action(4, 3)],
+    repeat=2,
+    initial_speed=20. * CV.MPH_TO_MS,
+  ),
+]
+
+
+def main():
+  params = Params()
+  cloudlog.info("joystickd is waiting for CarParams")
+  CP = messaging.log_from_bytes(params.get("CarParams", block=True), car.CarParams)
+
+  sm = messaging.SubMaster(['carState', 'controlsState', 'selfdriveState', 'modelV2'], poll='modelV2')
+  pm = messaging.PubMaster(['longitudinalPlan', 'driverAssistance', 'alertDebug'])
+
+  maneuvers = iter(MANEUVERS)
+  maneuver = None
+
+  while True:
+    sm.update()
+
+    if maneuver is None:
+      maneuver = next(maneuvers, None)
+
+    alert_msg = messaging.new_message('alertDebug')
+    alert_msg.valid = True
+
+    plan_send = messaging.new_message('longitudinalPlan')
+    plan_send.valid = sm.all_checks()
+
+    longitudinalPlan = plan_send.longitudinalPlan
+    accel = 0
+    cs = sm['carState']
+    v_ego = max(cs.vEgo, 0)
+
+    if maneuver is not None:
+      accel = maneuver.get_accel(v_ego, cs.cruiseState.enabled, cs.cruiseState.standstill)
+
+      if maneuver.active:
+        alert_msg.alertDebug.alertText1 = f'Maneuver Active: {accel:0.2f} m/s^2'
+      else:
+        alert_msg.alertDebug.alertText1 = f'Reaching Speed: {maneuver.initial_speed * CV.MS_TO_MPH:0.2f} mph'
+      alert_msg.alertDebug.alertText2 = f'{maneuver.description}'
+    else:
+      alert_msg.alertDebug.alertText1 = 'Maneuvers Finished'
+
+    pm.send('alertDebug', alert_msg)
+
+    longitudinalPlan.aTarget = accel
+    longitudinalPlan.shouldStop = v_ego < CP.vEgoStopping and accel < 1e-2
+
+    longitudinalPlan.allowBrake = True
+    longitudinalPlan.allowThrottle = True
+    longitudinalPlan.hasLead = True
+
+    pm.send('longitudinalPlan', plan_send)
+
+    assistance_send = messaging.new_message('driverAssistance')
+    assistance_send.valid = True
+    pm.send('driverAssistance', assistance_send)
+
+    if maneuver is not None and maneuver.finished:
+      maneuver = None