Merge paramsd, torque, and lagd into estimatorsd

openpilot/selfdrive/locationd/estimatord.py

@@ -0,0 +1,94 @@
+#!/usr/bin/env python3
+import os
+import numpy as np
+
+import openpilot.cereal.messaging as messaging
+from openpilot.cereal.services import SERVICE_LIST
+from opendbc.car.structs import car
+from openpilot.common.params import Params
+from openpilot.common.realtime import config_realtime_process
+from openpilot.selfdrive.locationd.lagd import LateralLagEstimator, retrieve_initial_lag
+from openpilot.selfdrive.locationd.paramsd import VehicleParamsEstimator, retrieve_initial_vehicle_params
+from openpilot.selfdrive.locationd.torqued import TorqueEstimator
+
+
+PARAMS_SERVICES = ['deviceMotion', 'extrinsicsCalibration', 'carState']
+LAG_SERVICES = ['deviceMotion', 'extrinsicsCalibration', 'carState', 'controlsState', 'carControl']
+TORQUE_SERVICES = ['carControl', 'carOutput', 'carState', 'extrinsicsCalibration', 'deviceMotion']
+SUBSCRIBED_SERVICES = list(dict.fromkeys(PARAMS_SERVICES + LAG_SERVICES + TORQUE_SERVICES))
+
+
+def main() -> None:
+  config_realtime_process([0, 1, 2, 3], 5)
+
+  debug = bool(int(os.getenv('DEBUG', '0')))
+  replay = bool(int(os.getenv('REPLAY', '0')))
+
+  pm = messaging.PubMaster(['vehicleParameters', 'lateralDelay', 'lateralTorqueParameters'])
+  sm = messaging.SubMaster(SUBSCRIBED_SERVICES, poll='deviceMotion')
+
+  params = Params()
+  CP = messaging.log_from_bytes(params.get('CarParams', block=True), car.CarParams)
+
+  steer_ratio, stiffness_factor, angle_offset_deg, p_initial = retrieve_initial_vehicle_params(params, CP, replay, debug)
+  params_estimator = VehicleParamsEstimator(CP, steer_ratio, stiffness_factor, np.radians(angle_offset_deg), p_initial)
+
+  lag_estimator = LateralLagEstimator(CP, 1. / SERVICE_LIST['deviceMotion'].frequency)
+  if (initial_lag_params := retrieve_initial_lag(params, CP)) is not None:
+    lag, valid_blocks = initial_lag_params
+    lag_estimator.reset(lag, valid_blocks)
+
+  torque_estimator = TorqueEstimator(CP)
+  estimators = (
+    (params_estimator, PARAMS_SERVICES),
+    (lag_estimator, LAG_SERVICES),
+    (torque_estimator, TORQUE_SERVICES),
+  )
+
+  while True:
+    sm.update()
+    valid = sm.all_checks()
+
+    if valid:
+      for which in sorted(sm.updated, key=lambda x: sm.logMonoTime[x]):
+        if not sm.updated[which]:
+          continue
+
+        t = sm.logMonoTime[which] * 1e-9
+        for estimator, services in estimators:
+          if which in services:
+            estimator.handle_log(t, which, sm[which])
+      lag_estimator.update_points()
+
+    if not sm.updated['deviceMotion']:
+      continue
+
+    params_msg = params_estimator.get_msg(valid, debug=debug)
+    params_msg_dat = params_msg.to_bytes()
+    if sm.frame % 1200 == 0:  # once a minute
+      params.put('LiveParametersV2', params_msg_dat)
+    pm.send('vehicleParameters', params_msg_dat)
+
+    # The remaining estimators publish at 4 Hz, driven by deviceMotion.
+    if sm.frame % 5 != 0:
+      continue
+
+    lag_estimator.update_estimate()
+    lag_msg = lag_estimator.get_msg(valid, debug)
+    lag_msg_dat = lag_msg.to_bytes()
+    pm.send('lateralDelay', lag_msg_dat)
+
+    # Feed the new lag directly rather than subscribing to our own publication.
+    torque_estimator.handle_log(sm.logMonoTime['deviceMotion'] * 1e-9, 'lateralDelay', lag_msg.lateralDelay)
+    pm.send('lateralTorqueParameters', torque_estimator.get_msg(valid=valid, with_points=debug))
+
+    if sm.frame % 1200 == 0:  # once a minute
+      params.put('LiveDelay', lag_msg_dat)
+
+    if sm.frame % 240 == 0:  # preserve torqued's cache cadence
+      torque_msg = torque_estimator.get_msg(valid=valid, with_points=True)
+      params.put('LiveTorqueParameters', torque_msg.to_bytes())
+
+
+if __name__ == '__main__':
+  main()

