diff --git a/openpilot/selfdrive/controls/controlsd.py b/openpilot/selfdrive/controls/controlsd.py old mode 100755 new mode 100644 index 2e41495ade..fd103c6f2e --- a/openpilot/selfdrive/controls/controlsd.py +++ b/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() diff --git a/openpilot/selfdrive/controls/lib/cycle_timing.py b/openpilot/selfdrive/controls/lib/cycle_timing.py index aeb06a1c38..87bf49ec29 100644 --- a/openpilot/selfdrive/controls/lib/cycle_timing.py +++ b/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 diff --git a/openpilot/selfdrive/controls/tests/test_publish_cycle_timing.py b/openpilot/selfdrive/controls/tests/test_publish_cycle_timing.py new file mode 100644 index 0000000000..38c37360ff --- /dev/null +++ b/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'] diff --git a/prebuilt.json b/prebuilt.json index 7698978015..cccb809baa 100644 --- a/prebuilt.json +++ b/prebuilt.json @@ -1 +1 @@ -{"protected_inventory_sha256": "686aaeb643ad9c2302bfcd57ee730d1de60a10034b638374b5d5560c98c23687", "schema_version": 1} +{"protected_inventory_sha256": "a6f314fca4f5b54607ebf554388a182cd5afd1afeae910a917dc243024c46eb2", "schema_version": 1}