Localize steering command publication stalls

openpilot/selfdrive/controls/controlsd.py

@@ -12,7 +12,7 @@ from openpilot.common.constants import CV
 from openpilot.common.hardware import HARDWARE
 from openpilot.common.params import Params
 from openpilot.common.realtime import config_realtime_process, DT_CTRL, Priority, Ratekeeper, set_core_affinity
-from openpilot.selfdrive.controls.lib.cycle_timing import ControlCycleTiming
+from openpilot.selfdrive.controls.lib.cycle_timing import ControlCycleTiming, PUBLISH_PHASES
 from openpilot.common.swaglog import cloudlog
 
 from opendbc.car.car_helpers import interfaces
@@ -511,6 +511,9 @@ class Controls:
     return CC, lac_log
 
   def publish(self, CC, lac_log):
+    publish_timing = getattr(self, 'publish_cycle_timing', None)
+    if publish_timing is not None:
+      publish_timing.start()
     CS = self.sm['carState']
 
     # Orientation and angle rates can be useful for carcontroller
@@ -586,13 +589,19 @@ class Controls:
     elif lat_tuning == 'torque':
       cs.lateralControlState.torqueState = lac_log
 
+    if publish_timing is not None:
+      publish_timing.mark()
     self.pm.send('controlsState', dat)
+    if publish_timing is not None:
+      publish_timing.mark()
 
     # carControl
     cc_send = messaging.new_message('carControl')
     cc_send.valid = CS.canValid
     cc_send.carControl = CC
     self.pm.send('carControl', cc_send)
+    if publish_timing is not None:
+      publish_timing.mark()
     feedback = feedback_message(
       self.last_lane_centering_result, self.lane_centering_applied, CC.latActive,
       model_mono_time=self.sm.logMonoTime['modelV2'], car_control_mono_time=cc_send.logMonoTime,
@@ -625,15 +634,20 @@ class Controls:
         self.learning_feedback_prior_publish_ns = 0
     else:
       self.learning_feedback_prior_publish_ns = 0
+    if publish_timing is not None:
+      publish_timing.mark()
     self.pm.send('starpilotLateralState', feedback)
     diagnostic_observer = getattr(self, 'aol_transition_diagnostic', None)
     if diagnostic_observer is not None:
       diagnostic_observer.emit(cloudlog.event, cc_send)
+    if publish_timing is not None:
+      publish_timing.mark()
 
   def run(self):
     rk = Ratekeeper(100, print_delay_threshold=None)
     next_overrun_log_ns = 0
     self.control_cycle_timing = ControlCycleTiming()
+    self.publish_cycle_timing = ControlCycleTiming(phases=PUBLISH_PHASES)
     while True:
       started, cpu_started = time.monotonic_ns(), time.thread_time_ns()
       self.update()
@@ -649,7 +663,9 @@ class Controls:
                        control_ms=(controlled - updated) / 1e6, publish_ms=(published - controlled) / 1e6,
                        wall_ms=(published - started) / 1e6, cpu_ms=(time.thread_time_ns() - cpu_started) / 1e6,
                        control_phases=self.control_cycle_timing.snapshot(),
-                       control_phases_complete=self.control_cycle_timing.complete, error=True)
+                       control_phases_complete=self.control_cycle_timing.complete,
+                       publish_phases=self.publish_cycle_timing.snapshot(),
+                       publish_phases_complete=self.publish_cycle_timing.complete, error=True)
       rk.monitor_time()
 
 

openpilot/selfdrive/controls/lib/cycle_timing.py

@@ -4,13 +4,16 @@ import time
 
 PHASES = ('authority_and_preferences', 'longitudinal', 'lateral_planning', 'learning', 'lateral_controller_and_finish')
 
+PUBLISH_PHASES = ('prepare', 'controls_state_send', 'car_control_send', 'optional_feedback', 'feedback_send_and_diagnostic')
+
 
 class ControlCycleTiming:
   def __init__(self, *, wall_clock: Callable[[], int] = time.perf_counter_ns,
-               cpu_clock: Callable[[], int] = time.thread_time_ns):
+               cpu_clock: Callable[[], int] = time.thread_time_ns, phases: tuple[str, ...] = PHASES):
+    self.phases = phases
     self.wall_clock, self.cpu_clock = wall_clock, cpu_clock
-    self.wall = [0] * len(PHASES)
-    self.cpu = [0] * len(PHASES)
+    self.wall = [0] * len(self.phases)
+    self.cpu = [0] * len(self.phases)
     self.count = 0
     self.last_wall = self.last_cpu = 0
     self.overflow = False
@@ -21,7 +24,7 @@ class ControlCycleTiming:
     self.last_wall, self.last_cpu = self.wall_clock(), self.cpu_clock()
 
   def mark(self) -> None:
-    if self.count >= len(PHASES):
+    if self.count >= len(self.phases):
       self.overflow = True
       return
     wall, cpu = self.wall_clock(), self.cpu_clock()
