dragonpilot beta3

date: 2023-12-23T21:19:29
commit: 38612b14f1a8aa49d1c6ef61bd67f5a095abb3f9

common/api/__init__.py

@@ -2,7 +2,7 @@ import jwt
 import os
 import requests
 from datetime import datetime, timedelta
-from openpilot.common.basedir import PERSIST
+from openpilot.system.hardware.hw import Paths
 from openpilot.system.version import get_version
 
 API_HOST = os.getenv('API_HOST', 'https://api.commadotai.com')
@@ -10,7 +10,7 @@ API_HOST = os.getenv('API_HOST', 'https://api.commadotai.com')
 class Api():
   def __init__(self, dongle_id):
     self.dongle_id = dongle_id
-    with open(PERSIST+'/comma/id_rsa') as f:
+    with open(Paths.persist_root()+'/comma/id_rsa') as f:
       self.private_key = f.read()
 
   def get(self, *args, **kwargs):

common/basedir.py

@@ -1,11 +1,4 @@
 import os
-from pathlib import Path
 
-from openpilot.system.hardware import PC
 
 BASEDIR = os.path.abspath(os.path.join(os.path.dirname(os.path.realpath(__file__)), "../"))
-
-if PC:
-  PERSIST = os.path.join(str(Path.home()), ".comma", "persist")
-else:
-  PERSIST = "/persist"

common/file_helpers.py

@@ -4,16 +4,6 @@ import tempfile
 from atomicwrites import AtomicWriter
 
 
-def mkdirs_exists_ok(path):
-  if path.startswith(('http://', 'https://')):
-    raise ValueError('URL path')
-  try:
-    os.makedirs(path)
-  except OSError:
-    if not os.path.isdir(path):
-      raise
-
-
 def rm_not_exists_ok(path):
   try:
     os.remove(path)

common/kalman/simple_kalman.py

@@ -9,4 +9,4 @@ def get_kalman_gain(dt, A, C, Q, R, iterations=100):
     S = C.dot(P).dot(C.T) + R
     K = P.dot(C.T).dot(np.linalg.inv(S))
     P = (np.eye(len(P)) - K.dot(C)).dot(P)
-  return K
+  return K

common/lazy_property.py

@@ -0,0 +1,12 @@
+class lazy_property():
+  """Defines a property whose value will be computed only once and as needed.
+
+     This can only be used on instance methods.
+  """
+  def __init__(self, func):
+    self._func = func
+
+  def __get__(self, obj_self, cls):
+    value = self._func(obj_self)
+    setattr(obj_self, self._func.__name__, value)
+    return value

common/modeldata.h

@@ -1,38 +0,0 @@
-#pragma once
-
-#include <array>
-#include "common/mat.h"
-#include "system/hardware/hw.h"
-
-const int TRAJECTORY_SIZE = 33;
-const int LAT_MPC_N = 16;
-const int LON_MPC_N = 32;
-const float MIN_DRAW_DISTANCE = 10.0;
-const float MAX_DRAW_DISTANCE = 100.0;
-
-const float RYG_GREEN = 0.01165;
-const float RYG_YELLOW = 0.06157;
-
-template <typename T, size_t size>
-constexpr std::array<T, size> build_idxs(float max_val) {
-  std::array<T, size> result{};
-  for (int i = 0; i < size; ++i) {
-    result[i] = max_val * ((i / (double)(size - 1)) * (i / (double)(size - 1)));
-  }
-  return result;
-}
-
-constexpr auto T_IDXS = build_idxs<double, TRAJECTORY_SIZE>(10.0);
-constexpr auto T_IDXS_FLOAT = build_idxs<float, TRAJECTORY_SIZE>(10.0);
-constexpr auto X_IDXS = build_idxs<double, TRAJECTORY_SIZE>(192.0);
-constexpr auto X_IDXS_FLOAT = build_idxs<float, TRAJECTORY_SIZE>(192.0);
-
-const mat3 FCAM_INTRINSIC_MATRIX = (mat3){{2648.0, 0.0, 1928.0 / 2,
-                                           0.0, 2648.0, 1208.0 / 2,
-                                           0.0, 0.0, 1.0}};
-
-// tici ecam focal probably wrong? magnification is not consistent across frame
-// Need to retrain model before this can be changed
-const mat3 ECAM_INTRINSIC_MATRIX = (mat3){{567.0, 0.0, 1928.0 / 2,
-                                           0.0, 567.0, 1208.0 / 2,
-                                           0.0, 0.0, 1.0}};

