IQ.Pilot Release Commit @ 2b39aa6

tools/longitudinal_maneuvers/README.md

@@ -6,9 +6,9 @@ Test your vehicle's longitudinal control tuning with this tool. The tool will te
 
 ## Instructions
 
-1. Check out a development branch such as `master` on your comma device.
-2. Locate either a large empty parking lot or road devoid of any car or foot traffic. Flat, straight road is preferred. The full maneuver suite can take 1 mile or more if left running, however it is recommended to disengage openpilot between maneuvers and turn around if there is not enough space.
-3. Turn off the vehicle and set this parameter which will signal to openpilot to start the longitudinal maneuver daemon:
+1. Check out a development branch such as `master-mici` on your device. The toggle is hidden on release branches.
+2. Locate either a large empty parking lot or road devoid of any car or foot traffic. Flat, straight road is preferred. The full maneuver suite can take 1 mile or more if left running, however it is recommended to disengage IQ.Pilot between maneuvers and turn around if there is not enough space.
+3. Turn off the vehicle and enable "Longitudinal Maneuver Mode" in Settings > Developer. The toggle requires IQ.Pilot longitudinal control and only enables while offroad. Alternatively, set the parameter manually:
 
    ```sh
    echo -n 1 > /data/params/d/LongitudinalManeuverMode
@@ -42,7 +42,19 @@ Test your vehicle's longitudinal control tuning with this tool. The tool will te
     plotting maneuver: creep: alternate between +1m/s^2 and -1m/s^2, runs: 2
     plotting maneuver: gas step response: +1m/s^2 from 20mph, runs: 2
 
-    Report written to /home/batman/openpilot/tools/longitudinal_maneuvers/longitudinal_reports/LEXUS_ES_TSS2_57048cfce01d9625_0000010e--5b26bc3be7.html
+    Report written to tools/longitudinal_maneuvers/longitudinal_reports/LEXUS_ES_TSS2_57048cfce01d9625_0000010e--5b26bc3be7.html
     ```
 
