openpilot release

common/api/__init__.py

@@ -0,0 +1,8 @@
+import requests
+
+def api_get(endpoint, method='GET', timeout=None, **params):
+  backend = "https://api.commadotai.com/"
+
+  params['_version'] = "OPENPILOTv0.0"
+
+  return requests.request(method, backend+endpoint, timeout=timeout, params=params)

common/crash.py

@@ -0,0 +1,26 @@
+"""Install exception handler for process crash."""
+import os
+import sys
+
+if os.getenv("NOLOG"):
+  def capture_exception(*exc_info):
+    pass
+  def install():
+    pass
+else:
+  from raven import Client
+  from raven.transport.http import HTTPTransport
+
+  client = Client('https://1994756b5e6f41cf939a4c65de45f4f2:cefebaf3a8aa40d182609785f7189bd7@app.getsentry.com/77924',
+                  install_sys_hook=False, transport=HTTPTransport)
+
+  capture_exception = client.captureException
+
+  def install():
+    # installs a sys.excepthook
+    __excepthook__ = sys.excepthook
+    def handle_exception(*exc_info):
+      if exc_info[0] not in (KeyboardInterrupt, SystemExit):
+        client.captureException(exc_info=exc_info)
+      __excepthook__(*exc_info)
+    sys.excepthook = handle_exception

common/dbc.py