common/numpy_helpers.py

@@ -0,0 +1,22 @@
+import numpy as np
+
+
+def deep_interp_np(x, xp, fp, axis=None):
+  if axis is not None:
+    fp = fp.swapaxes(0,axis)
+  x = np.atleast_1d(x)
+  xp = np.array(xp)
+  if len(xp) < 2:
+    return np.repeat(fp, len(x), axis=0)
+  if min(np.diff(xp)) < 0:
+    raise RuntimeError('Bad x array for interpolation')
+  j = np.searchsorted(xp, x) - 1
+  j = np.clip(j, 0, len(xp)-2)
+  d = np.divide(x - xp[j], xp[j + 1] - xp[j], out=np.ones_like(x, dtype=np.float64), where=xp[j + 1] - xp[j] != 0)
+  vals_interp = (fp[j].T*(1 - d)).T + (fp[j + 1].T*d).T
+  if axis is not None:
+    vals_interp = vals_interp.swapaxes(0,axis)
+  if len(vals_interp) == 1:
+    return vals_interp[0]
+  else:
+    return vals_interp

common/params.h

@@ -1,21 +1,28 @@
 #pragma once
 
+#include <future>
 #include <map>
 #include <string>
+#include <tuple>
+#include <utility>
 #include <vector>
 
+#include "common/queue.h"
+
 enum ParamKeyType {
   PERSISTENT = 0x02,
   CLEAR_ON_MANAGER_START = 0x04,
   CLEAR_ON_ONROAD_TRANSITION = 0x08,
   CLEAR_ON_OFFROAD_TRANSITION = 0x10,
   DONT_LOG = 0x20,
+  DEVELOPMENT_ONLY = 0x40,
   ALL = 0xFFFFFFFF
 };
 
 class Params {
 public:
   explicit Params(const std::string &path = {});
+  ~Params();
   // Not copyable.
   Params(const Params&) = delete;
   Params& operator=(const Params&) = delete;
@@ -24,7 +31,7 @@ public:
   bool checkKey(const std::string &key);
   ParamKeyType getKeyType(const std::string &key);
   inline std::string getParamPath(const std::string &key = {}) {
-    return params_path + prefix + (key.empty() ? "" : "/" + key);
+    return params_path + params_prefix + (key.empty() ? "" : "/" + key);
   }
 
   // Delete a value
@@ -46,8 +53,18 @@ public:
   inline int putBool(const std::string &key, bool val) {
     return put(key.c_str(), val ? "1" : "0", 1);
   }
+  void putNonBlocking(const std::string &key, const std::string &val);
+  inline void putBoolNonBlocking(const std::string &key, bool val) {
+    putNonBlocking(key, val ? "1" : "0");
+  }
 
 private:
+  void asyncWriteThread();
+
   std::string params_path;
-  std::string prefix;
+  std::string params_prefix;
+
+  // for nonblocking write
+  std::future<void> future;
+  SafeQueue<std::pair<std::string, std::string>> queue;
 };

common/params.py

@@ -1,10 +1,7 @@
-from openpilot.common.params_pyx import Params, ParamKeyType, UnknownKeyName, put_nonblocking, \
-                                        put_bool_nonblocking
+from openpilot.common.params_pyx import Params, ParamKeyType, UnknownKeyName
 assert Params
 assert ParamKeyType
 assert UnknownKeyName
-assert put_nonblocking
-assert put_bool_nonblocking
 
 if __name__ == "__main__":
   import sys

common/params_pyx.pyx

@@ -3,7 +3,6 @@
 from libcpp cimport bool
 from libcpp.string cimport string
 from libcpp.vector cimport vector
-import threading
 
 cdef extern from "common/params.h":
   cpdef enum ParamKeyType:
