IQ.Pilot Release Commit @ 0798119

tools/lateral_maneuvers/.gitignore

@@ -0,0 +1 @@
+/lateral_reports/

tools/lateral_maneuvers/README.md

@@ -0,0 +1,54 @@
+# Lateral Maneuvers Testing Tool
+
+> [!WARNING]
+> Use caution when using this tool.
+
+Test your vehicle's lateral control tuning with this tool. The tool will test the vehicle's ability to follow a few lateral maneuvers and includes a tool to generate a report from the route.
+
+## Instructions
+
+1. Check out a development branch such as `master-mici` on your device. The toggle is hidden on release branches.
+2. The full maneuver suite runs at 20 and 30 mph.
+3. Enable "Lateral Maneuver Mode" in Settings > Developer on the device while offroad. Alternatively, set the parameter manually:
+
+   ```sh
+   echo -n 1 > /data/params/d/LateralManeuverMode
+   ```
+
+4. Turn your vehicle back on. You will see "Lateral Maneuver Mode".
+
+5. Ensure the area ahead is clear, as IQ.Pilot will command lateral acceleration steps in this mode. Once you are ready, set ACC manually to the target speed shown on screen and let IQ.Pilot stabilize lateral. After 2 seconds of steady straight driving on a road under 250 m radius and under 6.8° of roll, the maneuver will begin automatically. IQ.Pilot lateral control stays engaged between maneuvers normally while waiting for the next maneuver's readiness conditions. The maneuver will be aborted and repeated if speed is out of range, the steering wheel or gas is touched, or IQ.Pilot disengages.
+
+6. When the testing is complete, you'll see an alert that says "Maneuvers Finished." Complete the route by pulling over and turning off the vehicle.
+
+7. Locate the route(s) — they will stand out with lots of orange intervals in their timeline. Ensure "All logs" show as "uploaded."
+
+8. Gather the route ID and then run the report generator. The file will be exported to the same directory:
+
+    ```sh
+    $ python tools/lateral_maneuvers/generate_report.py 98395b7c5b27882e/000001cc--5a73bde686
+
+    processing report for KIA_EV6
+    plotting maneuver: step right 20mph, runs: 3
+    plotting maneuver: step left 20mph, runs: 3
+    plotting maneuver: sine 0.5Hz 20mph, runs: 3
+    plotting maneuver: step right 30mph, runs: 3
+
+    Opening report: tools/lateral_maneuvers/lateral_reports/KIA_EV6_98395b7c5b27882e_000001cc--5a73bde686.html
+    ```
+
+   The IQ.Pilot `generate_report.py` also takes a path to a local `rlog.zst` or a directory of them, supports
+   auto-detection of lateral sweeps in any route without `alertDebug` markers (pass `--auto`), and ranks the
+   top-N highest-peak sweeps by speed/peak filters. See `generate_report.py --help`.
+
+## Testing the tooling without a car
+
+`sim_maneuvers.py` runs `lateral_maneuversd` as a real process against a synthetic steering rack and writes an
+rlog that `generate_report.py` reads. Use it to verify the daemon and the report generator after changing either:
+
+```sh
+$ python tools/lateral_maneuvers/sim_maneuvers.py --out /tmp/lat/rlog.zst
+$ python tools/lateral_maneuvers/generate_report.py /tmp/lat/rlog.zst
+```
+
+The full suite takes about 5 minutes of wall clock; `--max-maneuvers N` stops early.

tools/lateral_maneuvers/generate_report.py