@@ -0,0 +1,182 @@
+import re
+from collections import namedtuple
+import bitstring
+
+def int_or_float(s):
+  # return number, trying to maintain int format
+  try:
+    return int(s)
+  except ValueError:
+    return float(s)
+
+DBCSignal = namedtuple(
+  "DBCSignal", ["name", "start_bit", "size", "is_little_endian", "is_signed",
+                "factor", "offset", "tmin", "tmax", "units"])
+
+class dbc(object):
+  def __init__(self, fn):
+    self.txt = open(fn).read().split("\n")
+    self._warned_addresses = set()
+
+    # regexps from https://github.com/ebroecker/canmatrix/blob/master/canmatrix/importdbc.py
+    bo_regexp = re.compile("^BO\_ (\w+) (\w+) *: (\w+) (\w+)")
+    sg_regexp = re.compile("^SG\_ (\w+) : (\d+)\|(\d+)@(\d+)([\+|\-]) \(([0-9.+\-eE]+),([0-9.+\-eE]+)\) \[([0-9.+\-eE]+)\|([0-9.+\-eE]+)\] \"(.*)\" (.*)")
+    sgm_regexp = re.compile("^SG\_ (\w+) (\w+) *: (\d+)\|(\d+)@(\d+)([\+|\-]) \(([0-9.+\-eE]+),([0-9.+\-eE]+)\) \[([0-9.+\-eE]+)\|([0-9.+\-eE]+)\] \"(.*)\" (.*)")
+
+    # A dictionary which maps message ids to tuples ((name, size), signals).
+    #   name is the ASCII name of the message.
+    #   size is the size of the message in bytes.
+    #   signals is a list signals contained in the message.
+    # signals is a list of DBCSignal in order of increasing start_bit.
+    self.msgs = {}
+
+    self.bits = []
+    for i in range(0, 64, 8):
+      for j in range(7, -1, -1):
+        self.bits.append(i+j)
+
+    for l in self.txt:
+      l = l.strip()
+
+      if l.startswith("BO_ "):
+        # new group
+        dat = bo_regexp.match(l)
+
+        if dat is None:
+          print "bad BO", l
+        name = dat.group(2)
+        size = int(dat.group(3))
+        ids = int(dat.group(1), 0) # could be hex
+
+        self.msgs[ids] = ((name, size), [])
+
+      if l.startswith("SG_ "):
+        # new signal
+        dat = sg_regexp.match(l)
+        go = 0
+        if dat is None:
+          dat = sgm_regexp.match(l)
+          go = 1
+        if dat is None:
+          print "bad SG", l
+
+        sgname = dat.group(1)
+        start_bit = int(dat.group(go+2))
+        signal_size = int(dat.group(go+3))
+        is_little_endian = int(dat.group(go+4))==1
+        is_signed = dat.group(go+5)=='-'
+        factor = int_or_float(dat.group(go+6))
+        offset = int_or_float(dat.group(go+7))
+        tmin = int_or_float(dat.group(go+8))
+        tmax = int_or_float(dat.group(go+9))
+        units = dat.group(go+10)
+
+        self.msgs[ids][1].append(
+          DBCSignal(sgname, start_bit, signal_size, is_little_endian,
+                    is_signed, factor, offset, tmin, tmax, units))
+
+    for msg in self.msgs.viewvalues():
+      msg[1].sort(key=lambda x: x.start_bit)
+
+  def encode(self, msg_id, dd):
+    """Encode a CAN message using the dbc.
+
+       Inputs:
+        msg_id: The message ID.
+        dd: A dictionary mapping signal name to signal data.
+    """
+    # TODO: Stop using bitstring, which is super slow.
+    msg_def = self.msgs[msg_id]
+    size = msg_def[0][1]
+
+    bsf = bitstring.Bits(hex="00"*size)
+    for s in msg_def[1]:
+      ival = dd.get(s.name)
+      if ival is not None:
+        ival = (ival / s.factor) - s.offset
+        ival = int(round(ival))
+
+        # should pack this
+        if s.is_little_endian:
+          ss = s.start_bit
+        else:
+          ss = self.bits.index(s.start_bit)
+
+
+        if s.is_signed:
+          tbs = bitstring.Bits(int=ival, length=s.size)
+        else:
+          tbs = bitstring.Bits(uint=ival, length=s.size)
+
+        lpad = bitstring.Bits(bin="0b"+"0"*ss)
+        rpad = bitstring.Bits(bin="0b"+"0"*(8*size-(ss+s.size)))
+        tbs = lpad+tbs+rpad
+
+        bsf |= tbs
+    return bsf.tobytes()
+
+  def decode(self, x, arr=None, debug=False):
+    """Decode a CAN message using the dbc.
+
+       Inputs:
+        x: A collection with elements (address, time, data), where address is
+           the CAN address, time is the bus time, and data is the CAN data as a
+           hex string.
+        arr: Optional list of signals which should be decoded and returned.
+        debug: True to print debugging statements.
+
+       Returns:
+        A tuple (name, data), where name is the name of the CAN message and data
+        is the decoded result. If arr is None, data is a dict of properties.
+        Otherwise data is a list of the same length as arr.
+
+        Returns (None, None) if the message could not be decoded.
+    """
+    if arr is None:
+      out = {}
+    else:
+      out = [None]*len(arr)
+
+    msg = self.msgs.get(x[0])
+    if msg is None:
+      if x[0] not in self._warned_addresses:
+        print("WARNING: Unknown message address {}".format(x[0]))
+        self._warned_addresses.add(x[0])
+      return None, None
+
+    name = msg[0][0]
+    if debug:
+      print name
+
+    blen = (len(x[2])/2)*8
+    x2_int = int(x[2], 16)
+
+    for s in msg[1]:
+      if arr is not None and s[0] not in arr:
+        continue
+
+      # big or little endian?
+      #   see http://vi-firmware.openxcplatform.com/en/master/config/bit-numbering.html
+      if s[3] is False:
+        ss = self.bits.index(s[1])
+      else:
+        ss = s[1]
+
+      data_bit_pos = (blen - (ss + s[2]))
+      if data_bit_pos < 0:
+        continue
+      ival = (x2_int >> data_bit_pos) & ((1 << (s[2])) - 1)
+
+      if s[4] and (ival & (1<<(s[2]-1))): # signed
+        ival -= (1<<s[2])
+
+      # control the offset
+      ival = (ival + s[6])*s[5]
+      if debug:
+        print "%40s  %2d %2d  %7.2f %s" % (s[0], s[1], s[2], ival, s[-1])
+
+      if arr is None:
+        out[s[0]] = ival
+      else:
+        out[arr.index(s[0])] = ival
+    return name, out