@@ -11,6 +10,7 @@ cdef extern from "common/params.h":
     CLEAR_ON_MANAGER_START
     CLEAR_ON_ONROAD_TRANSITION
     CLEAR_ON_OFFROAD_TRANSITION
+    DEVELOPMENT_ONLY
     ALL
 
   cdef cppclass c_Params "Params":
@@ -19,6 +19,8 @@ cdef extern from "common/params.h":
     bool getBool(string, bool) nogil
     int remove(string) nogil
     int put(string, string) nogil
+    void putNonBlocking(string, string) nogil
+    void putBoolNonBlocking(string, bool) nogil
     int putBool(string, bool) nogil
     bool checkKey(string) nogil
     string getParamPath(string) nogil
@@ -79,7 +81,7 @@ cdef class Params:
     """
     Warning: This function blocks until the param is written to disk!
     In very rare cases this can take over a second, and your code will hang.
-    Use the put_nonblocking helper function in time sensitive code, but
+    Use the put_nonblocking, put_bool_nonblocking in time sensitive code, but
     in general try to avoid writing params as much as possible.
     """
     cdef string k = self.check_key(key)
@@ -92,6 +94,17 @@ cdef class Params:
     with nogil:
       self.p.putBool(k, val)
 
+  def put_nonblocking(self, key, dat):
+    cdef string k = self.check_key(key)
+    cdef string dat_bytes = ensure_bytes(dat)
+    with nogil:
+      self.p.putNonBlocking(k, dat_bytes)
+
+  def put_bool_nonblocking(self, key, bool val):
+    cdef string k = self.check_key(key)
+    with nogil:
+      self.p.putBoolNonBlocking(k, val)
+
   def remove(self, key):
     cdef string k = self.check_key(key)
     with nogil:
@@ -103,9 +116,3 @@ cdef class Params:
 
   def all_keys(self):
     return self.p.allKeys()
-
-def put_nonblocking(key, val, d=""):
-  threading.Thread(target=lambda: Params(d).put(key, val)).start()
-
-def put_bool_nonblocking(key, bool val, d=""):
-  threading.Thread(target=lambda: Params(d).put_bool(key, val)).start()

common/prefix.py

@@ -0,0 +1,46 @@
+import os
+import shutil
+import uuid
+
+from typing import Optional
+
+from openpilot.common.params import Params
+from openpilot.system.hardware.hw import Paths
+
+class OpenpilotPrefix:
+  def __init__(self, prefix: Optional[str] = None, clean_dirs_on_exit: bool = True):
+    self.prefix = prefix if prefix else str(uuid.uuid4().hex[0:15])
+    self.msgq_path = os.path.join('/dev/shm', self.prefix)
+    self.clean_dirs_on_exit = clean_dirs_on_exit
+
+  def __enter__(self):
+    self.original_prefix = os.environ.get('OPENPILOT_PREFIX', None)
+    os.environ['OPENPILOT_PREFIX'] = self.prefix
+    try:
+      os.mkdir(self.msgq_path)
+    except FileExistsError:
+      pass
+    os.makedirs(Paths.log_root(), exist_ok=True)
+
+    return self
+
+  def __exit__(self, exc_type, exc_obj, exc_tb):
+    if self.clean_dirs_on_exit:
+      self.clean_dirs()
+    try:
+      del os.environ['OPENPILOT_PREFIX']
+      if self.original_prefix is not None:
+        os.environ['OPENPILOT_PREFIX'] = self.original_prefix
+    except KeyError:
+      pass
+    return False
+
+  def clean_dirs(self):
+    symlink_path = Params().get_param_path()
+    if os.path.exists(symlink_path):
+      shutil.rmtree(os.path.realpath(symlink_path), ignore_errors=True)
+      os.remove(symlink_path)
+    shutil.rmtree(self.msgq_path, ignore_errors=True)
+    shutil.rmtree(Paths.log_root(), ignore_errors=True)
+    shutil.rmtree(Paths.download_cache_root(), ignore_errors=True)
+    shutil.rmtree(Paths.comma_home(), ignore_errors=True)

common/retry.py