openpilot/selfdrive/locationd/lagd.py

@@ -1,5 +1,4 @@
 #!/usr/bin/env python3
-import os
 import numpy as np
 import capnp
 from collections import deque
@@ -8,10 +7,8 @@
 import openpilot.cereal.messaging as messaging
 from openpilot.cereal import log
 from opendbc.car.structs import car
-from openpilot.cereal.services import SERVICE_LIST
 from openpilot.common.constants import CV
 from openpilot.common.params import Params
-from openpilot.common.realtime import config_realtime_process
 from openpilot.common.swaglog import cloudlog
 from openpilot.selfdrive.locationd.helpers import PoseCalibrator, Pose, fft_next_good_size, parabolic_peak_interp
 
@@ -382,39 +379,3 @@ def retrieve_initial_lag(params: Params, CP: car.CarParams):
       params.remove("LiveDelay")
 
   return None
-
-
-def main():
-  config_realtime_process([0, 1, 2, 3], 5)
-
-  DEBUG = bool(int(os.getenv("DEBUG", "0")))
-
-  pm = messaging.PubMaster(['lateralDelay'])
-  sm = messaging.SubMaster(['deviceMotion', 'extrinsicsCalibration', 'carState', 'controlsState', 'carControl'], poll='deviceMotion')
-
-  params = Params()
-  CP = messaging.log_from_bytes(params.get("CarParams", block=True), car.CarParams)
-
-  lag_learner = LateralLagEstimator(CP, 1. / SERVICE_LIST['deviceMotion'].frequency)
-  if (initial_lag_params := retrieve_initial_lag(params, CP)) is not None:
-    lag, valid_blocks = initial_lag_params
-    lag_learner.reset(lag, valid_blocks)
-
-  while True:
-    sm.update()
-    if sm.all_checks():
-      for which in sorted(sm.updated.keys(), key=lambda x: sm.logMonoTime[x]):
-        if sm.updated[which]:
-          t = sm.logMonoTime[which] * 1e-9
-          lag_learner.handle_log(t, which, sm[which])
-      lag_learner.update_points()
-
-    # 4Hz driven by deviceMotion
-    if sm.frame % 5 == 0:
-      lag_learner.update_estimate()
-      lag_msg = lag_learner.get_msg(sm.all_checks(), DEBUG)
-      lag_msg_dat = lag_msg.to_bytes()
-      pm.send('lateralDelay', lag_msg_dat)
-
-      if sm.frame % 1200 == 0: # cache every 60 seconds
-        params.put("LiveDelay", lag_msg_dat)

openpilot/selfdrive/locationd/paramsd.py

@@ -1,13 +1,12 @@
 #!/usr/bin/env python3
-import os
 import numpy as np
 import capnp
 
 import openpilot.cereal.messaging as messaging
 from openpilot.cereal import log
 from opendbc.car.structs import car
 from openpilot.common.params import Params
-from openpilot.common.realtime import config_realtime_process, DT_MDL
+from openpilot.common.realtime import DT_MDL
 from openpilot.selfdrive.locationd.models.car_kf import CarKalman, ObservationKind, States
 from openpilot.selfdrive.locationd.models.constants import GENERATED_DIR
 from openpilot.selfdrive.locationd.helpers import PoseCalibrator, Pose
@@ -25,7 +24,7 @@
 LOW_ACTIVE_SPEED = 10.0
 
 