common/filters.py

@@ -0,0 +1,17 @@
+"""Classes for filtering discrete time signals."""
+import numpy as np
+
+
+class FirstOrderLowpassFilter(object):
+  def __init__(self, fc, dt, x1=0):
+    self.kf = 2 * np.pi * fc * dt / (1 + 2 * np.pi * fc * dt)
+    self.x1 = x1
+
+  def __call__(self, x):
+    self.x1 = (1 - self.kf) * self.x1 + self.kf * x
+
+    # If previous or current is NaN, reset filter.
+    if np.isnan(self.x1):
+      self.x1 = x
+
+    return self.x1

common/kalman/ekf.py

@@ -0,0 +1,300 @@
+import abc
+import numpy as np
+import numpy.matlib
+
+# The EKF class contains the framework for an Extended Kalman Filter, but must be subclassed to use.
+# A subclass must implement:
+#   1) calc_transfer_fun(); see bottom of file for more info.
+#   2) __init__() to initialize self.state, self.covar, and self.process_noise appropriately
+
+# Alternatively, the existing implementations of EKF can be used (e.g. EKF2D)
+
+# Sensor classes are optionally used to pass measurement information into the EKF, to keep
+#   sensor parameters and processing methods for a each sensor together.
+# Sensor classes have a read() method which takes raw sensor data and returns
+#   a SensorReading object, which can be passed to the EKF update() method.
+
+# For usage, see run_ekf1d.py in selfdrive/new for a simple example.
+# ekf.predict(dt) should be called between update cycles with the time since it was last called.
+# Ideally, predict(dt) should be called at a relatively constant rate.
+# update() should be called once per sensor, and can be called multiple times between predict steps.
+# Access and set the state of the filter directly with ekf.state and ekf.covar.
+
+class SensorReading:
+  # Given a perfect model and no noise, data = obs_model * state
+  def __init__(self, data, covar, obs_model):
+    self.data = data
+    self.obs_model = obs_model
+    self.covar = covar
+
+  def __repr__(self):
+    return "SensorReading(data={}, covar={}, obs_model={})".format(
+      repr(self.data), repr(self.covar), repr(self.obs_model))
+
+
+# A generic sensor class that does no pre-processing of data
+class SimpleSensor:
+  # obs_model can be 
+  #   a full obesrvation model matrix, or
+  #   an integer or tuple of indices into ekf.state, indicating which variables are being directly observed
+  # covar can be
+  #   a full covariance matrix
+  #   a float or tuple of individual covars for each component of the sensor reading
+  # dims is the number of states in the EKF
+  def __init__(self, obs_model, covar, dims):
+    # Allow for integer covar/obs_model
+    if not hasattr(obs_model, "__len__"):
+      obs_model = (obs_model, )
+    if not hasattr(covar, "__len__"):
+      covar = (covar, )
+
+    # Full observation model passed
+    if dims in np.array(obs_model).shape:
+      self.obs_model = np.asmatrix(obs_model)
+      self.covar = np.asmatrix(covar)
+    # Indices of unit observations passed
+    else:
+      self.obs_model = np.matlib.zeros((len(obs_model), dims))
+      self.obs_model[:, list(obs_model)] = np.identity(len(obs_model))
+      if np.asarray(covar).ndim == 2:
+        self.covar = np.asmatrix(covar)
+      elif len(covar) == len(obs_model):
+        self.covar = np.matlib.diag(covar)
+      else:
+        self.covar = np.matlib.identity(len(obs_model)) * covar
+
+  def read(self, data, covar=None):
+    if covar:
+      self.covar = covar
+    return SensorReading(data, self.covar, self.obs_model)
+
+class GPS:
+  earth_r = 6371e3  # m, average earth radius
+
+  def __init__(self, xy_idx=(0, 1), dims=2, var=1e4):
+    self.obs_model = np.matlib.zeros((2, dims))
+    self.obs_model[:, tuple(xy_idx)] = np.matlib.identity(2)
+    self.covar = np.matlib.identity(2) * var
+
+  # [lat, lon] in decimal degrees
+  def init_pos(self, latlon):
+    self.init_lat, self.init_lon = np.radians(np.asarray(latlon[:2]))
+
+  # Compute straight-line distance, in meters, between two lat/long coordinates
+  # Input in radians
+  def haversine(self, lat1, lon1, lat2, lon2):
+    lat_diff = lat2 - lat1
+    lon_diff = lon2 - lon1
+    d = np.sin(lat_diff * 0.5)**2 + np.cos(lat1) * np.cos(lat2) * np.sin(
+      lon_diff * 0.5)**2
+    h = 2 * GPS.earth_r * np.arcsin(np.sqrt(d))
+    return h
+
+  # Convert decimal degrees into meters
+  def convert_deg2m(self, lat, lon):
+    lat, lon = np.radians([lat, lon])
+
+    xs = (lon - self.init_lon) * np.cos(self.init_lat) * GPS.earth_r
+    ys = (lat - self.init_lat) * GPS.earth_r
+
+    return xs, ys
+
+  # Convert meters into decimal degrees
+  def convert_m2deg(self, xs, ys):
+    lat = ys / GPS.earth_r + self.init_lat
+    lon = xs / (GPS.earth_r * np.cos(self.init_lat)) + self.init_lon
+
+    return np.degrees(lat), np.degrees(lon)
+
+  # latlon is [lat, long,] as decimal degrees
+  # accuracy is as given by Android location service: radius of 68% confidence
+  def read(self, latlon, accuracy=None):
+    x_dist, y_dist = self.convert_deg2m(latlon[0], latlon[1])
+    if not accuracy:
+      covar = self.covar
+    else:
+      covar = np.matlib.identity(2) * accuracy**2
+
+    return SensorReading(
+      np.asmatrix([x_dist, y_dist]).T, covar, self.obs_model)
+
+class EKF:
+  __metaclass__ = abc.ABCMeta
+
+  def __init__(self, debug=False):
+    self.DEBUG = debug
+
+
+  def __str__(self):
+    return "EKF(state={}, covar={})".format(self.state, self.covar)
+
+  # Measurement update
+  # Reading should be a SensorReading object with data, covar, and obs_model attributes
+  def update(self, reading):
+    # Potential improvements:
+    # deal with negative covars
+    # add noise to really low covars to ensure stability
+    # use mahalanobis distance to reject outliers
+    # wrap angles after state updates and innovation
+
+    # y = z - H*x
+    innovation = reading.data - reading.obs_model * self.state
+
+    if self.DEBUG:
+      print "reading:\n",reading.data
+      print "innovation:\n",innovation
+
+    # S = H*P*H' + R
+    innovation_covar = reading.obs_model * self.covar * reading.obs_model.T + reading.covar
+
+    # K = P*H'*S^-1
+    kalman_gain = self.covar * reading.obs_model.T * np.linalg.inv(
+      innovation_covar)
+
+    if self.DEBUG:
+      print "gain:\n", kalman_gain
+      print "innovation_covar:\n", innovation_covar
+      print "innovation: ", innovation
+      print "test: ", self.covar * reading.obs_model.T * (
+        reading.obs_model * self.covar * reading.obs_model.T + reading.covar *
+        0).I
+
+    # x = x + K*y
+    self.state += kalman_gain*innovation
+
+    # print "covar", np.diag(self.covar)
+    #self.state[(roll_vel, yaw_vel, pitch_vel),:] = reading.data
+
+    # Standard form: P = (I - K*H)*P
+    # self.covar = (self.identity - kalman_gain*reading.obs_model) * self.covar
+
+    # Use the Joseph form for numerical stability: P = (I-K*H)*P*(I - K*H)' + K*R*K'
+    aux_mtrx = (self.identity - kalman_gain * reading.obs_model)
+    self.covar = aux_mtrx * self.covar * aux_mtrx.T + kalman_gain * reading.covar * kalman_gain.T
+
+    if self.DEBUG:
+      print "After update"
+      print "state\n", self.state
+      print "covar:\n",self.covar
+
+  def update_scalar(self, reading):
+    # like update but knowing that measurment is a scalar
+    # this avoids matrix inversions and speeds up (surprisingly) drived.py a lot
+
+    # innovation = reading.data - np.matmul(reading.obs_model, self.state)   
+    # innovation_covar = np.matmul(np.matmul(reading.obs_model, self.covar), reading.obs_model.T) + reading.covar    
+    # kalman_gain = np.matmul(self.covar, reading.obs_model.T)/innovation_covar   
+    # self.state += np.matmul(kalman_gain, innovation)   
+    # aux_mtrx = self.identity - np.matmul(kalman_gain, reading.obs_model)    
+    # self.covar =  np.matmul(aux_mtrx, np.matmul(self.covar, aux_mtrx.T)) + np.matmul(kalman_gain, np.matmul(reading.covar, kalman_gain.T))
+
+    # written without np.matmul
+    es = np.einsum
+    ABC_T = "ij,jk,lk->il"
+    AB_T = "ij,kj->ik"
+    AB = "ij,jk->ik"
+    innovation = reading.data - es(AB, reading.obs_model, self.state)
+    innovation_covar = es(ABC_T, reading.obs_model, self.covar,
+                          reading.obs_model) + reading.covar
+    kalman_gain = es(AB_T, self.covar, reading.obs_model) / innovation_covar
+
+    self.state += es(AB, kalman_gain, innovation)
+    aux_mtrx = self.identity - es(AB, kalman_gain, reading.obs_model)
+    self.covar = es(ABC_T, aux_mtrx, self.covar, aux_mtrx) + \
+                 es(ABC_T, kalman_gain, reading.covar, kalman_gain)
+
+  # Prediction update
+  def predict(self, dt):
+    es = np.einsum
+    ABC_T = "ij,jk,lk->il"
+    AB = "ij,jk->ik"
+
+    # State update
+    transfer_fun, transfer_fun_jacobian = self.calc_transfer_fun(dt)
+
+    # self.state = np.matmul(transfer_fun, self.state)
+    # self.covar = np.matmul(np.matmul(transfer_fun_jacobian, self.covar), transfer_fun_jacobian.T) + self.process_noise * dt
+
+    # x = f(x, u), written in the form x = A(x, u)*x
+    self.state = es(AB, transfer_fun, self.state)
+
+    # P = J*P*J' + Q
+    self.covar = es(ABC_T, transfer_fun_jacobian, self.covar,
+                    transfer_fun_jacobian) + self.process_noise * dt  #!dt
+
+    #! Clip covariance to avoid explosions
+    self.covar = np.clip(self.covar,-1e10,1e10)
+
+  @abc.abstractmethod
+  def calc_transfer_fun(self, dt):
+    """Return a tuple with the transfer function and transfer function jacobian
+    The transfer function and jacobian should both be a numpy matrix of size DIMSxDIMS
+
+    The transfer function matrix A should satisfy the state-update equation
+      x_(k+1) = A * x_k
+
+    The jacobian J is the direct jacobian A*x_k. For linear systems J=A.
+
+    Current implementations calculate A and J as functions of state. Control input
+      can be added trivially by adding a control parameter to predict() and calc_tranfer_update(),
+      and using it during calcualtion of A and J
+    """
+
+class FastEKF1D(EKF):
+  """Fast version of EKF for 1D problems with scalar readings."""
+
+  def __init__(self, dt, var_init, Q):
+    super(FastEKF1D, self).__init__(False)
+    self.state = [0, 0]
+    self.covar = [var_init, var_init, 0]
+
+    # Process Noise
+    self.dtQ0 = dt * Q[0]
+    self.dtQ1 = dt * Q[1]
+
+  def update(self, reading):
+    raise NotImplementedError
+
+  def update_scalar(self, reading):
+    # TODO(mgraczyk): Delete this for speed.
+    # assert np.all(reading.obs_model == [1, 0])
+
+    rcov = reading.covar[0, 0]
+
+    x = self.state
+    S = self.covar
+
+    innovation = reading.data - x[0]
+    innovation_covar = S[0] + rcov
+
+    k0 = S[0] / innovation_covar
+    k1 = S[2] / innovation_covar
+
+    x[0] += k0 * innovation
+    x[1] += k1 * innovation
+
+    mk = 1 - k0
+    S[1] += k1 * (k1 * (S[0] + rcov) - 2 * S[2])
+    S[2] = mk * (S[2] - k1 * S[0]) + rcov * k0 * k1
+    S[0] = mk * mk * S[0] + rcov * k0 * k0
+
+  def predict(self, dt):
+    # State update
+    x = self.state
+
+    x[0] += dt * x[1]
+
+    # P = J*P*J' + Q
+    S = self.covar
+    S[0] += dt * (2 * S[2] + dt * S[1]) + self.dtQ0
+    S[2] += dt * S[1]
+    S[1] += self.dtQ1
+
+    # Clip covariance to avoid explosions
+    S = max(-1e10, min(S, 1e10))
+
+  def calc_transfer_fun(self, dt):
+    tf = np.identity(2)
+    tf[0, 1] = dt
+    tfj = tf
+    return tf, tfj