@@ -0,0 +1,30 @@
+import time
+import functools
+
+from openpilot.common.swaglog import cloudlog
+
+
+def retry(attempts=3, delay=1.0, ignore_failure=False):
+  def decorator(func):
+    @functools.wraps(func)
+    def wrapper(*args, **kwargs):
+      for _ in range(attempts):
+        try:
+          return func(*args, **kwargs)
+        except Exception:
+          cloudlog.exception(f"{func.__name__} failed, trying again")
+          time.sleep(delay)
+
+      if ignore_failure:
+        cloudlog.error(f"{func.__name__} failed after retry")
+      else:
+        raise Exception(f"{func.__name__} failed after retry")
+    return wrapper
+  return decorator
+
+
+if __name__ == "__main__":
+  @retry(attempts=10)
+  def abc():
+    raise ValueError("abc failed :(")
+  abc()

common/swaglog.py

@@ -0,0 +1,135 @@
+import logging
+import os
+import time
+import warnings
+from pathlib import Path
+from logging.handlers import BaseRotatingHandler
+
+import zmq
+
+from openpilot.common.logging_extra import SwagLogger, SwagFormatter, SwagLogFileFormatter
+from openpilot.system.hardware.hw import Paths
+
+
+def get_file_handler():
+  Path(Paths.swaglog_root()).mkdir(parents=True, exist_ok=True)
+  base_filename = os.path.join(Paths.swaglog_root(), "swaglog")
+  handler = SwaglogRotatingFileHandler(base_filename)
+  return handler
+
+class SwaglogRotatingFileHandler(BaseRotatingHandler):
+  def __init__(self, base_filename, interval=60, max_bytes=1024*256, backup_count=2500, encoding=None):
+    super().__init__(base_filename, mode="a", encoding=encoding, delay=True)
+    self.base_filename = base_filename
+    self.interval = interval # seconds
+    self.max_bytes = max_bytes
+    self.backup_count = backup_count
+    self.log_files = self.get_existing_logfiles()
+    log_indexes = [f.split(".")[-1] for f in self.log_files]
+    self.last_file_idx = max([int(i) for i in log_indexes if i.isdigit()] or [-1])
+    self.last_rollover = None
+    self.doRollover()
+
+  def _open(self):
+    self.last_rollover = time.monotonic()
+    self.last_file_idx += 1
+    next_filename = f"{self.base_filename}.{self.last_file_idx:010}"
+    stream = open(next_filename, self.mode, encoding=self.encoding)
+    self.log_files.insert(0, next_filename)
+    return stream
+
+  def get_existing_logfiles(self):
+    log_files = list()
+    base_dir = os.path.dirname(self.base_filename)
+    for fn in os.listdir(base_dir):
+      fp = os.path.join(base_dir, fn)
+      if fp.startswith(self.base_filename) and os.path.isfile(fp):
+        log_files.append(fp)
+    return sorted(log_files)
+
+  def shouldRollover(self, record):
+    size_exceeded = self.max_bytes > 0 and self.stream.tell() >= self.max_bytes
+    time_exceeded = self.interval > 0 and self.last_rollover + self.interval <= time.monotonic()
+    return size_exceeded or time_exceeded
+
+  def doRollover(self):
+    if self.stream:
+      self.stream.close()
+    self.stream = self._open()
+
+    if self.backup_count > 0:
+      while len(self.log_files) > self.backup_count:
+        to_delete = self.log_files.pop()
+        if os.path.exists(to_delete): # just being safe, should always exist
+          os.remove(to_delete)
+
+class UnixDomainSocketHandler(logging.Handler):
+  def __init__(self, formatter):
+    logging.Handler.__init__(self)
+    self.setFormatter(formatter)
+    self.pid = None
+
+    self.zctx = None
+    self.sock = None
+
+  def __del__(self):
+    self.close()
+
+  def close(self):
+    if self.sock is not None:
+      self.sock.close()
+    if self.zctx is not None:
+      self.zctx.term()
+
+  def connect(self):
+    self.zctx = zmq.Context()
+    self.sock = self.zctx.socket(zmq.PUSH)
+    self.sock.setsockopt(zmq.LINGER, 10)
+    self.sock.connect(Paths.swaglog_ipc())
+    self.pid = os.getpid()
+
+  def emit(self, record):
+    if os.getpid() != self.pid:
+      # TODO suppresses warning about forking proc with zmq socket, fix root cause
+      warnings.filterwarnings("ignore", category=ResourceWarning, message="unclosed.*<zmq.*>")
+      self.connect()
+
+    msg = self.format(record).rstrip('\n')
+    # print("SEND".format(repr(msg)))
+    try:
+      s = chr(record.levelno)+msg
+      self.sock.send(s.encode('utf8'), zmq.NOBLOCK)
+    except zmq.error.Again:
+      # drop :/
+      pass
+
+
+def add_file_handler(log):
+  """
+  Function to add the file log handler to swaglog.
+  This can be used to store logs when logmessaged is not running.
+  """
+  handler = get_file_handler()
+  handler.setFormatter(SwagLogFileFormatter(log))
+  log.addHandler(handler)
+
+
+cloudlog = log = SwagLogger()
+log.setLevel(logging.DEBUG)
+
+
+outhandler = logging.StreamHandler()
+
+print_level = os.environ.get('LOGPRINT', 'warning')
+if print_level == 'debug':
+  outhandler.setLevel(logging.DEBUG)
+elif print_level == 'info':
+  outhandler.setLevel(logging.INFO)
+elif print_level == 'warning':
+  outhandler.setLevel(logging.WARNING)
+
+ipchandler = UnixDomainSocketHandler(SwagFormatter(log))
+
+log.addHandler(outhandler)
+# logs are sent through IPC before writing to disk to prevent disk I/O blocking
+log.addHandler(ipchandler)

