Live torque (#25456)

* wip torqued

* add basic logic

* setup in manager

* check sanity and publish msg

* add first order filter to outputs

* wire up controlsd, and update gains

* rename intercept to offset

* add cloudlog, live values are not updated

* fix bugs, do not reset points for now

* fix crashes

* rename to main

* fix bugs, works offline

* fix float in cereal bug

* add latacc filter

* randomly choose points, approx for iid

* add variable decay

* local param to capnp instead of dict

* verify works in replay

* use torqued output in controlsd

* use in controlsd; use points from past routes

* controlsd bugfix

* filter before updating gains, needs to be replaced

* save all points to ensure smooth transition across routes, revert friction factor to 1.5

* add filters to prevent noisy low-speed data points; improve fit sanity

* add engaged buffer

* revert lat_acc thresh

* use paramsd realtime process config

* make latacc-to-torque generic, and overrideable

* move freq to 4Hz, avoid storing in np.array, don't publish points in the message

* float instead of np

* remove constant while storing pts

* rename slope, offset to lat_accet_factor, offset

* resolve issues

* use camelcase in all capnp params

* use camelcase everywhere

* reduce latacc threshold or sanity, add car_sane todo, save points properly

* add and check tag

* write param to disk at end of route

* remove args

* rebase op, cereal

* save on exit

* restore default handler

* cpu usage check

* add to process replay

* handle reset better, reduce unnecessary computation

* always publish raw values - useful for debug

* regen routes

* update refs

* checks on cache restore

* check tuning vals too

* clean that up

* reduce cpu usage

* reduce cpu usage by 75%

* cleanup

* optimize further

* handle reset condition better, don't put points in init, use only in corolla

* bump cereal after rebasing

* update refs

* Update common/params.cc

Co-authored-by: Adeeb Shihadeh <adeebshihadeh@gmail.com>

* remove unnecessary checks

* Update RELEASES.md

Co-authored-by: Adeeb Shihadeh <adeebshihadeh@gmail.com>
old-commit-hash: 4fa62f1464

RELEASES.md

@@ -2,6 +2,8 @@ Version 0.8.17 (2022-XX-XX)
 ========================
 * New driving model
   * Internal feature space accuracy increased tenfold during training, this makes the model dramatically more accurate.
+* torqued
+  * Learn torque parameters live for each car as opposed to using platform average values, which improves lateral control
 
 Version 0.8.16 (2022-08-26)
 ========================

common/params.cc

@@ -144,6 +144,8 @@ std::unordered_map<std::string, uint32_t> keys = {
     {"LastUpdateException", CLEAR_ON_MANAGER_START},
     {"LastUpdateTime", PERSISTENT},
     {"LiveParameters", PERSISTENT},
+    {"LiveTorqueCarParams", PERSISTENT},
+    {"LiveTorqueParameters", PERSISTENT | DONT_LOG},
     {"NavDestination", CLEAR_ON_MANAGER_START | CLEAR_ON_IGNITION_OFF},
     {"NavSettingTime24h", PERSISTENT},
     {"NavSettingLeftSide", PERSISTENT},

release/files_common

@@ -240,6 +240,7 @@ selfdrive/locationd/models/live_kf.cc
 selfdrive/locationd/models/constants.py
 selfdrive/locationd/models/gnss_helpers.py
 
+selfdrive/locationd/torqued.py
 selfdrive/locationd/calibrationd.py
 
 system/logcatd/.gitignore

selfdrive/car/interfaces.py

@@ -167,6 +167,7 @@ class CarInterfaceBase(ABC):
     tune.torque.friction = params['FRICTION']
     tune.torque.latAccelFactor = params['LAT_ACCEL_FACTOR']
     tune.torque.latAccelOffset = 0.0
+    tune.torque.steeringAngleDeadzoneDeg = steering_angle_deadzone_deg
 
   @abstractmethod
   def _update(self, c: car.CarControl) -> car.CarState:

selfdrive/controls/controlsd.py

@@ -104,7 +104,7 @@ class Controls:
         ignore += ['roadCameraState']
       self.sm = messaging.SubMaster(['deviceState', 'pandaStates', 'peripheralState', 'modelV2', 'liveCalibration',
                                      'driverMonitoringState', 'longitudinalPlan', 'lateralPlan', 'liveLocationKalman',
-                                     'managerState', 'liveParameters', 'radarState'] + self.camera_packets + joystick_packet,
+                                     'managerState', 'liveParameters', 'radarState', 'liveTorqueParameters'] + self.camera_packets + joystick_packet,
                                     ignore_alive=ignore, ignore_avg_freq=['radarState', 'longitudinalPlan'])
 
     # set alternative experiences from parameters
@@ -578,6 +578,12 @@ class Controls:
     sr = max(params.steerRatio, 0.1)
     self.VM.update_params(x, sr)
 
+    # Update Torque Params
+    if self.CP.lateralTuning.which() == 'torque':
+      torque_params = self.sm['liveTorqueParameters']
+      if self.sm.all_checks(['liveTorqueParameters']) and torque_params.useParams:
+        self.LaC.update_live_torque_params(torque_params.latAccelFactorFiltered, torque_params.latAccelOffsetFiltered, torque_params.frictionCoefficientFiltered)
+
     lat_plan = self.sm['lateralPlan']
     long_plan = self.sm['longitudinalPlan']
 

selfdrive/controls/lib/latcontrol_torque.py

@@ -51,7 +51,7 @@ class LatControlTorque(LatControl):
       desired_lateral_accel = desired_curvature * CS.vEgo ** 2
 
       # desired rate is the desired rate of change in the setpoint, not the absolute desired curvature
-      #desired_lateral_jerk = desired_curvature_rate * CS.vEgo ** 2
+      # desired_lateral_jerk = desired_curvature_rate * CS.vEgo ** 2
       actual_lateral_accel = actual_curvature * CS.vEgo ** 2
       lateral_accel_deadzone = curvature_deadzone * CS.vEgo ** 2
 

selfdrive/locationd/torqued.py

@@ -0,0 +1,290 @@
+#!/usr/bin/env python3
+import os
+import sys
+import signal
+import numpy as np
+from collections import deque, defaultdict
+
+import cereal.messaging as messaging
+from cereal import car, log
+from common.params import Params
+from common.realtime import config_realtime_process, DT_MDL
+from common.filter_simple import FirstOrderFilter
+from system.swaglog import cloudlog
+from selfdrive.controls.lib.vehicle_model import ACCELERATION_DUE_TO_GRAVITY
+from selfdrive.car.toyota.values import CAR as TOYOTA
+
+HISTORY = 5  # secs
+POINTS_PER_BUCKET = 1500
+MIN_POINTS_TOTAL = 4000
+FIT_POINTS_TOTAL = 2000
+MIN_VEL = 15  # m/s
+FRICTION_FACTOR = 1.5  # ~85% of data coverage
+FACTOR_SANITY = 0.3
+FRICTION_SANITY = 0.5
+STEER_MIN_THRESHOLD = 0.02
+MIN_FILTER_DECAY = 50
+MAX_FILTER_DECAY = 250
+LAT_ACC_THRESHOLD = 1
+STEER_BUCKET_BOUNDS = [(-0.5, -0.3), (-0.3, -0.2), (-0.2, -0.1), (-0.1, 0), (0, 0.1), (0.1, 0.2), (0.2, 0.3), (0.3, 0.5)]
+MIN_BUCKET_POINTS = [100, 300, 500, 500, 500, 500, 300, 100]
+MAX_RESETS = 5.0
+MAX_INVALID_THRESHOLD = 10
+MIN_ENGAGE_BUFFER = 2  # secs
+
+VERSION = 1  # bump this to invalidate old parameter caches
+ALLOWED_FINGERPRINTS = [TOYOTA.COROLLA_TSS2, TOYOTA.COROLLA, TOYOTA.COROLLAH_TSS2]
+
+
+def slope2rot(slope):
+  sin = np.sqrt(slope**2 / (slope**2 + 1))
+  cos = np.sqrt(1 / (slope**2 + 1))
+  return np.array([[cos, -sin], [sin, cos]])
+
+
+class npqueue:
+  def __init__(self, maxlen, rowsize):
+    self.maxlen = maxlen
+    self.arr = np.empty((0, rowsize))
+
+  def __len__(self):
+    return len(self.arr)
+
+  def append(self, pt):
+    if len(self.arr) < self.maxlen:
+      self.arr = np.append(self.arr, [pt], axis=0)
+    else:
+      self.arr[:-1] = self.arr[1:]
+      self.arr[-1] = pt
+
+
+class PointBuckets:
+  def __init__(self, x_bounds, min_points):
+    self.x_bounds = x_bounds
+    self.buckets = {bounds: npqueue(maxlen=POINTS_PER_BUCKET, rowsize=3) for bounds in x_bounds}
+    self.buckets_min_points = {bounds: min_point for bounds, min_point in zip(x_bounds, min_points)}
+
+  def bucket_lengths(self):
+    return [len(v) for v in self.buckets.values()]
+
+  def __len__(self):
+    return sum(self.bucket_lengths())
+
+  def is_valid(self):
+    return all(len(v) >= min_pts for v, min_pts in zip(self.buckets.values(), self.buckets_min_points.values())) and (self.__len__() >= MIN_POINTS_TOTAL)
+
+  def add_point(self, x, y):
+    for bound_min, bound_max in self.x_bounds:
+      if (x >= bound_min) and (x < bound_max):
+        self.buckets[(bound_min, bound_max)].append([x, 1.0, y])
+        break
+
+  def get_points(self, num_points=None):
+    points = np.concatenate([x.arr for x in self.buckets.values() if len(x) > 0])
+    if num_points is None:
+      return points
+    return points[np.random.choice(np.arange(len(points)), min(len(points), num_points), replace=False)]
+
+  def load_points(self, points):
+    for x, y in points:
+      self.add_point(x, y)
+
+
+class TorqueEstimator:
+  def __init__(self, CP):
+    self.hist_len = int(HISTORY / DT_MDL)
+    self.lag = CP.steerActuatorDelay + .2   # from controlsd
+
+    self.offline_friction = 0.0
+    self.offline_latAccelFactor = 0.0
+    self.resets = 0.0
+    self.use_params = CP.carFingerprint in ALLOWED_FINGERPRINTS
+
+    if CP.lateralTuning.which() == 'torque':
+      self.offline_friction = CP.lateralTuning.torque.friction
+      self.offline_latAccelFactor = CP.lateralTuning.torque.latAccelFactor
+
+    self.reset()
+
+    initial_params = {
+      'latAccelFactor': self.offline_latAccelFactor,
+      'latAccelOffset': 0.0,
+      'frictionCoefficient': self.offline_friction,
+      'points': []
+    }
+    self.decay = MIN_FILTER_DECAY
+    self.min_lataccel_factor = (1.0 - FACTOR_SANITY) * self.offline_latAccelFactor
+    self.max_lataccel_factor = (1.0 + FACTOR_SANITY) * self.offline_latAccelFactor
+    self.min_friction = (1.0 - FRICTION_SANITY) * self.offline_friction
+    self.max_friction = (1.0 + FRICTION_SANITY) * self.offline_friction
+
+    # try to restore cached params
+    params = Params()
+    params_cache = params.get("LiveTorqueCarParams")
+    torque_cache = params.get("LiveTorqueParameters")
+    if params_cache is not None and torque_cache is not None:
+      try:
+        cache_ltp = log.Event.from_bytes(torque_cache).liveTorqueParameters
+        cache_CP = car.CarParams.from_bytes(params_cache)
+        if self.get_restore_key(cache_CP, cache_ltp.version) == self.get_restore_key(CP, VERSION):
+          initial_params = {
+            'latAccelFactor': cache_ltp.latAccelFactorFiltered,
+            'latAccelOffset': cache_ltp.latAccelOffsetFiltered,
+            'frictionCoefficient': cache_ltp.frictionCoefficientFiltered,
+            'points': cache_ltp.points
+          }
+          self.decay = cache_ltp.decay
+          self.filtered_points.load_points(initial_params['points'])
+          cloudlog.info("restored torque params from cache")
+      except Exception:
+        cloudlog.exception("failed to restore cached torque params")
+        params.remove("LiveTorqueCarParams")
+        params.remove("LiveTorqueParameters")
+
+    self.filtered_params = {}
+    for param in initial_params:
+      self.filtered_params[param] = FirstOrderFilter(initial_params[param], self.decay, DT_MDL)
+
+  def get_restore_key(self, CP, version):
+    a, b = None, None
+    if CP.lateralTuning.which() == 'torque':
+      a = CP.lateralTuning.torque.friction
+      b = CP.lateralTuning.torque.latAccelFactor
+    return (CP.carFingerprint, CP.lateralTuning.which(), a, b, version)
+
+  def reset(self):
+    self.resets += 1.0
+    self.invalid_values_tracker = 0.0
+    self.decay = MIN_FILTER_DECAY
+    self.raw_points = defaultdict(lambda: deque(maxlen=self.hist_len))
+    self.filtered_points = PointBuckets(x_bounds=STEER_BUCKET_BOUNDS, min_points=MIN_BUCKET_POINTS)
+
+  def estimate_params(self):
+    points = self.filtered_points.get_points(FIT_POINTS_TOTAL)
+    # total least square solution as both x and y are noisy observations
+    # this is empirically the slope of the hysteresis parallelogram as opposed to the line through the diagonals
+    try:
+      _, _, v = np.linalg.svd(points, full_matrices=False)
+      slope, offset = -v.T[0:2, 2] / v.T[2, 2]
+      _, spread = np.matmul(points[:, [0, 2]], slope2rot(slope)).T
+      friction_coeff = np.std(spread) * FRICTION_FACTOR
+    except np.linalg.LinAlgError as e:
+      cloudlog.exception(f"Error computing live torque params: {e}")
+      slope = offset = friction_coeff = np.nan
+    return slope, offset, friction_coeff
+
+  def update_params(self, params):
+    self.decay = min(self.decay + DT_MDL, MAX_FILTER_DECAY)
+    for param, value in params.items():
+      self.filtered_params[param].update(value)
+      self.filtered_params[param].update_alpha(self.decay)
+
+  def is_sane(self, latAccelFactor, latAccelOffset, friction):
+    if any([val is None or np.isnan(val) for val in [latAccelFactor, latAccelOffset, friction]]):
+      return False
+    return (self.max_friction >= friction >= self.min_friction) and\
+      (self.max_lataccel_factor >= latAccelFactor >= self.min_lataccel_factor)
+
+  def handle_log(self, t, which, msg):
+    if which == "carControl":
+      self.raw_points["carControl_t"].append(t + self.lag)
+      self.raw_points["steer_torque"].append(-msg.actuatorsOutput.steer)
+      self.raw_points["active"].append(msg.latActive)
+    elif which == "carState":
+      self.raw_points["carState_t"].append(t + self.lag)
+      self.raw_points["vego"].append(msg.vEgo)
+      self.raw_points["steer_override"].append(msg.steeringPressed)
+    elif which == "liveLocationKalman":
+      if len(self.raw_points['steer_torque']) == self.hist_len:
+        yaw_rate = msg.angularVelocityCalibrated.value[2]
+        roll = msg.orientationNED.value[0]
+        active = np.interp(np.arange(t - MIN_ENGAGE_BUFFER, t, DT_MDL), self.raw_points['carControl_t'], self.raw_points['active']).astype(bool)
+        steer_override = np.interp(np.arange(t - MIN_ENGAGE_BUFFER, t, DT_MDL), self.raw_points['carState_t'], self.raw_points['steer_override']).astype(bool)
+        vego = np.interp(t, self.raw_points['carState_t'], self.raw_points['vego'])
+        steer = np.interp(t, self.raw_points['carControl_t'], self.raw_points['steer_torque'])
+        lateral_acc = (vego * yaw_rate) - (np.sin(roll) * ACCELERATION_DUE_TO_GRAVITY)
+        if all(active) and (not any(steer_override)) and (vego > MIN_VEL) and (abs(steer) > STEER_MIN_THRESHOLD) and (abs(lateral_acc) <= LAT_ACC_THRESHOLD):
+          self.filtered_points.add_point(float(steer), float(lateral_acc))
+
+  def get_msg(self, valid=True, with_points=False):
+    msg = messaging.new_message('liveTorqueParameters')
+    msg.valid = valid
+    liveTorqueParameters = msg.liveTorqueParameters
+    liveTorqueParameters.version = VERSION
+    liveTorqueParameters.useParams = self.use_params
+
+    if self.filtered_points.is_valid():
+      latAccelFactor, latAccelOffset, friction_coeff = self.estimate_params()
+      liveTorqueParameters.latAccelFactorRaw = float(latAccelFactor)
+      liveTorqueParameters.latAccelOffsetRaw = float(latAccelOffset)
+      liveTorqueParameters.frictionCoefficientRaw = float(friction_coeff)
+
+      if self.is_sane(latAccelFactor, latAccelOffset, friction_coeff):
+        liveTorqueParameters.liveValid = True
+        self.update_params({'latAccelFactor': latAccelFactor, 'latAccelOffset': latAccelOffset, 'frictionCoefficient': friction_coeff})
+        self.invalid_values_tracker = max(0.0, self.invalid_values_tracker - 0.5)
+      else:
+        cloudlog.exception("live torque params are numerically unstable")
+        liveTorqueParameters.liveValid = False
+        self.invalid_values_tracker += 1.0
+        # Reset when ~10 invalid over 5 secs
+        if self.invalid_values_tracker > MAX_INVALID_THRESHOLD:
+          # Do not reset the filter as it may cause a drastic jump, just reset points
+          self.reset()
+    else:
+      liveTorqueParameters.liveValid = False
+
+    if with_points:
+      liveTorqueParameters.points = self.filtered_points.get_points()[:, [0, 2]].tolist()
+
+    liveTorqueParameters.latAccelFactorFiltered = float(self.filtered_params['latAccelFactor'].x)
+    liveTorqueParameters.latAccelOffsetFiltered = float(self.filtered_params['latAccelOffset'].x)
+    liveTorqueParameters.frictionCoefficientFiltered = float(self.filtered_params['frictionCoefficient'].x)
+    liveTorqueParameters.totalBucketPoints = len(self.filtered_points)
+    liveTorqueParameters.decay = self.decay
+    liveTorqueParameters.maxResets = self.resets
+    return msg
+
+
+def main(sm=None, pm=None):
+  config_realtime_process([0, 1, 2, 3], 5)
+
+  if sm is None:
+    sm = messaging.SubMaster(['carControl', 'carState', 'liveLocationKalman'], poll=['liveLocationKalman'])
+
+  if pm is None:
+    pm = messaging.PubMaster(['liveTorqueParameters'])
+
+  params = Params()
+  CP = car.CarParams.from_bytes(params.get("CarParams", block=True))
+  estimator = TorqueEstimator(CP)
+
+  def cache_params(sig, frame):
+    signal.signal(sig, signal.SIG_DFL)
+    cloudlog.warning("caching torque params")
+
+    params = Params()
+    params.put("LiveTorqueCarParams", CP.as_builder().to_bytes())
+
+    msg = estimator.get_msg(with_points=True)
+    params.put("LiveTorqueParameters", msg.to_bytes())
+
+    sys.exit(0)
+  if "REPLAY" not in os.environ:
+    signal.signal(signal.SIGINT, cache_params)
+
+  while True:
+    sm.update()
+    if sm.all_checks():
+      for which in sm.updated.keys():
+        if sm.updated[which]:
+          t = sm.logMonoTime[which] * 1e-9
+          estimator.handle_log(t, which, sm[which])
+
+    # 4Hz driven by liveLocationKalman
+    if sm.frame % 5 == 0:
+      pm.send('liveTorqueParameters', estimator.get_msg(valid=sm.all_checks()))
+
+
+if __name__ == "__main__":
+  main()

selfdrive/manager/process_config.py

@@ -38,6 +38,7 @@ procs = [
   NativeProcess("locationd", "selfdrive/locationd", ["./locationd"]),
   NativeProcess("boardd", "selfdrive/boardd", ["./boardd"], enabled=False),
   PythonProcess("calibrationd", "selfdrive.locationd.calibrationd"),
+  PythonProcess("torqued", "selfdrive.locationd.torqued"),
   PythonProcess("controlsd", "selfdrive.controls.controlsd"),
   PythonProcess("deleter", "selfdrive.loggerd.deleter", offroad=True),
   PythonProcess("dmonitoringd", "selfdrive.monitoring.dmonitoringd", enabled=(not PC or WEBCAM), callback=driverview),

selfdrive/test/process_replay/process_replay.py

@@ -229,6 +229,15 @@ def calibration_rcv_callback(msg, CP, cfg, fsm):
   return recv_socks, fsm.frame == 0 or msg.which() == 'cameraOdometry'
 
 
+def torqued_rcv_callback(msg, CP, cfg, fsm):
+  # should_recv always true to increment frame
+  recv_socks = []
+  frame = fsm.frame + 1 # incrementing hasn't happened yet in SubMaster
+  if msg.which() == 'liveLocationKalman' and (frame % 5) == 0:
+    recv_socks = ["liveTorqueParameters"]
+  return recv_socks, fsm.frame == 0 or msg.which() == 'liveLocationKalman'
+
+
 def ublox_rcv_callback(msg):
   msg_class, msg_id = msg.ubloxRaw[2:4]
   if (msg_class, msg_id) in {(1, 7 * 16)}:
@@ -251,8 +260,10 @@ CONFIGS = [
     proc_name="controlsd",
     pub_sub={
       "can": ["controlsState", "carState", "carControl", "sendcan", "carEvents", "carParams"],
-      "deviceState": [], "pandaStates": [], "peripheralState": [], "liveCalibration": [], "driverMonitoringState": [], "longitudinalPlan": [], "lateralPlan": [], "liveLocationKalman": [], "liveParameters": [], "radarState": [],
+      "deviceState": [], "pandaStates": [], "peripheralState": [], "liveCalibration": [], "driverMonitoringState": [],
-      "modelV2": [], "driverCameraState": [], "roadCameraState": [], "wideRoadCameraState": [], "managerState": [], "testJoystick": [],
+      "longitudinalPlan": [], "lateralPlan": [], "liveLocationKalman": [], "liveParameters": [], "radarState": [],
+      "modelV2": [], "driverCameraState": [], "roadCameraState": [], "wideRoadCameraState": [], "managerState": [],
+      "testJoystick": [], "liveTorqueParameters": [],
     },
     ignore=["logMonoTime", "valid", "controlsState.startMonoTime", "controlsState.cumLagMs"],
     init_callback=fingerprint,
@@ -374,6 +385,18 @@ CONFIGS = [
     tolerance=NUMPY_TOLERANCE,
     fake_pubsubmaster=True,
   ),
+  ProcessConfig(
+    proc_name="torqued",
+    pub_sub={
+      "liveLocationKalman": ["liveTorqueParameters"],
+      "carState": [], "controlsState": [],
+    },
+    ignore=["logMonoTime"],
+    init_callback=get_car_params,
+    should_recv_callback=torqued_rcv_callback,
+    tolerance=NUMPY_TOLERANCE,
+    fake_pubsubmaster=True,
+  ),
 ]
 
 

selfdrive/test/process_replay/ref_commit

@@ -1 +1 @@
-d14f1a61a4bfde810128a6bb703aa543268fa4a9
+deb07ca8c5dc021e57e81307764a84aa3d7aef32

selfdrive/test/process_replay/regen.py

@@ -269,11 +269,11 @@ def regen_segment(lr, frs=None, outdir=FAKEDATA, disable_tqdm=False):
   return seg_path
 
 
-def regen_and_save(route, sidx, upload=False, use_route_meta=False, outdir=FAKEDATA, disable_tqdm=False):
+def regen_and_save(route, sidx, upload=False, use_route_meta=True, outdir=FAKEDATA, disable_tqdm=False):
   if use_route_meta:
-    r = Route(args.route)
+    r = Route(route)
-    lr = LogReader(r.log_paths()[args.seg])
+    lr = LogReader(r.log_paths()[sidx])
-    fr = FrameReader(r.camera_paths()[args.seg])
+    fr = FrameReader(r.camera_paths()[sidx])
   else:
     lr = LogReader(f"cd:/{route.replace('|', '/')}/{sidx}/rlog.bz2")
     fr = FrameReader(f"cd:/{route.replace('|', '/')}/{sidx}/fcamera.hevc")

selfdrive/test/process_replay/regen_all.py

@@ -3,11 +3,12 @@ import argparse
 import concurrent.futures
 import os
 import random
+import traceback
 from tqdm import tqdm
 
 from selfdrive.test.process_replay.helpers import OpenpilotPrefix
 from selfdrive.test.process_replay.regen import regen_and_save
-from selfdrive.test.process_replay.test_processes import FAKEDATA, original_segments as segments
+from selfdrive.test.process_replay.test_processes import FAKEDATA, source_segments as segments
 from tools.lib.route import SegmentName
 
 
@@ -16,11 +17,15 @@ def regen_job(segment, upload, disable_tqdm):
     sn = SegmentName(segment[1])
     fake_dongle_id = 'regen' + ''.join(random.choice('0123456789ABCDEF') for _ in range(11))
     try:
-      relr = regen_and_save(sn.route_name.canonical_name, sn.segment_num, upload=upload, use_route_meta=False, outdir=os.path.join(FAKEDATA, fake_dongle_id), disable_tqdm=disable_tqdm)
+      relr = regen_and_save(sn.route_name.canonical_name, sn.segment_num, upload=upload, use_route_meta=False,
+                            outdir=os.path.join(FAKEDATA, fake_dongle_id), disable_tqdm=disable_tqdm)
       relr = '|'.join(relr.split('/')[-2:])
       return f'  ("{segment[0]}", "{relr}"), '
     except Exception as e:
-      return f"  {segment} failed: {str(e)}"
+      err = f"  {segment} failed: {str(e)}"
+      err += traceback.format_exc()
+      err += "\n\n"
+      return err
 
 
 if __name__ == "__main__":

selfdrive/test/process_replay/test_processes.py

@@ -15,22 +15,22 @@ from system.version import get_commit
 from tools.lib.filereader import FileReader
 from tools.lib.logreader import LogReader
 
-original_segments = [
+source_segments = [
   ("BODY", "937ccb7243511b65|2022-05-24--16-03-09--1"),        # COMMA.BODY
   ("HYUNDAI", "02c45f73a2e5c6e9|2021-01-01--19-08-22--1"),     # HYUNDAI.SONATA
-  ("HYUNDAI", "d824e27e8c60172c|2022-08-19--17-58-07--2"),     # HYUNDAI.KIA_EV6
+  ("HYUNDAI", "d824e27e8c60172c|2022-09-13--11-26-50--2"),     # HYUNDAI.KIA_EV6
   ("TOYOTA", "0982d79ebb0de295|2021-01-04--17-13-21--13"),     # TOYOTA.PRIUS (INDI)
   ("TOYOTA2", "0982d79ebb0de295|2021-01-03--20-03-36--6"),     # TOYOTA.RAV4  (LQR)
   ("TOYOTA3", "f7d7e3538cda1a2a|2021-08-16--08-55-34--6"),     # TOYOTA.COROLLA_TSS2
   ("HONDA", "eb140f119469d9ab|2021-06-12--10-46-24--27"),      # HONDA.CIVIC (NIDEC)
   ("HONDA2", "7d2244f34d1bbcda|2021-06-25--12-25-37--26"),     # HONDA.ACCORD (BOSCH)
   ("CHRYSLER", "4deb27de11bee626|2021-02-20--11-28-55--8"),    # CHRYSLER.PACIFICA
-  ("RAM", "2f4452b03ccb98f0|2022-07-07--08-01-56--3"),         # CHRYSLER.RAM_1500
+  ("RAM", "2f4452b03ccb98f0|2022-09-07--13-55-08--10"),        # CHRYSLER.RAM_1500
-  ("SUBARU", "4d70bc5e608678be|2021-01-15--17-02-04--5"),      # SUBARU.IMPREZA
+  ("SUBARU", "341dccd5359e3c97|2022-09-12--10-35-33--3"),      # SUBARU.OUTBACK
   ("GM", "0c58b6a25109da2b|2021-02-23--16-35-50--11"),         # GM.VOLT
   ("NISSAN", "35336926920f3571|2021-02-12--18-38-48--46"),     # NISSAN.XTRAIL
   ("VOLKSWAGEN", "de9592456ad7d144|2021-06-29--11-00-15--6"),  # VOLKSWAGEN.GOLF
-  ("MAZDA", "bd6a637565e91581|2021-10-30--15-14-53--2"),       # MAZDA.CX9_2021
+  ("MAZDA", "bd6a637565e91581|2021-10-30--15-14-53--4"),       # MAZDA.CX9_2021
 
   # Enable when port is tested and dashcamOnly is no longer set
   #("TESLA", "bb50caf5f0945ab1|2021-06-19--17-20-18--3"),      # TESLA.AP2_MODELS

selfdrive/test/test_onroad.py

@@ -36,6 +36,7 @@ PROCS = {
   "./_dmonitoringmodeld": 5.0,
   "selfdrive.thermald.thermald": 3.87,
   "selfdrive.locationd.calibrationd": 2.0,
+  "selfdrive.locationd.torqued": 5.0,
   "./_soundd": 1.0,
   "selfdrive.monitoring.dmonitoringd": 4.0,
   "./proclogd": 1.54,

selfdrive/test/update_ci_routes.py

@@ -4,7 +4,7 @@ import subprocess
 from azure.storage.blob import BlockBlobService  # pylint: disable=import-error
 
 from selfdrive.car.tests.routes import routes as test_car_models_routes
-from selfdrive.test.process_replay.test_processes import original_segments as replay_segments
+from selfdrive.test.process_replay.test_processes import source_segments as replay_segments
 from xx.chffr.lib import azureutil  # pylint: disable=import-error
 from xx.chffr.lib.storage import _DATA_ACCOUNT_PRODUCTION, _DATA_ACCOUNT_CI, _DATA_BUCKET_PRODUCTION  # pylint: disable=import-error