common/logging_extra.py

@@ -0,0 +1,134 @@
+import os
+import sys
+import copy
+import json
+import socket
+import logging
+from threading import local
+from collections import OrderedDict
+from contextlib import contextmanager
+
+class SwagFormatter(logging.Formatter):
+  def __init__(self, swaglogger):
+    logging.Formatter.__init__(self, None, '%a %b %d %H:%M:%S %Z %Y')
+
+    self.swaglogger = swaglogger
+    self.host = socket.gethostname()
+
+  def json_handler(self, obj):
+    # if isinstance(obj, (datetime.date, datetime.time)):
+    #   return obj.isoformat()
+    return repr(obj)
+
+  def format(self, record):
+    record_dict = OrderedDict()
+
+    if isinstance(record.msg, dict):
+      record_dict['msg'] = record.msg
+    else:
+      try:
+        record_dict['msg'] = record.getMessage()
+      except (ValueError, TypeError):
+        record_dict['msg'] = [record.msg]+record.args
+
+    record_dict['ctx'] = self.swaglogger.get_ctx()
+
+    if record.exc_info:
+      record_dict['exc_info'] = self.formatException(record.exc_info)
+
+    record_dict['level'] = record.levelname
+    record_dict['levelnum'] = record.levelno
+    record_dict['name'] = record.name
+    record_dict['filename'] = record.filename
+    record_dict['lineno'] = record.lineno
+    record_dict['pathname'] = record.pathname
+    record_dict['module'] = record.module
+    record_dict['funcName'] = record.funcName
+    record_dict['host'] = self.host
+    record_dict['process'] = record.process
+    record_dict['thread'] = record.thread
+    record_dict['threadName'] = record.threadName
+    record_dict['created'] = record.created
+
+    # asctime = self.formatTime(record, self.datefmt)
+
+    return json.dumps(record_dict, default=self.json_handler)
+
+_tmpfunc = lambda: 0
+_srcfile = os.path.normcase(_tmpfunc.__code__.co_filename)
+
+class SwagLogger(logging.Logger):
+  def __init__(self):
+    logging.Logger.__init__(self, "swaglog")
+
+    self.global_ctx = {}
+
+    self.log_local = local()
+    self.log_local.ctx = {}
+
+  def findCaller(self):
+    """
+      Find the stack frame of the caller so that we can note the source
+      file name, line number and function name.
+      """
+    # f = currentframe()
+    f = sys._getframe(3)
+    #On some versions of IronPython, currentframe() returns None if
+    #IronPython isn't run with -X:Frames.
+    if f is not None:
+      f = f.f_back
+    rv = "(unknown file)", 0, "(unknown function)"
+    while hasattr(f, "f_code"):
+      co = f.f_code
+      filename = os.path.normcase(co.co_filename)
+      if filename in (logging._srcfile, _srcfile):
+        f = f.f_back
+        continue
+      rv = (co.co_filename, f.f_lineno, co.co_name)
+      break
+    return rv
+
+  def local_ctx(self):
+    try:
+      return self.log_local.ctx
+    except AttributeError:
+      self.log_local.ctx = {}
+      return self.log_local.ctx
+
+  def get_ctx(self):
+    return dict(self.local_ctx(), **self.global_ctx)
+
+  @contextmanager
+  def ctx(self, **kwargs):
+    old_ctx = self.local_ctx()
+    self.log_local.ctx = copy.copy(old_ctx) or {}
+    self.log_local.ctx.update(kwargs)
+    try:
+      yield
+    finally:
+      self.log_local.ctx = old_ctx
+
+  def bind(self, **kwargs):
+    self.local_ctx().update(kwargs)
+
+  def bind_global(self, **kwargs):
+    self.global_ctx.update(kwargs)
+
+  def event(self, event_name, *args, **kwargs):
+    evt = OrderedDict()
+    evt['event'] = event_name
+    if args:
+      evt['args'] = args
+    evt.update(kwargs)
+    self.info(evt)
+
+if __name__ == "__main__":
+  log = SwagLogger()
+
+  log.info("asdasd %s", "a")
+  log.info({'wut': 1})
+
+  with log.ctx():
+    log.bind(user="some user")
+    log.info("in req")
+    log.event("do_req")