@@ -31,8 +34,8 @@ class ControlCycleTiming:
 
   def snapshot(self) -> dict[str, dict[str, float]]:
     return {name: {'wall_ms': self.wall[i] / 1e6, 'cpu_ms': self.cpu[i] / 1e6}
-            for i, name in enumerate(PHASES[:self.count])}
+            for i, name in enumerate(self.phases[:self.count])}
 
   @property
   def complete(self) -> bool:
-    return self.count == len(PHASES) and not self.overflow
+    return self.count == len(self.phases) and not self.overflow

openpilot/selfdrive/controls/tests/test_publish_cycle_timing.py

@@ -0,0 +1,78 @@
+"""Publishing stalls must be localized without changing published commands."""
+from types import SimpleNamespace as NS
+from unittest.mock import Mock, patch
+
+import pytest
+from opendbc.car import structs
+from openpilot.cereal import log
+from openpilot.selfdrive.controls import controlsd
+from openpilot.selfdrive.controls.lib import cycle_timing
+
+
+class Messages(dict):
+  logMonoTime = {'modelV2': 10, 'carState': 11, 'longitudinalPlan': 12}
+  valid = {'driverAssistance': False}
+
+  def all_checks(self, services):
+    return True
+
+
+def run_publish(*, measured, blocked=''):
+  owner = Mock(spec=controlsd.Controls)
+  owner.CP = structs.CarParams(steerControlType='torque', openpilotLongitudinalControl=False, pcmCruise=True)
+  owner.CP.lateralTuning.init('torque')
+  cs = structs.CarState(canValid=True, vEgo=17., vCruiseCluster=72.)
+  owner.sm = Messages(carState=cs, carOutput=controlsd.messaging.new_message('carOutput').carOutput,
+                      selfdriveState=NS(personality=NS(raw=1), state=controlsd.State.disabled, alertHudVisual='none'),
+                      longitudinalPlan=NS(shouldStop=False, hasLead=True),
+                      driverMonitoringState=NS(noResponseForceDecel=False))
+  owner.curvature = owner.desired_curvature = .001
+  owner.calibrated_pose = None
+  owner.LoC = NS(extension=None, long_control_state='off', pid=NS(p=0., i=0., f=0.))
+  owner.longitudinal_inputs = NS(lead_visible=lambda value: value)
+  owner.steer_limited_by_safety = False
+  owner.last_lane_centering_result = None
+  owner.lane_centering_applied = 0.
+  clocks = {'wall': 0, 'cpu': 0}
+  if measured:
+    kwargs = {'phases': cycle_timing.PUBLISH_PHASES} if hasattr(cycle_timing, 'PUBLISH_PHASES') else {}
+    owner.publish_cycle_timing = cycle_timing.ControlCycleTiming(
+      wall_clock=lambda: clocks['wall'], cpu_clock=lambda: clocks['cpu'], **kwargs)
+  output = []
+
+  def send(service, event):
+    output.append((service, event.to_dict()))
+    if service == blocked:
+      clocks['wall'] += 60_000_000
+      clocks['cpu'] += 1_000_000
+
+  owner.pm = NS(send=send)
+  original = controlsd.messaging.new_message
+  def message(service, **kwargs):
+    return original(service, logMonoTime=100, **kwargs)
+  command = structs.CarControl(latActive=True)
+  command.actuators.torque = .2
+  lateral_log = log.ControlsState.LateralTorqueState(active=True, output=.2, desiredLateralAccel=.1, actualLateralAccel=.1)
+  with patch.object(controlsd.messaging, 'new_message', side_effect=message):
+    controlsd.Controls.publish(owner, command, lateral_log)
+  return owner, output
+
+
+@pytest.mark.parametrize('service,phase', [('controlsState', 'controls_state_send'), ('carControl', 'car_control_send'),
+                                          ('starpilotLateralState', 'feedback_send_and_diagnostic')])
+def test_actual_publish_identifies_blocking_send_without_zeroing_torque(service, phase):
+  owner, messages = run_publish(measured=True, blocked=service)
+  report = owner.publish_cycle_timing.snapshot()
+  assert phase in report, 'PUBLISH_STALL_NOT_LOCALIZED'
+  assert owner.publish_cycle_timing.complete
+  assert report[phase] == {'wall_ms': 60., 'cpu_ms': 1.}
+  assert sum(row['wall_ms'] for row in report.values()) == 60.
+  command = next(data['carControl'] for name, data in messages if name == 'carControl')
+  assert command['latActive'] and command['actuators']['torque'] == pytest.approx(.2)
+
+
+def test_timing_preserves_all_actual_published_fields_and_order():
+  _, before = run_publish(measured=False)
+  _, after = run_publish(measured=True)
+  assert after == before
+  assert [name for name, _ in after] == ['controlsState', 'carControl', 'starpilotLateralState']

prebuilt.json

@@ -1 +1 @@
-{"protected_inventory_sha256": "686aaeb643ad9c2302bfcd57ee730d1de60a10034b638374b5d5560c98c23687", "schema_version": 1}
+{"protected_inventory_sha256": "a6f314fca4f5b54607ebf554388a182cd5afd1afeae910a917dc243024c46eb2", "schema_version": 1}