@@ -0,0 +1,261 @@
+#!/usr/bin/env python3
+import argparse
+import base64
+import io
+import math
+import numpy as np
+import os
+import webbrowser
+from collections import defaultdict
+from pathlib import Path
+import matplotlib.pyplot as plt
+from openpilot.common.utils import tabulate
+
+from cereal import car
+from openpilot.common.filter_simple import FirstOrderFilter
+from openpilot.selfdrive.controls.lib.latcontrol_torque import LP_FILTER_CUTOFF_HZ
+from openpilot.tools.lib.logreader import LogReader
+from openpilot.system.hardware.hw import Paths
+from openpilot.common.constants import CV
+from openpilot.tools.longitudinal_maneuvers.generate_report import format_car_params
+
+ANGLE_CONTROL = (car.CarParams.SteerControlType.angle, car.CarParams.SteerControlType.curvatureDEPRECATED)
+
+
+def lat_accel(curvature, v):
+  return curvature * max(v, 1.0) ** 2
+
+
+def report(platform, route, _description, CP, ID, maneuvers):
+  output_path = Path(__file__).resolve().parent / "lateral_reports"
+  output_fn = output_path / f"{platform}_{route.replace('/', '_').replace('|', '_')}.html"
+  output_path.mkdir(exist_ok=True)
+  target_cross_times = defaultdict(list)
+
+  builder = [
+    "<style>summary { cursor: pointer; }\n td, th { padding: 8px; } </style>\n",
+    "<h1>Lateral maneuver report</h1>\n",
+    f"<h3>{platform}</h3>\n",
+    f"<h3>{route}</h3>\n",
+    f"<h3>{ID.gitCommit}, {ID.gitBranch}, {ID.gitRemote}</h3>\n",
+  ]
+  if _description is not None:
+    builder.append(f"<h3>Description: {_description}</h3>\n")
+  builder.append(f"<details><summary><h3 style='display: inline-block;'>CarParams</h3></summary><pre>{format_car_params(CP)}</pre></details>\n")
+  builder.append('{ summary }')  # to be replaced below
+  for description, runs in maneuvers:
+    # filter incomplete runs
+    completed_runs = [msgs for msgs in runs
+                      if any(m.alertDebug.alertText1 == 'Complete' for m in msgs if m.which() == 'alertDebug')]
+    print(f'plotting maneuver: {description}, runs: {len(completed_runs)}')
+    if not completed_runs:
+      continue
+    builder.append("<div style='border-top: 1px solid #000; margin: 20px 0;'></div>\n")
+    builder.append(f"<h2>{description}</h2>\n")
+    for run, msgs in enumerate(completed_runs):
+      last_active = max(m.logMonoTime for m in msgs if m.which() == 'lateralManeuverPlan' and m.valid)
+      msgs = [m for m in msgs if m.logMonoTime <= last_active]
+      t_carControl, carControl = zip(*[(m.logMonoTime, m.carControl) for m in msgs if m.which() == 'carControl'], strict=True)
+      t_carState, carState = zip(*[(m.logMonoTime, m.carState) for m in msgs if m.which() == 'carState'], strict=True)
+      t_controlsState, controlsState = zip(*[(m.logMonoTime, m.controlsState) for m in msgs if m.which() == 'controlsState'], strict=True)
+      t_lateralPlan, lateralPlan = zip(*[(m.logMonoTime, m.lateralManeuverPlan) for m in msgs if m.which() == 'lateralManeuverPlan' and m.valid], strict=True)
+      t_carOutput, carOutput = zip(*[(m.logMonoTime, m.carOutput) for m in msgs if m.which() == 'carOutput'], strict=True)
+
+      # make time relative seconds
+      t_carControl = [(t - t_carControl[0]) / 1e9 for t in t_carControl]
+      t_carState = [(t - t_carState[0]) / 1e9 for t in t_carState]
+      t_controlsState = [(t - t_controlsState[0]) / 1e9 for t in t_controlsState]
+      t_lateralPlan = [(t - t_lateralPlan[0]) / 1e9 for t in t_lateralPlan]
+      t_carOutput = [(t - t_carOutput[0]) / 1e9 for t in t_carOutput]
+
+      # maneuver validity
+      latActive = [m.latActive for m in carControl]
+      maneuver_valid = all(latActive) and not any(cs.steeringPressed 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 '')
+
+      builder.append(f"<details {_open}><summary><h3 style='display: inline-block;'>{title}</h3></summary>\n")
+
+      baseline_accel = lat_accel(controlsState[0].curvature, carState[0].vEgo)
+      v_ego = [m.vEgo for m in carState]
+      cross_markers = []
+
+      if description.startswith(('sine', 'jitter')):
+        amplitude = max(abs(lat_accel(lp.desiredCurvature, v) - baseline_accel)
+                        for lp, v in zip(lateralPlan, v_ego, strict=False))
+        threshold = amplitude * 0.5
+        builder.append('<h3 style="font-weight: normal">50% peak')
+        for t, cs, v in zip(t_controlsState, controlsState, v_ego, strict=False):
+          actual = lat_accel(cs.curvature, v) - baseline_accel
+          if abs(actual) > threshold:
+            builder.append(f', <strong>crossed in {t:.3f}s</strong>')
+            cross_markers.append((t, actual + baseline_accel))
+            if maneuver_valid:
+              target_cross_times[description].append(t)
+            break
+        else:
+          builder.append(', <strong>not crossed</strong>')
+        builder.append('</h3>')
+        if maneuver_valid:
+          target_cross_times.setdefault(description, [])
+      else:
+        action_targets = [(0, lat_accel(lateralPlan[0].desiredCurvature, v_ego[0]) - baseline_accel)]
+        for i in range(1, min(len(lateralPlan), len(v_ego))):
+          if abs(lateralPlan[i].desiredCurvature - lateralPlan[i - 1].desiredCurvature) > 0.001:
+            desired = lat_accel(lateralPlan[i].desiredCurvature, v_ego[i]) - baseline_accel
+            action_targets.append((i, desired))
+
+        for j, (start_i, act_target) in enumerate(action_targets):
+          start_time = t_lateralPlan[start_i]
+          end_time = t_lateralPlan[action_targets[j + 1][0]] if j + 1 < len(action_targets) else t_controlsState[-1]
+
+          builder.append(f'<h3 style="font-weight: normal">aTarget: {round(act_target, 1)} m/s^2')
+          prev_crossed = False
+          for t, cs, v in zip(t_controlsState, controlsState, v_ego, strict=False):
+            if not (start_time <= t <= end_time):
+              continue
+            actual_accel = lat_accel(cs.curvature, v) - baseline_accel
+            crossed = (0 < act_target < actual_accel) or (0 > act_target > actual_accel)
+            if crossed and prev_crossed:
+              cross_time = t - start_time
+              builder.append(f', <strong>crossed in {cross_time:.3f}s</strong>')
+              cross_markers.append((t, act_target + baseline_accel))
+              if maneuver_valid:
+                target_cross_times[description].append(cross_time)
+              break
+            prev_crossed = crossed
+          else:
+            builder.append(', <strong>not crossed</strong>')
+          builder.append('</h3>')
+          if maneuver_valid:
+            target_cross_times.setdefault(description, [])
+
+      plt.rcParams['font.size'] = 40
+      fig = plt.figure(figsize=(30, 40))
+      ax = fig.subplots(5, 1, sharex=True, gridspec_kw={'height_ratios': [5, 5, 3, 3, 3]})
+
+      ax[0].grid(linewidth=4)
+      desired_label = 'lateralManeuverPlan.desiredCurvature * vEgo^2'
+      desired_lat_accel = [lat_accel(m.desiredCurvature, v) for m, v in zip(lateralPlan, v_ego, strict=False)]
+      if description.startswith(('sine', 'jitter')):
+        ax[0].plot(t_lateralPlan[:len(desired_lat_accel)], desired_lat_accel, 'C1', label=desired_label, linewidth=6)
+      else:
+        t_desired = [t_lateralPlan[0]] + t_lateralPlan[:len(desired_lat_accel)]
+        desired_lat_accel = [baseline_accel] + desired_lat_accel
+        ax[0].step(t_desired, desired_lat_accel, 'C1', label=desired_label, linewidth=6, where='post')
+      actual_lat_accel = [lat_accel(cs.curvature, v) for cs, v in zip(controlsState, v_ego, strict=False)]
+      ax[0].plot(t_controlsState[:len(actual_lat_accel)], actual_lat_accel, 'g', label='controlsState.curvature * vEgo^2', linewidth=6)
+      ax[0].set_ylabel('Lateral Accel (m/s^2)')
+      for ct, cv in cross_markers:
+        ax[0].plot(ct, cv, marker='o', markersize=50, markeredgewidth=7, markeredgecolor='black', markerfacecolor='None')
+      ax[0].legend(prop={'size': 30})
+
+      ax[1].grid(linewidth=4)
+      if CP.steerControlType in ANGLE_CONTROL:
+        steer_field, steer_ylabel = 'steeringAngleDeg', 'Steer angle (deg)'
+      else:
+        steer_field, steer_ylabel = 'torque', 'Steer torque'
+      ax[1].plot(t_carControl, [getattr(m.actuators, steer_field) for m in carControl], 'C1', label=f'carControl.actuators.{steer_field}', linewidth=6)
+      ax[1].plot(t_carOutput, [getattr(m.actuatorsOutput, steer_field) for m in carOutput], 'g', label=f'carOutput.actuatorsOutput.{steer_field}', linewidth=6)
+      ax[1].set_ylabel(steer_ylabel)
+      ax[1].legend(prop={'size': 30})
+
+      ax[2].grid(linewidth=4)
+      ax[2].plot(t_carState, [v * CV.MS_TO_MPH for v in v_ego], label='carState.vEgo', linewidth=6)
+      ax[2].set_ylabel('Velocity (mph)')
+      ax[2].yaxis.set_major_formatter(plt.FormatStrFormatter('%.1f'))
+      ax[2].legend()
+
+      t_accel = np.array(t_controlsState[:len(actual_lat_accel)])
+      raw_jerk = np.gradient(actual_lat_accel, t_accel)
+      dt_avg = np.mean(np.diff(t_accel))
+      jerk_filter = FirstOrderFilter(0.0, 1 / (2 * np.pi * LP_FILTER_CUTOFF_HZ), dt_avg)
+      filtered_jerk = [jerk_filter.update(j) for j in raw_jerk]
+      ax[3].grid(linewidth=4)
+      ax[3].plot(t_accel, filtered_jerk, label='d/dt(controlsState.curvature * vEgo^2)', linewidth=6)
+      ax[3].set_ylabel('Jerk (m/s^3)')
+      ax[3].legend()
+
+      ax[4].grid(linewidth=4)
+      ax[4].plot(t_carControl, [math.degrees(m.orientationNED[0]) if len(m.orientationNED) == 3 else 0.0 for m in carControl],
+                 label='carControl.orientationNED[0]', linewidth=6)
+      ax[4].set_ylabel('Roll (deg)')
+      ax[4].legend()
+
+      ax[-1].set_xlabel("Time (s)")
+      fig.tight_layout()
+
+      buffer = io.BytesIO()
+      fig.savefig(buffer, format='webp')
+      plt.close(fig)
+      buffer.seek(0)
+      builder.append(f"<img src='data:image/webp;base64,{base64.b64encode(buffer.getvalue()).decode()}' style='width:100%; max-width:800px;'>\n")
+      builder.append("</details>\n")
+
+  summary = ["<h2>Summary</h2>\n"]
+  cols = ['maneuver', 'crossed', 'mean', 'min', 'max']
+  table = []
+  for description, times in target_cross_times.items():
+    l = [description, len(times)]
+    if len(times):
+      l.extend([round(sum(times) / len(times), 2), round(min(times), 2), round(max(times), 2)])
+    table.append(l)
+  summary.append(tabulate(table, headers=cols, tablefmt='html', numalign='left') + '\n')
+
+  sum_idx = builder.index('{ summary }')
+  builder[sum_idx:sum_idx + 1] = summary
+
+  with open(output_fn, "w") as f:
+    f.write(''.join(builder))
+
+  print(f"\nOpening report: {output_fn}\n")
+  webbrowser.open_new_tab(str(output_fn))
+
+
+def open_route(route: str) -> LogReader:
+  if os.path.isdir(route):
+    rlogs = sorted(str(p) for p in Path(route).glob("*rlog.zst"))
+    if not rlogs:
+      raise SystemExit(f"no *rlog.zst files in {route}")
+    print(f"loading {len(rlogs)} rlogs from {route}")
+    return LogReader(rlogs, only_union_types=True)
+  if os.path.exists(route) or '/' in route or '|' in route:
+    return LogReader(route, only_union_types=True)
+  segs = [seg for seg in os.listdir(Paths.log_root()) if route in seg]
+  return LogReader([os.path.join(Paths.log_root(), seg, 'rlog.zst') for seg in segs], only_union_types=True)
+
+
+if __name__ == '__main__':
+  parser = argparse.ArgumentParser(description='Generate lateral maneuver report from route')
+  parser.add_argument('route', type=str, help='Route name, local rlog path, or directory of rlogs')
+  parser.add_argument('description', type=str, nargs='?')
+
+  args = parser.parse_args()
+
+  lr = open_route(args.route)
+
+  CP = lr.first('carParams')
+  ID = lr.first('initData')
+  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 = 'Active' in msg.alertDebug.alertText1 or msg.alertDebug.alertText1 == 'Complete'
+      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, args.route, args.description, CP, ID, maneuvers)