-You can reach out on [Discord](https://discord.comma.ai) if you have any questions about these instructions or the tool itself.
+   `generate_report.py` also takes a path to a local `rlog.zst` or a directory of them.
+
+## Testing the tooling without a car
+
+`sim_maneuvers.py` runs `maneuversd` as a real process against a synthetic powertrain 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/longitudinal_maneuvers/sim_maneuvers.py --out /tmp/long/rlog.zst
+$ python tools/longitudinal_maneuvers/generate_report.py /tmp/long/rlog.zst
+```
+
+The full suite takes about 4 minutes of wall clock; `--max-maneuvers N` stops early.

tools/longitudinal_maneuvers/generate_report.py

@@ -9,7 +9,7 @@ import webbrowser
 from collections import defaultdict
 from pathlib import Path
 import matplotlib.pyplot as plt
-from tabulate import tabulate
+from openpilot.common.utils import tabulate
 
 from openpilot.tools.lib.logreader import LogReader
 from openpilot.system.hardware.hw import Paths

tools/longitudinal_maneuvers/sim_harness.py

@@ -0,0 +1,240 @@
+#!/usr/bin/env python3
+"""Closed-loop offline harness for the maneuver daemons.
+
+Runs maneuversd / lateral_maneuversd as real subprocesses over msgq, drives them with a
+synthetic vehicle, and records every message to an rlog that generate_report.py can read.
+Used to validate the maneuver tooling without a car.
+"""
+import math
+import os
+import signal
+import subprocess
+import sys
+import time
+from pathlib import Path
+from typing import NamedTuple
+
+import numpy as np
+import zstandard as zstd
+
+from cereal import car, messaging
+from openpilot.common.params import Params
+from openpilot.common.realtime import DT_CTRL, Ratekeeper
+from openpilot.common.basedir import BASEDIR
+
+PUB_100HZ = ('carState', 'carControl', 'carOutput', 'controlsState', 'selfdriveState')
+PUB_20HZ = ('modelV2', 'livePose', 'liveParameters')
+SUB = ('alertDebug', 'longitudinalPlan', 'lateralManeuverPlan')
+
+STEER_RATIO = 15.0
+WHEELBASE = 2.78
+
+
+class LongPlan(NamedTuple):
+  aTarget: float
+  shouldStop: bool
+
+
+class LatPlan(NamedTuple):
+  desiredCurvature: float
+
+
+class Plant:
+  """Vehicle model. Subclasses consume the daemon's plan and fill the published messages."""
+
+  sim = None
+  PLAN = 'longitudinalPlan'
+
+  def __init__(self, v_ego: float = 0.0):
+    self.v_ego = v_ego
+    self.a_ego = 0.0
+    self.curvature = 0.0           # commanded, controlsState.desiredCurvature
+    self.achieved_curvature = 0.0  # measured, controlsState.curvature
+    self.lat_accel = 0.0
+    self.long_active = True
+    self.lat_active = True
+
+  def step(self, dt: float, plan) -> None:
+    raise NotImplementedError
+
+  def _angle(self, curvature: float) -> float:
+    return math.degrees(curvature * WHEELBASE * STEER_RATIO)
+
+  def _torque(self, curvature: float) -> float:
+    return float(np.clip(curvature * max(self.v_ego, 1.0) ** 2 / 3.0, -1.0, 1.0))
+
+  def fill_car_state(self, cs) -> None:
+    cs.vEgo = float(self.v_ego)
+    cs.vEgoRaw = float(self.v_ego)
+    cs.vEgoCluster = float(self.v_ego)
+    cs.aEgo = float(self.a_ego)
+    cs.standstill = self.v_ego < 0.01
+    cs.steeringAngleDeg = self._angle(self.achieved_curvature)
+    cs.cruiseState.enabled = True
+    cs.cruiseState.available = True
+    cs.cruiseState.speed = float(max(self.v_ego, 1.0))
+
+  def fill_car_control(self, cc) -> None:
+    cc.enabled = True
+    cc.latActive = self.lat_active
+    cc.longActive = self.long_active
+    cc.orientationNED = [0.0, 0.0, 0.0]
+    cc.actuators.curvature = float(self.curvature)
+    cc.actuators.accel = float(self.a_ego)
+    cc.actuators.steeringAngleDeg = self._angle(self.curvature)
+    cc.actuators.torque = self._torque(self.curvature)
+
+
+class ManeuverSim:
+  def __init__(self, module: str, plant: Plant, fingerprint: str = "TOYOTA_SIENNA",
+               max_maneuvers: int = 0, timeout: float = 600.0, verbose: bool = True):
+    self.module = module
+    self.plant = plant
+    plant.sim = self
+    self.fingerprint = fingerprint
+    self.max_maneuvers = max_maneuvers
+    self.timeout = timeout
+    self.verbose = verbose
+
+    self.events: list[bytes] = []
+    self.alert1 = ''
+    self.alert2 = ''
+    self.seen_maneuvers: list[str] = []
+    self.finished = False
+
+  def _write_car_params(self):
+    CP = car.CarParams.new_message()
+    CP.carFingerprint = self.fingerprint
+    CP.brand = "toyota"
+    CP.openpilotLongitudinalControl = True
+    CP.autoResumeSng = True
+    CP.steerRatio = STEER_RATIO
+    CP.wheelbase = WHEELBASE
+    Params().put("CarParams", CP.to_bytes())
+    return CP
+
+  def _head_events(self, CP):
+    init = messaging.new_message('initData')
+    init.valid = True
+    init.initData.gitCommit = "simulated"
+    init.initData.gitBranch = "sim"
+    init.initData.gitRemote = "iqpilot-sim"
+    self.events.append(init.to_bytes())
+
+    cpm = messaging.new_message('carParams')
+    cpm.valid = True
+    cpm.carParams = CP
+    self.events.append(cpm.to_bytes())
+
+  def _launch(self):
+    env = dict(os.environ)
+    env["PYTHONPATH"] = str(BASEDIR) + os.pathsep + env.get("PYTHONPATH", "")
+    return subprocess.Popen([sys.executable, "-c", f"from {self.module} import main; main()"],
+                            cwd=str(BASEDIR), env=env, start_new_session=True)
+
+  def _on_alert(self, ad):
+    text1, text2 = ad.alertText1, ad.alertText2
+    if (text1, text2) != (self.alert1, self.alert2):
+      if self.verbose:
+        print(f"  [{time.monotonic() - self.t_start:6.1f}s] {text1!r} | {text2!r}")
+      if text2 and text2 not in self.seen_maneuvers:
+        self.seen_maneuvers.append(text2)
+      if text1 == 'Maneuvers Finished':
+        self.finished = True
+    self.alert1, self.alert2 = text1, text2
+
+  def run(self, out: Path) -> Path:
+    self._head_events(self._write_car_params())
+
+    pm = messaging.PubMaster(list(PUB_100HZ) + list(PUB_20HZ))
+    socks = {s: messaging.sub_sock(s, conflate=False, timeout=0) for s in SUB}
+
+    proc = self._launch()
+    self.t_start = time.monotonic()
+    rk = Ratekeeper(int(1.0 / DT_CTRL), print_delay_threshold=None)
+
+    plans: dict[str, object | None] = {'longitudinalPlan': None, 'lateralManeuverPlan': None}
+    frame = 0
+    try:
+      while True:
+        for s, sock in socks.items():
+          while True:
+            raw = sock.receive(non_blocking=True)
+            if raw is None:
+              break
+            self.events.append(raw)
+            evt = messaging.log_from_bytes(raw)
+            if s == 'alertDebug':
+              self._on_alert(evt.alertDebug)
+            elif s == 'longitudinalPlan':
+              plans[s] = LongPlan(evt.longitudinalPlan.aTarget, evt.longitudinalPlan.shouldStop)
+            elif s == 'lateralManeuverPlan':
+              plans[s] = LatPlan(evt.lateralManeuverPlan.desiredCurvature) if evt.valid else None
+
+        self.plant.step(DT_CTRL, plans[self.plant.PLAN])
+
+        for s in PUB_100HZ:
+          raw = self._build(s).to_bytes()
+          self.events.append(raw)
+          pm.send(s, raw)
+
+        if frame % 5 == 0:
+          for s in PUB_20HZ:
+            raw = self._build(s).to_bytes()
+            self.events.append(raw)
+            pm.send(s, raw)
+
+        frame += 1
+        if self.finished:
+          break
+        if self.max_maneuvers and len(self.seen_maneuvers) > self.max_maneuvers:
+          break
+        if time.monotonic() - self.t_start > self.timeout:
+          print("  timed out")
+          break
+        rk.keep_time()
+    finally:
+      if proc.poll() is None:
+        os.killpg(os.getpgid(proc.pid), signal.SIGTERM)
+        proc.wait(timeout=5)
+      for sock in socks.values():
+        del sock
+
+    out.parent.mkdir(parents=True, exist_ok=True)
+    out.write_bytes(zstd.compress(b"".join(self.events), 10))
+    return out
+
+  def _build(self, s: str):
+    msg = messaging.new_message(s)
+    msg.valid = True
+    if s == 'carState':
+      self.plant.fill_car_state(msg.carState)
+    elif s == 'carControl':
+      self.plant.fill_car_control(msg.carControl)
+    elif s == 'carOutput':
+      msg.carOutput.actuatorsOutput.accel = float(self.plant.a_ego)
+      msg.carOutput.actuatorsOutput.curvature = float(self.plant.curvature)
+      msg.carOutput.actuatorsOutput.steeringAngleDeg = self.plant._angle(self.plant.achieved_curvature)
+      msg.carOutput.actuatorsOutput.torque = self.plant._torque(self.plant.achieved_curvature)
+    elif s == 'controlsState':
+      msg.controlsState.curvature = float(self.plant.achieved_curvature)
+      msg.controlsState.desiredCurvature = float(self.plant.curvature)
+    elif s == 'selfdriveState':
+      msg.selfdriveState.enabled = True
+      msg.selfdriveState.active = True
+      msg.selfdriveState.state = 'enabled'
+    elif s == 'modelV2':
+      msg.modelV2.frameId = 0
+      msg.modelV2.action.desiredCurvature = 0.0
+    elif s == 'livePose':
+      msg.livePose.accelerationDevice.x = float(self.plant.a_ego)
+      msg.livePose.accelerationDevice.y = float(self.plant.lat_accel)
+      msg.livePose.velocityDevice.x = float(self.plant.v_ego)
+      msg.livePose.inputsOK = True
+      msg.livePose.posenetOK = True
+      msg.livePose.sensorsOK = True
+    elif s == 'liveParameters':
+      msg.liveParameters.valid = True
+      msg.liveParameters.roll = 0.0
+      msg.liveParameters.steerRatio = STEER_RATIO
+    return msg

tools/longitudinal_maneuvers/sim_maneuvers.py

@@ -0,0 +1,51 @@
+#!/usr/bin/env python3
+"""Run maneuversd against a synthetic longitudinal plant and write an rlog.
+
+    ./tools/longitudinal_maneuvers/sim_maneuvers.py --out /tmp/long_rlog.zst
+    ./tools/longitudinal_maneuvers/generate_report.py /tmp/long_rlog.zst
+"""
+import argparse
+from pathlib import Path
+
+from openpilot.tools.longitudinal_maneuvers.sim_harness import ManeuverSim, Plant
+
+WN = 6.0    # powertrain natural frequency (rad/s)
+ZETA = 0.6  # underdamped, so actual accel overshoots the target like a real car
+
+
+class LongitudinalPlant(Plant):
+  def __init__(self):
+    super().__init__()
+    self.jerk = 0.0
+
+  def step(self, dt, plan):
+    a_target = float(plan.aTarget) if plan is not None else 0.0
+    if plan is not None and plan.shouldStop:
+      a_target = min(a_target, -0.5)
+
+    self.jerk += dt * (WN ** 2 * (a_target - self.a_ego) - 2 * ZETA * WN * self.jerk)
+    self.a_ego += dt * self.jerk
+
+    self.v_ego = max(self.v_ego + self.a_ego * dt, 0.0)
+    if self.v_ego <= 0.0:
+      self.a_ego = min(self.a_ego, 0.0)
+      self.jerk = min(self.jerk, 0.0)
+    self.lat_accel = 0.0
+
+
+def main():
+  parser = argparse.ArgumentParser(description=__doc__)
+  parser.add_argument("--out", type=Path, default=Path("/tmp/longitudinal_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.longitudinal_maneuvers.maneuversd", LongitudinalPlant(),
+                    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()