common/numpy_fast.py

@@ -0,0 +1,2 @@
+def clip(x, lo, hi):
+  return max(lo, min(hi, x))

common/realtime.py

@@ -0,0 +1,94 @@
+"""Utilities for reading real time clocks and keeping soft real time constraints."""
+import time
+import ctypes
+import platform
+import subprocess
+import multiprocessing
+import os
+
+CLOCK_MONOTONIC_RAW = 4 # see <linux/time.h>
+CLOCK_BOOTTIME = 7
+
+class timespec(ctypes.Structure):
+  _fields_ = [
+    ('tv_sec', ctypes.c_long),
+    ('tv_nsec', ctypes.c_long),
+  ]
+
+
+try:
+  libc = ctypes.CDLL('libc.so', use_errno=True)
+except OSError:
+  try:
+    libc = ctypes.CDLL('libc.so.6', use_errno=True)
+  except OSError:
+    libc = None
+
+if libc is not None:
+  libc.clock_gettime.argtypes = [ctypes.c_int, ctypes.POINTER(timespec)]
+
+def clock_gettime(clk_id):
+  if platform.system() == "darwin":
+    # TODO: fix this
+    return time.time()
+  else:
+    t = timespec()
+    if libc.clock_gettime(clk_id, ctypes.pointer(t)) != 0:
+      errno_ = ctypes.get_errno()
+      raise OSError(errno_, os.strerror(errno_))
+    return t.tv_sec + t.tv_nsec * 1e-9
+
+def monotonic_time():
+  return clock_gettime(CLOCK_MONOTONIC_RAW)
+
+def sec_since_boot():
+  return clock_gettime(CLOCK_BOOTTIME)
+
+
+def set_realtime_priority(level):
+  if os.getuid() != 0:
+    print "not setting priority, not root"
+    return
+  if platform.machine() == "x86_64":
+    NR_gettid = 186
+  elif platform.machine() == "aarch64":
+    NR_gettid = 178
+  else:
+    raise NotImplementedError
+
+  tid = libc.syscall(NR_gettid)
+  subprocess.check_call(['chrt', '-f', '-p', str(level), str(tid)])
+
+
+class Ratekeeper(object):
+  def __init__(self, rate, print_delay_threshold=0.):
+    """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._print_delay_threshold = print_delay_threshold
+    self._frame = 0
+    self._remaining = 0
+    self._process_name = multiprocessing.current_process().name
+
+  @property
+  def frame(self):
+    return self._frame
+
+  @property
+  def remaining(self):
+    return self._remaining
+
+  # Maintain loop rate by calling this at the end of each loop
+  def keep_time(self):
+    self.monitor_time()
+    if self._remaining > 0:
+      time.sleep(self._remaining)
+
+  # this only monitor the cumulative lag, but does not enforce a rate
+  def monitor_time(self):
+    remaining = self._next_frame_time - sec_since_boot()
+    self._next_frame_time += self._interval
+    if remaining < -self._print_delay_threshold:
+      print self._process_name, "lagging by", round(-remaining * 1000, 2), "ms"
+    self._frame += 1
+    self._remaining = remaining

common/services.py

@@ -0,0 +1,82 @@
+# TODO: these port numbers are hardcoded in c, fix this
+
+# LogRotate: 8001 is a PUSH PULL socket between loggerd and visiond
+
+class Service(object):
+  def __init__(self, port, should_log):
+    self.port = port
+    self.should_log = should_log
+
+# all ZMQ pub sub
+service_list = {
+  # frame syncing packet
+  "frame": Service(8002, True),
+  # accel, gyro, and compass
+  "sensorEvents": Service(8003, True),
+  # GPS data, also global timestamp
+  "gpsNMEA": Service(8004, True),
+  # CPU+MEM+GPU+BAT temps
+  "thermal": Service(8005, True),
+  # List(CanData), list of can messages
+  "can": Service(8006, True),
+  "live100": Service(8007, True),
+  # random events we want to log
+  #"liveEvent": Service(8008, True),
+  "model": Service(8009, True),
+  "features": Service(8010, True),
+  "health": Service(8011, True),
+  "live20": Service(8012, True),
+  #"liveUI": Service(8014, True),
+  "encodeIdx": Service(8015, True),
+  "liveTracks": Service(8016, True),
+  "sendcan": Service(8017, True),
+  "logMessage": Service(8018, True),
+  "liveCalibration": Service(8019, True),
+  "androidLog": Service(8020, True),
+}
+
+# manager -- base process to manage starting and stopping of all others
+#   subscribes: health
+#   publishes:  thermal
+
+# boardd -- communicates with the car
+#   subscribes: sendcan
+#   publishes:  can, health
+
+# visiond -- talks to the cameras, runs the model, saves the videos
+#   subscribes: liveCalibration, sensorEvents
+#   publishes:  frame, encodeIdx, model, features
+
+# controlsd -- actually drives the car
+#   subscribes: can, thermal, model, live20
+#   publishes:  sendcan, live100
+
+# radard -- processes the radar data
+#   subscribes: can, live100, model
+#   publishes:  live20, liveTracks
+
+# sensord -- publishes the IMU and GPS
+#   publishes:  sensorEvents, gpsNMEA
+
+# calibrationd -- places the camera box
+#   subscribes: features, live100
+#   publishes:  liveCalibration
+
+# **** LOGGING SERVICE ****
+
+# loggerd
+#   subscribes: EVERYTHING
+
+# **** NON VITAL SERVICES ****
+
+# ui
+#   subscribes: live100, live20, liveCalibration, model, (raw frames)
+
+# uploader
+#   communicates through file system with loggerd
+
+# logmessaged -- central logging service, can log to cloud
+#   publishes:  logMessage
+
+# logcatd -- fetches logcat info from android
+#   publishes:  androidLog