From d61f86d3398900ef01423d24cfdf897392a8efbb Mon Sep 17 00:00:00 2001 From: Adeeb Shihadeh Date: Sun, 20 Aug 2023 18:50:58 -0700 Subject: [PATCH] replace custom clock helpers with time module (#29499) * replace custom clock stuff wtih time module * fix double * fix release * bump cereal * update type * fix one more --- cereal | 2 +- common/SConscript | 1 - common/clock.pyx | 24 ------------------- common/realtime.py | 11 ++++----- release/files_common | 1 - scripts/waste.py | 6 ++--- selfdrive/athena/athenad.py | 16 ++++++------- selfdrive/controls/controlsd.py | 7 +++--- .../controls/lib/lateral_mpc_lib/lat_mpc.py | 6 ++--- selfdrive/controls/lib/lateral_planner.py | 5 ++-- .../lib/longitudinal_mpc_lib/long_mpc.py | 8 +++---- selfdrive/debug/can_printer.py | 14 +++++------ selfdrive/debug/check_freq.py | 6 ++--- selfdrive/manager/process.py | 3 +-- selfdrive/thermald/power_monitoring.py | 6 ++--- selfdrive/thermald/thermald.py | 14 +++++------ 16 files changed, 52 insertions(+), 78 deletions(-) delete mode 100644 common/clock.pyx diff --git a/cereal b/cereal index 1ee48e0110..3cac040c5b 160000 --- a/cereal +++ b/cereal @@ -1 +1 @@ -Subproject commit 1ee48e0110a46fbdd9db50ed89a38bb5a748cfcb +Subproject commit 3cac040c5b219402cb9b234d53c78a6007298534 diff --git a/common/SConscript b/common/SConscript index 5d6170611f..71894923a6 100644 --- a/common/SConscript +++ b/common/SConscript @@ -31,5 +31,4 @@ if GetOption('test'): env.Program('tests/test_swaglog', ['tests/test_swaglog.cc'], LIBS=[_common, 'json11', 'zmq', 'pthread']) # Cython -envCython.Program('clock.so', 'clock.pyx') envCython.Program('params_pyx.so', 'params_pyx.pyx', LIBS=envCython['LIBS'] + [_common, 'zmq', 'json11']) diff --git a/common/clock.pyx b/common/clock.pyx deleted file mode 100644 index 81333565c5..0000000000 --- a/common/clock.pyx +++ /dev/null @@ -1,24 +0,0 @@ -# distutils: language = c++ -# cython: language_level = 3 -from posix.time cimport clock_gettime, timespec, CLOCK_MONOTONIC_RAW, clockid_t - -IF UNAME_SYSNAME == "Darwin": - # Darwin doesn't have a CLOCK_BOOTTIME - CLOCK_BOOTTIME = CLOCK_MONOTONIC_RAW -ELSE: - from posix.time cimport CLOCK_BOOTTIME - -cdef double readclock(clockid_t clock_id): - cdef timespec ts - cdef double current - - clock_gettime(clock_id, &ts) - current = ts.tv_sec + (ts.tv_nsec / 1000000000.) - return current - -def monotonic_time(): - return readclock(CLOCK_MONOTONIC_RAW) - -def sec_since_boot(): - return readclock(CLOCK_BOOTTIME) - diff --git a/common/realtime.py b/common/realtime.py index 7dd2eb98a6..dd3a5fdf8f 100644 --- a/common/realtime.py +++ b/common/realtime.py @@ -5,9 +5,8 @@ import time from collections import deque from typing import Optional, List, Union -from setproctitle import getproctitle # pylint: disable=no-name-in-module +from setproctitle import getproctitle -from common.clock import sec_since_boot # pylint: disable=no-name-in-module, import-error from system.hardware import PC @@ -50,13 +49,13 @@ class Ratekeeper: def __init__(self, rate: float, print_delay_threshold: Optional[float] = 0.0) -> None: """Rate in Hz for ratekeeping. print_delay_threshold must be nonnegative.""" self._interval = 1. / rate - self._next_frame_time = sec_since_boot() + self._interval + self._next_frame_time = time.monotonic() + self._interval self._print_delay_threshold = print_delay_threshold self._frame = 0 self._remaining = 0.0 self._process_name = getproctitle() self._dts = deque([self._interval], maxlen=100) - self._last_monitor_time = sec_since_boot() + self._last_monitor_time = time.monotonic() @property def frame(self) -> int: @@ -82,11 +81,11 @@ class Ratekeeper: # this only monitor the cumulative lag, but does not enforce a rate def monitor_time(self) -> bool: prev = self._last_monitor_time - self._last_monitor_time = sec_since_boot() + self._last_monitor_time = time.monotonic() self._dts.append(self._last_monitor_time - prev) lagged = False - remaining = self._next_frame_time - sec_since_boot() + remaining = self._next_frame_time - time.monotonic() self._next_frame_time += self._interval if self._print_delay_threshold is not None and remaining < -self._print_delay_threshold: print(f"{self._process_name} lagging by {-remaining * 1000:.2f} ms") diff --git a/release/files_common b/release/files_common index 8381546b03..0f43daa4a0 100644 --- a/release/files_common +++ b/release/files_common @@ -20,7 +20,6 @@ common/__init__.py common/conversions.py common/gpio.py common/realtime.py -common/clock.pyx common/timeout.py common/ffi_wrapper.py common/file_helpers.py diff --git a/scripts/waste.py b/scripts/waste.py index d3c96bf198..e72b367063 100755 --- a/scripts/waste.py +++ b/scripts/waste.py @@ -1,7 +1,7 @@ #!/usr/bin/env python3 import os +import time import numpy as np -from common.realtime import sec_since_boot from multiprocessing import Process from setproctitle import setproctitle # pylint: disable=no-name-in-module @@ -12,12 +12,12 @@ def waste(core): m2 = np.zeros((200, 200)) + 1.2 i = 1 - st = sec_since_boot() + st = time.monotonic() j = 0 while 1: if (i % 100) == 0: setproctitle("%3d: %8d" % (core, i)) - lt = sec_since_boot() + lt = time.monotonic() print("%3d: %8d %f %.2f" % (core, i, lt-st, j)) st = lt i += 1 diff --git a/selfdrive/athena/athenad.py b/selfdrive/athena/athenad.py index d7ef064072..b3a13d2279 100755 --- a/selfdrive/athena/athenad.py +++ b/selfdrive/athena/athenad.py @@ -34,7 +34,7 @@ from common.api import Api from common.basedir import PERSIST from common.file_helpers import CallbackReader from common.params import Params -from common.realtime import sec_since_boot, set_core_affinity +from common.realtime import set_core_affinity from system.hardware import HARDWARE, PC, AGNOS from system.loggerd.config import ROOT from system.loggerd.xattr_cache import getxattr, setxattr @@ -593,10 +593,10 @@ def log_handler(end_event: threading.Event) -> None: return log_files = [] - last_scan = 0 + last_scan = 0. while not end_event.is_set(): try: - curr_scan = sec_since_boot() + curr_scan = time.monotonic() if curr_scan - last_scan > 10: log_files = get_logs_to_send_sorted() last_scan = curr_scan @@ -652,8 +652,8 @@ def log_handler(end_event: threading.Event) -> None: def stat_handler(end_event: threading.Event) -> None: while not end_event.is_set(): - last_scan = 0 - curr_scan = sec_since_boot() + last_scan = 0. + curr_scan = time.monotonic() try: if curr_scan - last_scan > 10: stat_filenames = list(filter(lambda name: not name.startswith(tempfile.gettempprefix()), os.listdir(STATS_DIR))) @@ -721,7 +721,7 @@ def ws_proxy_send(ws: WebSocket, local_sock: socket.socket, signal_sock: socket. def ws_recv(ws: WebSocket, end_event: threading.Event) -> None: - last_ping = int(sec_since_boot() * 1e9) + last_ping = int(time.monotonic() * 1e9) while not end_event.is_set(): try: opcode, data = ws.recv_data(control_frame=True) @@ -730,10 +730,10 @@ def ws_recv(ws: WebSocket, end_event: threading.Event) -> None: data = data.decode("utf-8") recv_queue.put_nowait(data) elif opcode == ABNF.OPCODE_PING: - last_ping = int(sec_since_boot() * 1e9) + last_ping = int(time.monotonic() * 1e9) Params().put("LastAthenaPingTime", str(last_ping)) except WebSocketTimeoutException: - ns_since_last_ping = int(sec_since_boot() * 1e9) - last_ping + ns_since_last_ping = int(time.monotonic() * 1e9) - last_ping if ns_since_last_ping > RECONNECT_TIMEOUT_S * 1e9: cloudlog.exception("athenad.ws_recv.timeout") end_event.set() diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index 8084b18ff6..564a0aa6ac 100755 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -1,13 +1,14 @@ #!/usr/bin/env python3 import os import math +import time from typing import SupportsFloat from cereal import car, log from common.numpy_fast import clip -from common.realtime import sec_since_boot, config_realtime_process, Priority, Ratekeeper, DT_CTRL from common.profiler import Profiler from common.params import Params, put_nonblocking, put_bool_nonblocking +from common.realtime import DT_CTRL, Priority, Ratekeeper, config_realtime_process import cereal.messaging as messaging from cereal.visionipc import VisionIpcClient, VisionStreamType from common.conversions import Conversions as CV @@ -743,7 +744,7 @@ class Controls: if not self.read_only and self.initialized: # send car controls over can - now_nanos = self.can_log_mono_time if REPLAY else int(sec_since_boot() * 1e9) + now_nanos = self.can_log_mono_time if REPLAY else int(time.monotonic() * 1e9) self.last_actuators, can_sends = self.CI.apply(CC, now_nanos) self.pm.send('sendcan', can_list_to_can_capnp(can_sends, msgtype='sendcan', valid=CS.canValid)) CC.actuatorsOutput = self.last_actuators @@ -842,7 +843,7 @@ class Controls: self.CC = CC def step(self): - start_time = sec_since_boot() + start_time = time.monotonic() self.prof.checkpoint("Ratekeeper", ignore=True) self.is_metric = self.params.get_bool("IsMetric") diff --git a/selfdrive/controls/lib/lateral_mpc_lib/lat_mpc.py b/selfdrive/controls/lib/lateral_mpc_lib/lat_mpc.py index 6afcb99dab..506b03031b 100755 --- a/selfdrive/controls/lib/lateral_mpc_lib/lat_mpc.py +++ b/selfdrive/controls/lib/lateral_mpc_lib/lat_mpc.py @@ -1,9 +1,9 @@ #!/usr/bin/env python3 import os +import time import numpy as np from casadi import SX, vertcat, sin, cos -from common.realtime import sec_since_boot # WARNING: imports outside of constants will not trigger a rebuild from selfdrive.modeld.constants import T_IDXS @@ -182,9 +182,9 @@ class LateralMpc(): self.solver.set(N, "p", p_cp[N]) self.solver.cost_set(N, "yref", self.yref[N][:COST_E_DIM]) - t = sec_since_boot() + t = time.monotonic() self.solution_status = self.solver.solve() - self.solve_time = sec_since_boot() - t + self.solve_time = time.monotonic() - t for i in range(N+1): self.x_sol[i] = self.solver.get(i, 'x') diff --git a/selfdrive/controls/lib/lateral_planner.py b/selfdrive/controls/lib/lateral_planner.py index 38258b4b5d..41aa95223e 100644 --- a/selfdrive/controls/lib/lateral_planner.py +++ b/selfdrive/controls/lib/lateral_planner.py @@ -1,6 +1,7 @@ +import time import numpy as np -from common.realtime import sec_since_boot, DT_MDL from common.numpy_fast import interp +from common.realtime import DT_MDL from system.swaglog import cloudlog from selfdrive.controls.lib.lateral_mpc_lib.lat_mpc import LateralMpc from selfdrive.controls.lib.lateral_mpc_lib.lat_mpc import N as LAT_MPC_N @@ -108,7 +109,7 @@ class LateralPlanner: # Check for infeasible MPC solution mpc_nans = np.isnan(self.lat_mpc.x_sol[:, 3]).any() - t = sec_since_boot() + t = time.monotonic() if mpc_nans or self.lat_mpc.solution_status != 0: self.reset_mpc() self.x0[3] = measured_curvature * self.v_ego diff --git a/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py b/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py index dadf6cba28..19cb2bac04 100644 --- a/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py +++ b/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py @@ -1,8 +1,8 @@ #!/usr/bin/env python3 import os +import time import numpy as np from cereal import log -from common.realtime import sec_since_boot from common.numpy_fast import clip from system.swaglog import cloudlog # WARNING: imports outside of constants will not trigger a rebuild @@ -411,7 +411,7 @@ class LongitudinalMpc: self.source = 'lead1' def run(self): - # t0 = sec_since_boot() + # t0 = time.monotonic() # reset = 0 for i in range(N+1): self.solver.set(i, 'p', self.params[i]) @@ -442,14 +442,14 @@ class LongitudinalMpc: self.prev_a = np.interp(T_IDXS + 0.05, T_IDXS, self.a_solution) - t = sec_since_boot() + t = time.monotonic() if self.solution_status != 0: if t > self.last_cloudlog_t + 5.0: self.last_cloudlog_t = t cloudlog.warning(f"Long mpc reset, solution_status: {self.solution_status}") self.reset() # reset = 1 - # print(f"long_mpc timings: total internal {self.solve_time:.2e}, external: {(sec_since_boot() - t0):.2e} qp {self.time_qp_solution:.2e}, \ + # print(f"long_mpc timings: total internal {self.solve_time:.2e}, external: {(time.monotonic() - t0):.2e} qp {self.time_qp_solution:.2e}, \ # lin {self.time_linearization:.2e} qp_iter {qp_iter}, reset {reset}") diff --git a/selfdrive/debug/can_printer.py b/selfdrive/debug/can_printer.py index 3f991d4e6c..220008979d 100755 --- a/selfdrive/debug/can_printer.py +++ b/selfdrive/debug/can_printer.py @@ -1,17 +1,17 @@ #!/usr/bin/env python3 import argparse import binascii +import time from collections import defaultdict import cereal.messaging as messaging -from common.realtime import sec_since_boot def can_printer(bus, max_msg, addr, ascii_decode): logcan = messaging.sub_sock('can', addr=addr) - start = sec_since_boot() - lp = sec_since_boot() + start = time.monotonic() + lp = time.monotonic() msgs = defaultdict(list) while 1: can_recv = messaging.drain_sock(logcan, wait_for_one=True) @@ -20,17 +20,17 @@ def can_printer(bus, max_msg, addr, ascii_decode): if y.src == bus: msgs[y.address].append(y.dat) - if sec_since_boot() - lp > 0.1: + if time.monotonic() - lp > 0.1: dd = chr(27) + "[2J" - dd += f"{sec_since_boot() - start:5.2f}\n" + dd += f"{time.monotonic() - start:5.2f}\n" for addr in sorted(msgs.keys()): a = f"\"{msgs[addr][-1].decode('ascii', 'backslashreplace')}\"" if ascii_decode else "" x = binascii.hexlify(msgs[addr][-1]).decode('ascii') - freq = len(msgs[addr]) / (sec_since_boot() - start) + freq = len(msgs[addr]) / (time.monotonic() - start) if max_msg is None or addr < max_msg: dd += "%04X(%4d)(%6d)(%3dHz) %s %s\n" % (addr, addr, len(msgs[addr]), freq, x.ljust(20), a) print(dd) - lp = sec_since_boot() + lp = time.monotonic() if __name__ == "__main__": parser = argparse.ArgumentParser(description="simple CAN data viewer", diff --git a/selfdrive/debug/check_freq.py b/selfdrive/debug/check_freq.py index 6436abb4f1..7e7b05e950 100755 --- a/selfdrive/debug/check_freq.py +++ b/selfdrive/debug/check_freq.py @@ -1,10 +1,10 @@ #!/usr/bin/env python3 import argparse import numpy as np +import time from collections import defaultdict, deque from typing import DefaultDict, Deque, MutableSequence -from common.realtime import sec_since_boot import cereal.messaging as messaging @@ -22,7 +22,7 @@ if __name__ == "__main__": rcv_times: DefaultDict[str, MutableSequence[float]] = defaultdict(lambda: deque(maxlen=100)) valids: DefaultDict[str, Deque[bool]] = defaultdict(lambda: deque(maxlen=100)) - t = sec_since_boot() + t = time.monotonic() for name in socket_names: sock = messaging.sub_sock(name, poller=poller) sockets[sock] = name @@ -36,7 +36,7 @@ if __name__ == "__main__": name = msg.which() - t = sec_since_boot() + t = time.monotonic() rcv_times[name].append(msg.logMonoTime / 1e9) valids[name].append(msg.valid) diff --git a/selfdrive/manager/process.py b/selfdrive/manager/process.py index 0acb23857a..e6c2c52bec 100644 --- a/selfdrive/manager/process.py +++ b/selfdrive/manager/process.py @@ -15,7 +15,6 @@ import selfdrive.sentry as sentry from cereal import car from common.basedir import BASEDIR from common.params import Params -from common.realtime import sec_since_boot from system.swaglog import cloudlog from cereal import log @@ -104,7 +103,7 @@ class ManagerProcess(ABC): except Exception: pass - dt = sec_since_boot() - self.last_watchdog_time / 1e9 + dt = time.monotonic() - self.last_watchdog_time / 1e9 if dt > self.watchdog_max_dt: if self.watchdog_seen and ENABLE_WATCHDOG: diff --git a/selfdrive/thermald/power_monitoring.py b/selfdrive/thermald/power_monitoring.py index 06e2b5e8f9..e9b07da1f6 100644 --- a/selfdrive/thermald/power_monitoring.py +++ b/selfdrive/thermald/power_monitoring.py @@ -1,8 +1,8 @@ +import time import threading from typing import Optional from common.params import Params, put_nonblocking -from common.realtime import sec_since_boot from system.hardware import HARDWARE from system.swaglog import cloudlog from selfdrive.statsd import statlog @@ -41,7 +41,7 @@ class PowerMonitoring: # Calculation tick def calculate(self, voltage: Optional[int], ignition: bool): try: - now = sec_since_boot() + now = time.monotonic() # If peripheralState is None, we're probably not in a car, so we don't care if voltage is None: @@ -113,7 +113,7 @@ class PowerMonitoring: if offroad_timestamp is None: return False - now = sec_since_boot() + now = time.monotonic() should_shutdown = False offroad_time = (now - offroad_timestamp) low_voltage_shutdown = (self.car_voltage_mV < (VBATT_PAUSE_CHARGING * 1e3) and diff --git a/selfdrive/thermald/thermald.py b/selfdrive/thermald/thermald.py index 931f9be88f..d2ab8ff733 100755 --- a/selfdrive/thermald/thermald.py +++ b/selfdrive/thermald/thermald.py @@ -17,7 +17,7 @@ from common.dict_helpers import strip_deprecated_keys from common.time import MIN_DATE from common.filter_simple import FirstOrderFilter from common.params import Params -from common.realtime import DT_TRML, sec_since_boot +from common.realtime import DT_TRML from selfdrive.controls.lib.alertmanager import set_offroad_alert from system.hardware import HARDWARE, TICI, AGNOS from system.loggerd.config import get_available_percent @@ -230,7 +230,7 @@ def thermald_thread(end_event, hw_queue): if TICI: fan_controller = TiciFanController() - elif (sec_since_boot() - sm.rcv_time['pandaStates']) > DISCONNECT_TIMEOUT: + elif (time.monotonic() - sm.rcv_time['pandaStates']) > DISCONNECT_TIMEOUT: if onroad_conditions["ignition"]: onroad_conditions["ignition"] = False cloudlog.error("panda timed out onroad") @@ -273,7 +273,7 @@ def thermald_thread(end_event, hw_queue): if fan_controller is not None: msg.deviceState.fanSpeedPercentDesired = fan_controller.update(all_comp_temp, onroad_conditions["ignition"]) - is_offroad_for_5_min = (started_ts is None) and ((not started_seen) or (off_ts is None) or (sec_since_boot() - off_ts > 60 * 5)) + is_offroad_for_5_min = (started_ts is None) and ((not started_seen) or (off_ts is None) or (time.monotonic() - off_ts > 60 * 5)) if is_offroad_for_5_min and offroad_comp_temp > OFFROAD_DANGER_TEMP: # if device is offroad and already hot without the extra onroad load, # we want to cool down first before increasing load @@ -354,10 +354,10 @@ def thermald_thread(end_event, hw_queue): if should_start: off_ts = None if started_ts is None: - started_ts = sec_since_boot() + started_ts = time.monotonic() started_seen = True if startup_blocked_ts is not None: - cloudlog.event("Startup after block", block_duration=(sec_since_boot() - startup_blocked_ts), + cloudlog.event("Startup after block", block_duration=(time.monotonic() - startup_blocked_ts), startup_conditions=startup_conditions, onroad_conditions=onroad_conditions, startup_conditions_prev=startup_conditions_prev, error=True) startup_blocked_ts = None @@ -365,11 +365,11 @@ def thermald_thread(end_event, hw_queue): if onroad_conditions["ignition"] and (startup_conditions != startup_conditions_prev): cloudlog.event("Startup blocked", startup_conditions=startup_conditions, onroad_conditions=onroad_conditions, error=True) startup_conditions_prev = startup_conditions.copy() - startup_blocked_ts = sec_since_boot() + startup_blocked_ts = time.monotonic() started_ts = None if off_ts is None: - off_ts = sec_since_boot() + off_ts = time.monotonic() # Offroad power monitoring voltage = None if peripheralState.pandaType == log.PandaState.PandaType.unknown else peripheralState.voltage