common/time.py

@@ -3,4 +3,4 @@ import datetime
 MIN_DATE = datetime.datetime(year=2023, month=6, day=1)
 
 def system_time_valid():
-  return datetime.datetime.now() > MIN_DATE
+  return datetime.datetime.now() > MIN_DATE

common/version.h

@@ -1 +1 @@
-#define COMMA_VERSION "2023.11.20"
+#define COMMA_VERSION "2023.12.23"

common/window.py

@@ -0,0 +1,61 @@
+import sys
+import pygame
+import cv2
+
+class Window:
+  def __init__(self, w, h, caption="window", double=False, halve=False):
+    self.w = w
+    self.h = h
+    pygame.display.init()
+    pygame.display.set_caption(caption)
+    self.double = double
+    self.halve = halve
+    if self.double:
+      self.rw, self.rh = w*2, h*2
+    elif self.halve:
+      self.rw, self.rh = w//2, h//2
+    else:
+      self.rw, self.rh = w, h
+    self.screen = pygame.display.set_mode((self.rw, self.rh))
+    pygame.display.flip()
+
+    # hack for xmonad, it shrinks the window by 6 pixels after the display.flip
+    if self.screen.get_width() != self.rw:
+      self.screen = pygame.display.set_mode((self.rw+(self.rw-self.screen.get_width()), self.rh+(self.rh-self.screen.get_height())))
+      pygame.display.flip()
+
+  def draw(self, out):
+    pygame.event.pump()
+    if self.double:
+      out2 = cv2.resize(out, (self.w*2, self.h*2))
+      pygame.surfarray.blit_array(self.screen, out2.swapaxes(0, 1))
+    elif self.halve:
+      out2 = cv2.resize(out, (self.w//2, self.h//2))
+      pygame.surfarray.blit_array(self.screen, out2.swapaxes(0, 1))
+    else:
+      pygame.surfarray.blit_array(self.screen, out.swapaxes(0, 1))
+    pygame.display.flip()
+
+  def getkey(self):
+    while 1:
+      event = pygame.event.wait()
+      if event.type == pygame.QUIT:
+        pygame.quit()
+        sys.exit()
+      if event.type == pygame.KEYDOWN:
+        return event.key
+
+  def getclick(self):
+    for event in pygame.event.get():
+      if event.type == pygame.MOUSEBUTTONDOWN:
+        mx, my = pygame.mouse.get_pos()
+        return mx, my
+
+if __name__ == "__main__":
+  import numpy as np
+  win = Window(200, 200, double=True)
+  img: np.ndarray = np.zeros((200, 200, 3), np.uint8)
+  while 1:
+    print("draw")
+    img += 1
+    win.draw(img)