IQ.Pilot Release Commit @ b6534c0

iqpilot/tools/maneuvers/sim_harness.py

@@ -0,0 +1,244 @@
+#!/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 the report generators 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 iqpilot.cereal import car, messaging
+from iqpilot.common.params import Params
+from iqpilot.common.realtime import DT_CTRL, Ratekeeper
+from iqpilot.common.basedir import BASEDIR
+
+PUB_100HZ = ('carState', 'carControl', 'carOutput', 'controlsState', 'selfdriveState')
+PUB_20HZ = ('modelV2', 'deviceMotion', 'vehicleParameters')
+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
+    self.steering_pressed = False
+    self.gas_pressed = False
+
+  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.steeringPressed = self.steering_pressed
+    cs.gasPressed = self.gas_pressed
+    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 == 'deviceMotion':
+      msg.deviceMotion.accelerationDevice.x = float(self.plant.a_ego)
+      msg.deviceMotion.accelerationDevice.y = float(self.plant.lat_accel)
+      msg.deviceMotion.velocityDevice.x = float(self.plant.v_ego)
+      msg.deviceMotion.inputsOK = True
+      msg.deviceMotion.posenetOK = True
+      msg.deviceMotion.sensorsOK = True
+    elif s == 'vehicleParameters':
+      msg.vehicleParameters.valid = True
+      msg.vehicleParameters.roll = 0.0
+      msg.vehicleParameters.steerRatio = STEER_RATIO
+    return msg