tools/lateral_maneuvers/lateral_maneuversd.py

@@ -0,0 +1,213 @@
+#!/usr/bin/env python3
+import numpy as np
+from dataclasses import dataclass
+
+from cereal import messaging, car
+from openpilot.common.constants import CV
+from openpilot.common.realtime import DT_MDL, Ratekeeper
+from openpilot.common.params import Params
+from openpilot.common.swaglog import cloudlog
+from openpilot.selfdrive.controls.lib.drive_helpers import MIN_SPEED
+from openpilot.tools.longitudinal_maneuvers.maneuversd import Action, Maneuver as _Maneuver
+
+# thresholds for starting maneuvers
+MAX_SPEED_DEV = 0.7  # deviation in m/s
+MAX_CURV = 0.004     # 250 m radius
+MAX_ROLL = 0.12      # 6.8°
+TIMER = 2.0          # sec stable conditions before starting maneuver
+
+
+@dataclass
+class Maneuver(_Maneuver):
+  _baseline_curvature: float = 0.0
+
+  def get_accel(self, v_ego: float, lat_active: bool, curvature: float, roll: float) -> float:
+    self._run_completed = False
+    # only start maneuver on straight, flat roads
+    ready = abs(v_ego - self.initial_speed) < MAX_SPEED_DEV and lat_active and abs(curvature) < MAX_CURV and abs(roll) < MAX_ROLL
+    self._ready_cnt = (self._ready_cnt + 1) if ready else max(self._ready_cnt - 1, 0)
+
+    if self._ready_cnt > (TIMER / DT_MDL):
+      if not self._active:
+        self._baseline_curvature = curvature
+      self._active = True
+
+    if not self._active:
+      return 0.0
+
+    return self._step()
+
+  def reset(self):
+    super().reset()
+    self._ready_cnt = 0
+
+
+def _sine_action(amplitude, period, duration):
+  t = np.linspace(0, duration, int(duration / DT_MDL) + 1)
+  a = amplitude * np.sin(2 * np.pi * t / period)
+  return Action(a.tolist(), t.tolist())
+
+
+MANEUVERS = [
+  Maneuver(
+    "step right 20mph",
+    [Action([0.5], [1.0]), Action([-0.5], [1.5])],
+    repeat=2,
+    initial_speed=20. * CV.MPH_TO_MS,
+  ),
+  Maneuver(
+    "step left 20mph",
+    [Action([-0.5], [1.0]), Action([0.5], [1.5])],
+    repeat=2,
+    initial_speed=20. * CV.MPH_TO_MS,
+  ),
+  Maneuver(
+    "sine 0.5Hz 20mph",
+    [_sine_action(1.0, 2.0, 2.0), Action([0.0], [0.5])],
+    repeat=2,
+    initial_speed=20. * CV.MPH_TO_MS,
+  ),
+  Maneuver(
+    "jitter 20mph",
+    [Action([-0.5 if i % 2 == 0 else 0.5], [0.1]) for i in range(10)],
+    repeat=2,
+    initial_speed=20. * CV.MPH_TO_MS,
+  ),
+  Maneuver(
+    "step right 30mph",
+    [Action([0.5], [1.0]), Action([-0.5], [1.5])],
+    repeat=2,
+    initial_speed=30. * CV.MPH_TO_MS,
+  ),
+  Maneuver(
+    "step left 30mph",
+    [Action([-0.5], [1.0]), Action([0.5], [1.5])],
+    repeat=2,
+    initial_speed=30. * CV.MPH_TO_MS,
+  ),
+  Maneuver(
+    "sine 0.5Hz 30mph",
+    [_sine_action(1.0, 2.0, 2.0), Action([0.0], [0.5])],
+    repeat=2,
+    initial_speed=30. * CV.MPH_TO_MS,
+  ),
+  Maneuver(
+    "jitter 30mph",
+    [Action([-0.5 if i % 2 == 0 else 0.5], [0.1]) for i in range(10)],
+    repeat=2,
+    initial_speed=30. * CV.MPH_TO_MS,
+  ),
+]
+
+
+def main():
+  params = Params()
+  cloudlog.info("lateral_maneuversd is waiting for CarParams")
+  messaging.log_from_bytes(params.get("CarParams", block=True), car.CarParams)
+
+  # iqpilot: subscribe only to the services we actually read and drive timing with a
+  # Ratekeeper instead of polling modelV2. msgq caps each topic at NUM_READERS (15) and
+  # evicts ALL subscribers when exceeded; iqpilot runs many daemons, and unlike longitudinal
+  # maneuver mode (which disables plannerd), lateral mode keeps plannerd running. Subscribing
+  # to selfdriveState/modelV2 here (selfdriveState is unused; modelV2 was only a poll source)
+  # tips those topics past 15 → eviction storm → UI/speed render drops to a few fps.
+  sm = messaging.SubMaster(['carState', 'carControl', 'controlsState'])
+  pm = messaging.PubMaster(['lateralManeuverPlan', 'alertDebug'])
+  rk = Ratekeeper(int(1. / DT_MDL), print_delay_threshold=None)  # 20 Hz, matches DT_MDL maneuver timing
+
+  maneuvers = iter(MANEUVERS)
+  maneuver = None
+  complete_cnt = 0
+  aborted_cnt = 0
+  abort_reason = ''
+  display_holdoff = 0
+  prev_text = ''
+
+  while True:
+    sm.update(0)
+
+    if maneuver is None:
+      maneuver = next(maneuvers, None)
+
+    alert_msg = messaging.new_message('alertDebug')
+    alert_msg.valid = True
+
+    plan_send = messaging.new_message('lateralManeuverPlan')
+
+    accel = 0
+    v_ego = max(sm['carState'].vEgo, 0)
+    curvature = sm['controlsState'].desiredCurvature
+
+    if complete_cnt > 0:
+      complete_cnt -= 1
+      alert_msg.alertDebug.alertText1 = 'Completed'
+      alert_msg.alertDebug.alertText2 = maneuver.description
+    elif maneuver is not None:
+      # any driver input aborts the maneuver
+      CS = sm['carState']
+      if CS.steeringPressed or CS.gasPressed:
+        aborted_cnt = int(1.0 / DT_MDL)
+        abort_reason = ('steering pressed' if CS.steeringPressed else 'gas pressed').ljust(20)
+      aborted = aborted_cnt > 0
+      speed_out_of_range = maneuver.active and abs(v_ego - maneuver.initial_speed) > MAX_SPEED_DEV
+      if aborted or speed_out_of_range:
+        maneuver.reset()
+
+      roll = sm['carControl'].orientationNED[0] if len(sm['carControl'].orientationNED) == 3 else 0.0
+      accel = maneuver.get_accel(v_ego, sm['carControl'].latActive, curvature, roll)
+
+      if maneuver._run_completed:
+        complete_cnt = int(1.0 / DT_MDL)
+        alert_msg.alertDebug.alertText1 = 'Complete'
+        alert_msg.alertDebug.alertText2 = maneuver.description
+      elif maneuver.active:
+        action_remaining = maneuver.actions[maneuver._action_index].time_bp[-1] - maneuver._action_frames * DT_MDL
+        if maneuver.description.startswith('sine'):
+          freq = maneuver.description.split()[1]
+          alert_msg.alertDebug.alertText1 = f'Active sine {freq} {max(action_remaining, 0):.1f}s'
+        else:
+          alert_msg.alertDebug.alertText1 = f'Active {accel:+.1f}m/s² {max(action_remaining, 0):.1f}s'
+        alert_msg.alertDebug.alertText2 = maneuver.description
+      elif aborted_cnt > 0:
+        aborted_cnt -= 1
+        alert_msg.alertDebug.alertText1 = abort_reason
+      elif not (abs(v_ego - maneuver.initial_speed) < MAX_SPEED_DEV and sm['carControl'].latActive):
+        alert_msg.alertDebug.alertText1 = f'Set speed to {maneuver.initial_speed * CV.MS_TO_MPH:0.0f} mph'
+      elif maneuver._ready_cnt > 0:
+        ready_time = max(TIMER - maneuver._ready_cnt * DT_MDL, 0)
+        alert_msg.alertDebug.alertText1 = f'Starting: {int(ready_time) + 1}'
+        alert_msg.alertDebug.alertText2 = maneuver.description
+      else:
+        curv_ok = abs(curvature) < MAX_CURV
+        reason = 'road not straight' if not curv_ok else 'road not flat'
+        alert_msg.alertDebug.alertText1 = f'Waiting: {reason}'
+        alert_msg.alertDebug.alertText2 = maneuver.description
+    else:
+      alert_msg.alertDebug.alertText1 = 'Maneuvers Finished'
+
+    # prevent flickering text
+    setup = ('Set speed', 'Starting', 'Waiting')
+    text = alert_msg.alertDebug.alertText1
+    same = text == prev_text or (text.startswith('Starting') and prev_text.startswith('Starting'))
+    if not same and text.startswith(setup) and prev_text.startswith(setup) and display_holdoff > 0:
+      alert_msg.alertDebug.alertText1 = prev_text
+      display_holdoff -= 1
+    else:
+      prev_text = text
+      display_holdoff = int(0.5 / DT_MDL) if text.startswith(setup) else 0
+
+    pm.send('alertDebug', alert_msg)
+
+    plan_send.valid = maneuver is not None and maneuver.active and complete_cnt == 0
+    if plan_send.valid:
+      plan_send.lateralManeuverPlan.desiredCurvature = maneuver._baseline_curvature + accel / max(v_ego, MIN_SPEED) ** 2
+    pm.send('lateralManeuverPlan', plan_send)
+
+    if maneuver is not None and maneuver.finished and complete_cnt == 0:
+      maneuver = None
+
+    rk.keep_time()
+
+
+if __name__ == "__main__":
+  main()