-class VehicleParamsLearner:
+class VehicleParamsEstimator:
   def __init__(self, CP: car.CarParams, steer_ratio: float, stiffness_factor: float, angle_offset: float, P_initial: np.ndarray | None = None):
     self.kf = CarKalman(GENERATED_DIR)
 
@@ -237,43 +236,6 @@ def retrieve_initial_vehicle_params(params: Params, CP: car.CarParams, replay: b
     stiffness_factor = 1.0
 
   if not retrieve_success:
-    cloudlog.info("Parameter learner resetting to default values")
+    cloudlog.info("Vehicle parameter estimator resetting to default values")
 
   return steer_ratio, stiffness_factor, angle_offset_deg, p_initial
-
-
-def main():
-  config_realtime_process([0, 1, 2, 3], 5)
-
-  DEBUG = bool(int(os.getenv("DEBUG", "0")))
-  REPLAY = bool(int(os.getenv("REPLAY", "0")))
-
-  pm = messaging.PubMaster(['vehicleParameters'])
-  sm = messaging.SubMaster(['deviceMotion', 'extrinsicsCalibration', 'carState'], poll='deviceMotion')
-
-  params = Params()
-  CP = messaging.log_from_bytes(params.get("CarParams", block=True), car.CarParams)
-
-  steer_ratio, stiffness_factor, angle_offset_deg, pInitial = retrieve_initial_vehicle_params(params, CP, REPLAY, DEBUG)
-  learner = VehicleParamsLearner(CP, steer_ratio, stiffness_factor, np.radians(angle_offset_deg), pInitial)
-
-  while True:
-    sm.update()
-    if sm.all_checks():
-      for which in sorted(sm.updated.keys(), key=lambda x: sm.logMonoTime[x]):
-        if sm.updated[which]:
-          t = sm.logMonoTime[which] * 1e-9
-          learner.handle_log(t, which, sm[which])
-
-    if sm.updated['deviceMotion']:
-      msg = learner.get_msg(sm.all_checks(), debug=DEBUG)
-
-      msg_dat = msg.to_bytes()
-      if sm.frame % 1200 == 0:  # once a minute
-        params.put("LiveParametersV2", msg_dat)
-
-      pm.send('vehicleParameters', msg_dat)
-
-
-if __name__ == "__main__":
-  main()

openpilot/selfdrive/locationd/torqued.py

@@ -1,5 +1,4 @@
 #!/usr/bin/env python3
-import os
 import numpy as np
 from collections import deque, defaultdict
 
@@ -8,7 +7,7 @@
 from opendbc.car.structs import car
 from openpilot.common.constants import ACCELERATION_DUE_TO_GRAVITY
 from openpilot.common.params import Params
-from openpilot.common.realtime import config_realtime_process, DT_MDL
+from openpilot.common.realtime import DT_MDL
 from openpilot.common.filter_simple import FirstOrderFilter
 from openpilot.common.swaglog import cloudlog
 from openpilot.selfdrive.locationd.helpers import PointBuckets, ParameterEstimator, PoseCalibrator, Pose
@@ -242,41 +241,3 @@ def get_msg(self, valid=True, with_points=False):
     lateralTorqueParameters.decay = self.decay
     lateralTorqueParameters.maxResets = self.resets
     return msg
-
-
-def main(demo=False):
-  config_realtime_process([0, 1, 2, 3], 5)
-
-  DEBUG = bool(int(os.getenv("DEBUG", "0")))
-
-  pm = messaging.PubMaster(['lateralTorqueParameters'])
-  sm = messaging.SubMaster(['carControl', 'carOutput', 'carState', 'extrinsicsCalibration', 'deviceMotion', 'lateralDelay'], poll='deviceMotion')
-
-  params = Params()
-  estimator = TorqueEstimator(messaging.log_from_bytes(params.get("CarParams", block=True), car.CarParams))
-
-  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 deviceMotion
-    if sm.frame % 5 == 0:
-      pm.send('lateralTorqueParameters', estimator.get_msg(valid=sm.all_checks(), with_points=DEBUG))
-
-    # Cache points every 60 seconds while onroad
-    if sm.frame % 240 == 0:
-      msg = estimator.get_msg(valid=sm.all_checks(), with_points=True)
-      params.put("LiveTorqueParameters", msg.to_bytes())
-
-
-if __name__ == "__main__":
-  import argparse
-
-  parser = argparse.ArgumentParser(description='Process the --demo argument.')
-  parser.add_argument('--demo', action='store_true', help='A boolean for demo mode.')
-  args = parser.parse_args()
-  main(demo=args.demo)

openpilot/selfdrive/test/process_replay/README.md

@@ -15,9 +15,7 @@ Currently the following processes are tested:
 * calibrationd
 * dmonitoringd
 * locationd
-* paramsd
 * ubloxd
-* torqued
 
 ### Usage
 ```
@@ -79,9 +77,7 @@ Supported processes:
 * calibrationd
 * dmonitoringd
 * locationd
-* paramsd
 * ubloxd
-* torqued
 * modeld
 * dmonitoringmodeld
 

openpilot/selfdrive/test/process_replay/process_replay.py

@@ -513,40 +513,12 @@ def selfdrived_config_callback(params, cfg, lr):
     tolerance=NUMPY_TOLERANCE,
     processing_time=0.01,
   ),
-  ProcessConfig(
-    proc_name="paramsd",
-    pubs=["deviceMotion", "extrinsicsCalibration", "carState"],
-    subs=["vehicleParameters"],
-    ignore=["logMonoTime"],
-    init_callback=get_car_params_callback,
-    should_recv_callback=MessageBasedRcvCallback("deviceMotion"),
-    tolerance=NUMPY_TOLERANCE,
-    processing_time=0.004,
-  ),
-  ProcessConfig(
-    proc_name="lagd",
-    pubs=["deviceMotion", "extrinsicsCalibration", "carState", "carControl", "controlsState"],
-    subs=["lateralDelay"],
-    ignore=["logMonoTime"],
-    init_callback=get_car_params_callback,
-    should_recv_callback=MessageBasedRcvCallback("deviceMotion"),
-    tolerance=NUMPY_TOLERANCE,
-  ),
   ProcessConfig(
     proc_name="ubloxd",
     pubs=["ubloxRaw"],
     subs=["ubloxGnss", "gpsLocationExternal"],
     ignore=["logMonoTime"],
   ),
-  ProcessConfig(
-    proc_name="torqued",
-    pubs=["deviceMotion", "extrinsicsCalibration", "lateralDelay", "carState", "carControl", "carOutput"],
-    subs=["lateralTorqueParameters"],
-    ignore=["logMonoTime"],
-    init_callback=get_car_params_callback,
-    should_recv_callback=MessageBasedRcvCallback("deviceMotion", True),
-    tolerance=NUMPY_TOLERANCE,
-  ),
   ProcessConfig(
     proc_name="modeld",
     pubs=["deviceState", "narrowRoadCameraState", "wideRoadCameraState", "extrinsicsCalibration", "lateralDelay",
@@ -586,7 +558,7 @@ def get_process_config(name: str) -> ProcessConfig:
 def get_custom_params_from_lr(lr: LogIterable, initial_state: str = "first") -> dict[str, Any]:
   """
   Use this to get custom params dict based on provided logs.
-  Useful when replaying following processes: calibrationd, paramsd, torqued
+  Useful when replaying calibrationd.
   The params may be based on first or last message of given type (carParams, extrinsicsCalibration, vehicleParameters, lateralTorqueParameters) in the logs.
   """
 

openpilot/selfdrive/test/process_replay/test_fuzzy.py

@@ -10,7 +10,7 @@
 # These processes currently fail because of unrealistic data breaking assumptions
 # that openpilot makes causing error with NaN, inf, int size, array indexing ...
 # TODO: Make each one testable
-NOT_TESTED = ['selfdrived', 'controlsd', 'card', 'plannerd', 'calibrationd', 'dmonitoringd', 'paramsd', 'dmonitoringmodeld', 'modeld']
+NOT_TESTED = ['selfdrived', 'controlsd', 'card', 'plannerd', 'calibrationd', 'dmonitoringd', 'estimatord', 'dmonitoringmodeld', 'modeld']
 
 TEST_CASES = [(cfg.proc_name, copy.deepcopy(cfg)) for cfg in pr.CONFIGS if cfg.proc_name not in NOT_TESTED]
 

openpilot/selfdrive/test/process_replay/test_processes.py

@@ -188,7 +188,7 @@ def test_process(cfg, lr, segment, ref_log_msgs, new_log_path, ignore_fields=Non
           continue
 
         # to speed things up, we only test all segments on card
-        if cfg.proc_name not in ('card', 'controlsd', 'lagd') and car_brand not in ('HYUNDAI', 'TOYOTA'):
+        if cfg.proc_name not in ('card', 'controlsd') and car_brand not in ('HYUNDAI', 'TOYOTA'):
           continue
 
         cur_log_fn = os.path.join(FAKEDATA, f"{segment}_{cfg.proc_name}_{cur_commit}.zst".replace("|", "_"))

openpilot/selfdrive/test/test_onroad.py

@@ -52,10 +52,8 @@
   "openpilot.selfdrive.modeld.dmonitoringmodeld": 18.0,
   "openpilot.system.hardware.hardwared": 4.0,
   "openpilot.selfdrive.locationd.calibrationd": 2.0,
-  "openpilot.selfdrive.locationd.torqued": 5.0,
   "openpilot.selfdrive.locationd.locationd": 25.0,
-  "openpilot.selfdrive.locationd.paramsd": 9.0,
-  "openpilot.selfdrive.locationd.lagd": 11.0,
+  "openpilot.selfdrive.locationd.estimatord": 22.0,
   "openpilot.selfdrive.ui.soundd": 3.0,
   "openpilot.selfdrive.monitoring.dmonitoringd": 4.0,
   "openpilot.system.proclogd": 7.0,

openpilot/system/manager/process_config.py

@@ -94,7 +94,6 @@ def not_(*fns):
   PythonProcess("locationd", "openpilot.selfdrive.locationd.locationd", only_onroad),
   NativeProcess("_pandad", "openpilot/selfdrive/pandad", ["./pandad"], always_run, enabled=False),
   PythonProcess("calibrationd", "openpilot.selfdrive.locationd.calibrationd", only_onroad),
-  PythonProcess("torqued", "openpilot.selfdrive.locationd.torqued", only_onroad),
   PythonProcess("controlsd", "openpilot.selfdrive.controls.controlsd", and_(not_joystick, iscar)),
   PythonProcess("joystickd", "openpilot.tools.joystick.joystickd", or_(joystick, notcar)),
   PythonProcess("selfdrived", "openpilot.selfdrive.selfdrived.selfdrived", only_onroad),
@@ -103,8 +102,7 @@ def not_(*fns):
   PythonProcess("dmonitoringd", "openpilot.selfdrive.monitoring.dmonitoringd", driverview, enabled=(WEBCAM or not PC)),
   PythonProcess("qcomgpsd", "openpilot.system.qcomgpsd.qcomgpsd", qcomgps, enabled=COMMA_HARDWARE),
   PythonProcess("pandad", "openpilot.selfdrive.pandad.pandad", always_run),
-  PythonProcess("paramsd", "openpilot.selfdrive.locationd.paramsd", only_onroad),
-  PythonProcess("lagd", "openpilot.selfdrive.locationd.lagd", only_onroad),
+  PythonProcess("estimatord", "openpilot.selfdrive.locationd.estimatord", only_onroad),
   PythonProcess("ubloxd", "openpilot.system.ubloxd.ubloxd", ublox, enabled=COMMA_HARDWARE),
   PythonProcess("pigeond", "openpilot.system.ubloxd.pigeond", ublox, enabled=COMMA_HARDWARE),
   PythonProcess("plannerd", "openpilot.selfdrive.controls.plannerd", not_long_maneuver),