tools/lateral_maneuvers/sim_maneuvers.py

@@ -0,0 +1,73 @@
+#!/usr/bin/env python3
+"""Run lateral_maneuversd against a synthetic lateral plant and write an rlog.
+
+    ./tools/lateral_maneuvers/sim_maneuvers.py --out /tmp/lat_rlog.zst
+    ./tools/lateral_maneuvers/generate_report.py /tmp/lat_rlog.zst
+"""
+import argparse
+import re
+from pathlib import Path
+
+from openpilot.common.constants import CV
+from openpilot.tools.lateral_maneuvers.lateral_maneuversd import MANEUVERS
+from openpilot.tools.longitudinal_maneuvers.sim_harness import ManeuverSim, Plant
+
+CURV_TAU = 0.05  # controlsd curvature command tracking
+RACK_WN = 8.0    # steering rack + tire natural frequency (rad/s)
+RACK_ZETA = 0.7  # underdamped, so achieved curvature overshoots like a real rack
+CRUISE_ACCEL = 1.2
+
+SET_SPEED_RE = re.compile(r"Set speed to (\d+) mph")
+
+
+class LateralPlant(Plant):
+  PLAN = 'lateralManeuverPlan'
+
+  def __init__(self):
+    super().__init__(v_ego=MANEUVERS[0].initial_speed)
+    self.sim = None
+    self._rack_rate = 0.0
+    self.target_speed = MANEUVERS[0].initial_speed
+    self._by_description = {m.description: m.initial_speed for m in MANEUVERS}
+
+  def _update_target(self):
+    if self.sim is None:
+      return
+    speed = self._by_description.get(self.sim.alert2)
+    if speed is None:
+      match = SET_SPEED_RE.search(self.sim.alert1)
+      speed = float(match.group(1)) * CV.MPH_TO_MS if match else None
+    if speed is not None:
+      self.target_speed = speed
+
+  def step(self, dt, plan):
+    self._update_target()
+
+    err = self.target_speed - self.v_ego
+    self.a_ego = max(min(err / 1.0, CRUISE_ACCEL), -CRUISE_ACCEL)
+    self.v_ego = max(self.v_ego + self.a_ego * dt, 0.0)
+
+    desired_curvature = float(plan.desiredCurvature) if plan is not None else 0.0
+    self.curvature += (dt / (CURV_TAU + dt)) * (desired_curvature - self.curvature)
+
+    self._rack_rate += dt * (RACK_WN ** 2 * (self.curvature - self.achieved_curvature) - 2 * RACK_ZETA * RACK_WN * self._rack_rate)
+    self.achieved_curvature += dt * self._rack_rate
+    self.lat_accel = self.achieved_curvature * max(self.v_ego, 1.0) ** 2
+
+
+def main():
+  parser = argparse.ArgumentParser(description=__doc__)
+  parser.add_argument("--out", type=Path, default=Path("/tmp/lateral_maneuvers_sim/rlog.zst"))
+  parser.add_argument("--max-maneuvers", type=int, default=0, help="stop after N maneuvers (0 = all)")
+  parser.add_argument("--timeout", type=float, default=900.0)
+  args = parser.parse_args()
+
+  sim = ManeuverSim("openpilot.tools.lateral_maneuvers.lateral_maneuversd", LateralPlant(),
+                    max_maneuvers=args.max_maneuvers, timeout=args.timeout)
+  out = sim.run(args.out)
+  print(f"\nmaneuvers seen: {sim.seen_maneuvers}")
+  print(f"rlog: {out} ({out.stat().st_size / 1e6:.1f} MB)")
+
+
+if __name__ == "__main__":
+  main()