openpilot v0.9.7 release

date: 2024-06-11T01:36:39
master commit: f8cb04e4a8

common/api/__init__.py

@@ -7,7 +7,7 @@ from openpilot.system.version import get_version
 
 API_HOST = os.getenv('API_HOST', 'https://api.commadotai.com')
 
-class Api():
+class Api:
   def __init__(self, dongle_id):
     self.dongle_id = dongle_id
     with open(Paths.persist_root()+'/comma/id_rsa') as f:

common/file_helpers.py

@@ -1,7 +1,6 @@
 import os
 import tempfile
 import contextlib
-from typing import Optional
 
 
 class CallbackReader:
@@ -24,7 +23,7 @@ class CallbackReader:
 
 
 @contextlib.contextmanager
-def atomic_write_in_dir(path: str, mode: str = 'w', buffering: int = -1, encoding: Optional[str] = None, newline: Optional[str] = None,
+def atomic_write_in_dir(path: str, mode: str = 'w', buffering: int = -1, encoding: str = None, newline: str = None,
                         overwrite: bool = False):
   """Write to a file atomically using a temporary file in the same directory as the destination file."""
   dir_name = os.path.dirname(path)

common/git.py

@@ -0,0 +1,42 @@
+from functools import cache
+import subprocess
+from openpilot.common.run import run_cmd, run_cmd_default
+
+
+@cache
+def get_commit(cwd: str = None, branch: str = "HEAD") -> str:
+  return run_cmd_default(["git", "rev-parse", branch], cwd=cwd)
+
+
+@cache
+def get_commit_date(cwd: str = None, commit: str = "HEAD") -> str:
+  return run_cmd_default(["git", "show", "--no-patch", "--format='%ct %ci'", commit], cwd=cwd)
+
+
+@cache
+def get_short_branch(cwd: str = None) -> str:
+  return run_cmd_default(["git", "rev-parse", "--abbrev-ref", "HEAD"], cwd=cwd)
+
+
+@cache
+def get_branch(cwd: str = None) -> str:
+  return run_cmd_default(["git", "rev-parse", "--abbrev-ref", "--symbolic-full-name", "@{u}"], cwd=cwd)
+
+
+@cache
+def get_origin(cwd: str = None) -> str:
+  try:
+    local_branch = run_cmd(["git", "name-rev", "--name-only", "HEAD"], cwd=cwd)
+    tracking_remote = run_cmd(["git", "config", "branch." + local_branch + ".remote"], cwd=cwd)
+    return run_cmd(["git", "config", "remote." + tracking_remote + ".url"], cwd=cwd)
+  except subprocess.CalledProcessError:  # Not on a branch, fallback
+    return run_cmd_default(["git", "config", "--get", "remote.origin.url"], cwd=cwd)
+
+
+@cache
+def get_normalized_origin(cwd: str = None) -> str:
+  return get_origin(cwd) \
+    .replace("git@", "", 1) \
+    .replace(".git", "", 1) \
+    .replace("https://", "", 1) \
+    .replace(":", "/", 1)

common/gpio.py

@@ -1,6 +1,5 @@
 import os
-from functools import lru_cache
-from typing import Optional, List
+from functools import cache
 
 def gpio_init(pin: int, output: bool) -> None:
   try:
@@ -16,7 +15,7 @@ def gpio_set(pin: int, high: bool) -> None:
   except Exception as e:
     print(f"Failed to set gpio {pin} value: {e}")
 
-def gpio_read(pin: int) -> Optional[bool]:
+def gpio_read(pin: int) -> bool | None:
   val = None
   try:
     with open(f"/sys/class/gpio/gpio{pin}/value", 'rb') as f:
@@ -36,8 +35,8 @@ def gpio_export(pin: int) -> None:
   except Exception:
     print(f"Failed to export gpio {pin}")
 
-@lru_cache(maxsize=None)
-def get_irq_action(irq: int) -> List[str]:
+@cache
+def get_irq_action(irq: int) -> list[str]:
   try:
     with open(f"/sys/kernel/irq/{irq}/actions") as f:
       actions = f.read().strip().split(',')
@@ -45,7 +44,7 @@ def get_irq_action(irq: int) -> List[str]:
   except FileNotFoundError:
     return []
 
-def get_irqs_for_action(action: str) -> List[str]:
+def get_irqs_for_action(action: str) -> list[str]:
   ret = []
   with open("/proc/interrupts") as f:
     for l in f.readlines():

common/markdown.py

@@ -0,0 +1,45 @@
+HTML_REPLACEMENTS = [
+  (r'&', r'&'),
+  (r'"', r'"'),
+]
+
+def parse_markdown(text: str, tab_length: int = 2) -> str:
+  lines = text.split("\n")
+  output: list[str] = []
+  list_level = 0
+
+  def end_outstanding_lists(level: int, end_level: int) -> int:
+    while level > end_level:
+      level -= 1
+      output.append("</ul>")
+      if level > 0:
+        output.append("</li>")
+    return end_level
+
+  for i, line in enumerate(lines):
+    if i + 1 < len(lines) and lines[i + 1].startswith("==="):  # heading
+      output.append(f"<h1>{line}</h1>")
+    elif line.startswith("==="):
+      pass
+    elif line.lstrip().startswith("* "):  # list
+      line_level = 1 + line.count(" " * tab_length, 0, line.index("*"))
+      if list_level >= line_level:
+        list_level = end_outstanding_lists(list_level, line_level)
+      else:
+        list_level += 1
+        if list_level > 1:
+          output[-1] = output[-1].replace("</li>", "")
+        output.append("<ul>")
+      output.append(f"<li>{line.replace('*', '', 1).lstrip()}</li>")
+    else:
+      list_level = end_outstanding_lists(list_level, 0)
+      if len(line) > 0:
+        output.append(line)
+
+  end_outstanding_lists(list_level, 0)
+  output_str = "\n".join(output) + "\n"
+
+  for (fr, to) in HTML_REPLACEMENTS:
+    output_str = output_str.replace(fr, to)
+
+  return output_str

common/mock/__init__.py

@@ -0,0 +1,50 @@
+"""
+Utilities for generating mock messages for testing.
+example in common/tests/test_mock.py
+"""
+
+
+import functools
+import threading
+from cereal.messaging import PubMaster
+from cereal.services import SERVICE_LIST
+from openpilot.common.mock.generators import generate_liveLocationKalman
+from openpilot.common.realtime import Ratekeeper
+
+
+MOCK_GENERATOR = {
+  "liveLocationKalman": generate_liveLocationKalman
+}
+
+
+def generate_messages_loop(services: list[str], done: threading.Event):
+  pm = PubMaster(services)
+  rk = Ratekeeper(100)
+  i = 0
+  while not done.is_set():
+    for s in services:
+      should_send = i % (100/SERVICE_LIST[s].frequency) == 0
+      if should_send:
+        message = MOCK_GENERATOR[s]()
+        pm.send(s, message)
+    i += 1
+    rk.keep_time()
+
+
+def mock_messages(services: list[str] | str):
+  if isinstance(services, str):
+    services = [services]
+
+  def decorator(func):
+    @functools.wraps(func)
+    def wrapper(*args, **kwargs):
+      done = threading.Event()
+      t = threading.Thread(target=generate_messages_loop, args=(services, done))
+      t.start()
+      try:
+        return func(*args, **kwargs)
+      finally:
+        done.set()
+        t.join()
+    return wrapper
+  return decorator

common/mock/generators.py

@@ -0,0 +1,20 @@
+from cereal import messaging
+
+
+LOCATION1 = (32.7174, -117.16277)
+LOCATION2 = (32.7558, -117.2037)
+
+LLK_DECIMATION = 10
+RENDER_FRAMES = 15
+DEFAULT_ITERATIONS = RENDER_FRAMES * LLK_DECIMATION
+
+
+def generate_liveLocationKalman(location=LOCATION1):
+  msg = messaging.new_message('liveLocationKalman')
+  msg.liveLocationKalman.positionGeodetic = {'value': [*location, 0], 'std': [0., 0., 0.], 'valid': True}
+  msg.liveLocationKalman.positionECEF = {'value': [0., 0., 0.], 'std': [0., 0., 0.], 'valid': True}
+  msg.liveLocationKalman.calibratedOrientationNED = {'value': [0., 0., 0.], 'std': [0., 0., 0.], 'valid': True}
+  msg.liveLocationKalman.velocityCalibrated = {'value': [0., 0., 0.], 'std': [0., 0., 0.], 'valid': True}
+  msg.liveLocationKalman.status = 'valid'
+  msg.liveLocationKalman.gpsOK = True
+  return msg

common/params.cc

@@ -89,6 +89,7 @@ private:
 
 std::unordered_map<std::string, uint32_t> keys = {
     {"AccessToken", CLEAR_ON_MANAGER_START | DONT_LOG},
+    {"AlwaysOnDM", PERSISTENT},
     {"ApiCache_Device", PERSISTENT},
     {"ApiCache_NavDestinations", PERSISTENT},
     {"AssistNowToken", PERSISTENT},
@@ -171,7 +172,6 @@ std::unordered_map<std::string, uint32_t> keys = {
     {"Offroad_CarUnrecognized", CLEAR_ON_MANAGER_START | CLEAR_ON_ONROAD_TRANSITION},
     {"Offroad_ConnectivityNeeded", CLEAR_ON_MANAGER_START},
     {"Offroad_ConnectivityNeededPrompt", CLEAR_ON_MANAGER_START},
-    {"Offroad_InvalidTime", CLEAR_ON_MANAGER_START},
     {"Offroad_IsTakingSnapshot", CLEAR_ON_MANAGER_START},
     {"Offroad_NeosUpdate", CLEAR_ON_MANAGER_START},
     {"Offroad_NoFirmware", CLEAR_ON_MANAGER_START | CLEAR_ON_ONROAD_TRANSITION},
@@ -188,6 +188,7 @@ std::unordered_map<std::string, uint32_t> keys = {
     {"RecordFront", PERSISTENT},
     {"RecordFrontLock", PERSISTENT},  // for the internal fleet
     {"ReplayControlsState", CLEAR_ON_MANAGER_START | CLEAR_ON_ONROAD_TRANSITION},
+    {"RouteCount", PERSISTENT},
     {"SnoozeUpdate", CLEAR_ON_MANAGER_START | CLEAR_ON_OFFROAD_TRANSITION},
     {"SshEnabled", PERSISTENT},
     {"TermsVersion", PERSISTENT},
@@ -206,7 +207,6 @@ std::unordered_map<std::string, uint32_t> keys = {
     {"UpdaterTargetBranch", CLEAR_ON_MANAGER_START},
     {"UpdaterLastFetchTime", PERSISTENT},
     {"Version", PERSISTENT},
-    {"VisionRadarToggle", PERSISTENT},
 };
 
 } // namespace
@@ -273,7 +273,9 @@ int Params::put(const char* key, const char* value, size_t value_size) {
   } while (false);
 
   close(tmp_fd);
-  ::unlink(tmp_path.c_str());
+  if (result != 0) {
+    ::unlink(tmp_path.c_str());
+  }
   return result;
 }
 

common/prefix.h

@@ -5,6 +5,7 @@
 
 #include "common/params.h"
 #include "common/util.h"
+#include "system/hardware/hw.h"
 
 class OpenpilotPrefix {
 public:
@@ -25,6 +26,10 @@ public:
       system(util::string_format("rm %s -rf", real_path.c_str()).c_str());
       unlink(param_path.c_str());
     }
+    if (getenv("COMMA_CACHE") == nullptr) {
+      system(util::string_format("rm %s -rf", Path::download_cache_root().c_str()).c_str());
+    }
+    system(util::string_format("rm %s -rf", Path::comma_home().c_str()).c_str());
     system(util::string_format("rm %s -rf", msgq_path.c_str()).c_str());
     unsetenv("OPENPILOT_PREFIX");
   }

common/prefix.py

@@ -2,14 +2,14 @@ import os
 import shutil
 import uuid
 
-from typing import Optional
 
 from openpilot.common.params import Params
+from openpilot.system.hardware import PC
 from openpilot.system.hardware.hw import Paths
 from openpilot.system.hardware.hw import DEFAULT_DOWNLOAD_CACHE_ROOT
 
 class OpenpilotPrefix:
-  def __init__(self, prefix: Optional[str] = None, clean_dirs_on_exit: bool = True, shared_download_cache: bool = False):
+  def __init__(self, prefix: str = None, clean_dirs_on_exit: bool = True, shared_download_cache: bool = False):
     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
@@ -46,7 +46,8 @@ class OpenpilotPrefix:
       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)
+    if PC:
+      shutil.rmtree(Paths.log_root(), ignore_errors=True)
     if not os.environ.get("COMMA_CACHE", False):
       shutil.rmtree(Paths.download_cache_root(), ignore_errors=True)
     shutil.rmtree(Paths.comma_home(), ignore_errors=True)

common/ratekeeper.h

@@ -9,7 +9,7 @@ public:
   ~RateKeeper() {}
   bool keepTime();
   bool monitorTime();
-  inline double frame() const { return frame_; }
+  inline uint64_t frame() const { return frame_; }
   inline double remaining() const { return remaining_; }
 
 private:

common/realtime.py

@@ -3,7 +3,6 @@ import gc
 import os
 import time
 from collections import deque
-from typing import Optional, List, Union
 
 from setproctitle import getproctitle
 
@@ -13,7 +12,7 @@ from openpilot.system.hardware import PC
 # time step for each process
 DT_CTRL = 0.01  # controlsd
 DT_MDL = 0.05  # model
-DT_TRML = 0.5  # thermald and manager
+DT_HW = 0.5  # hardwared and manager
 DT_DMON = 0.05  # driver monitoring
 
 
@@ -24,7 +23,7 @@ class Priority:
   CTRL_LOW = 51 # plannerd & radard
 
   # CORE 3
-  # - boardd = 55
+  # - pandad = 55
   CTRL_HIGH = 53
 
 
@@ -33,12 +32,12 @@ def set_realtime_priority(level: int) -> None:
     os.sched_setscheduler(0, os.SCHED_FIFO, os.sched_param(level))
 
 
-def set_core_affinity(cores: List[int]) -> None:
+def set_core_affinity(cores: list[int]) -> None:
   if not PC:
     os.sched_setaffinity(0, cores)
 
 
-def config_realtime_process(cores: Union[int, List[int]], priority: int) -> None:
+def config_realtime_process(cores: int | list[int], priority: int) -> None:
   gc.disable()
   set_realtime_priority(priority)
   c = cores if isinstance(cores, list) else [cores, ]
@@ -46,7 +45,7 @@ def config_realtime_process(cores: Union[int, List[int]], priority: int) -> None
 
 
 class Ratekeeper:
-  def __init__(self, rate: float, print_delay_threshold: Optional[float] = 0.0) -> None:
+  def __init__(self, rate: float, print_delay_threshold: float | None = 0.0) -> None:
     """Rate in Hz for ratekeeping. print_delay_threshold must be nonnegative."""
     self._interval = 1. / rate
     self._next_frame_time = time.monotonic() + self._interval

common/run.py

@@ -0,0 +1,13 @@
+import subprocess
+
+
+def run_cmd(cmd: list[str], cwd=None, env=None) -> str:
+  return subprocess.check_output(cmd, encoding='utf8', cwd=cwd, env=env).strip()
+
+
+def run_cmd_default(cmd: list[str], default: str = "", cwd=None, env=None) -> str:
+  try:
+    return run_cmd(cmd, cwd=cwd, env=env)
+  except subprocess.CalledProcessError:
+    return default
+

common/spinner.py

@@ -3,7 +3,7 @@ import subprocess
 from openpilot.common.basedir import BASEDIR
 
 
-class Spinner():
+class Spinner:
   def __init__(self):
     try:
       self.spinner_proc = subprocess.Popen(["./spinner"],

common/stat_live.py

@@ -1,6 +1,6 @@
 import numpy as np
 
-class RunningStat():
+class RunningStat:
   # tracks realtime mean and standard deviation without storing any data
   def __init__(self, priors=None, max_trackable=-1):
     self.max_trackable = max_trackable
@@ -51,7 +51,7 @@ class RunningStat():
   def params_to_save(self):
     return [self.M, self.S, self.n]
 
-class RunningStatFilter():
+class RunningStatFilter:
   def __init__(self, raw_priors=None, filtered_priors=None, max_trackable=-1):
     self.raw_stat = RunningStat(raw_priors, -1)
     self.filtered_stat = RunningStat(filtered_priors, max_trackable)

common/time.py

@@ -1,6 +1,15 @@
 import datetime
+from pathlib import Path
 
-MIN_DATE = datetime.datetime(year=2024, month=1, day=28)
+_MIN_DATE = datetime.datetime(year=2024, month=3, day=30)
+
+def min_date():
+  # on systemd systems, the default time is the systemd build time
+  systemd_path = Path("/lib/systemd/systemd")
+  if systemd_path.exists():
+    d = datetime.datetime.fromtimestamp(systemd_path.stat().st_mtime)
+    return d + datetime.timedelta(days=1)
+  return _MIN_DATE
 
 def system_time_valid():
-  return datetime.datetime.now() > MIN_DATE
+  return datetime.datetime.now() > min_date()

common/transformations/.gitignore

@@ -0,0 +1,2 @@
+transformations
+transformations.cpp

common/transformations/README.md

@@ -0,0 +1,70 @@
+
+Reference Frames
+------
+Many reference frames are used throughout. This
+folder contains all helper functions needed to
+transform between them. Generally this is done
+by generating a rotation matrix and multiplying.
+
+
+| Name | [x, y, z] | Units | Notes |
+| :-------------: |:-------------:| :-----:| :----: |
+| Geodetic | [Latitude, Longitude, Altitude] | geodetic coordinates | Sometimes used as [lon, lat, alt], avoid this frame. |
+| ECEF | [x, y, z] | meters | We use **ITRF14 (IGS14)**, NOT NAD83. <br> This is the global Mesh3D frame. |
+| NED | [North, East, Down] | meters | Relative to earth's surface, useful for visualizing. |
+| Device | [Forward, Right, Down] | meters | This is the Mesh3D local frame. <br> Relative to camera, **not imu.** <br> ![img](http://upload.wikimedia.org/wikipedia/commons/thumb/2/2f/RPY_angles_of_airplanes.png/440px-RPY_angles_of_airplanes.png)|
+| Calibrated | [Forward, Right, Down] | meters | This is the frame the model outputs are in. <br> More details below. <br>|
+| Car | [Forward, Right, Down] | meters | This is useful for estimating position of points on the road. <br> More details below. <br>|
+| View | [Right, Down, Forward] | meters | Like device frame, but according to camera conventions. |
+| Camera | [u, v, focal] | pixels | Like view frame, but 2d on the camera image.|
+| Normalized Camera | [u / focal, v / focal, 1] | / | |
+| Model | [u, v, focal] | pixels | The sampled rectangle of the full camera frame the model uses. |
+| Normalized Model | [u / focal, v / focal, 1] | / | |
+
+
+
+
+Orientation Conventions
+------
+Quaternions, rotation matrices and euler angles are three
+equivalent representations of orientation and all three are
+used throughout the code base.
+
+For euler angles the preferred convention is [roll, pitch, yaw]
+which corresponds to rotations around the [x, y, z] axes. All
+euler angles should always be in radians or radians/s unless
+for plotting or display purposes. For quaternions the hamilton
+notations is preferred which is [q<sub>w</sub>, q<sub>x</sub>, q<sub>y</sub>, q<sub>z</sub>]. All quaternions
+should always be normalized with a strictly positive q<sub>w</sub>. **These
+quaternions are a unique representation of orientation whereas euler angles
+or rotation matrices are not.**
+
+To rotate from one frame into another with euler angles the
+convention is to rotate around roll, then pitch and then yaw,
+while rotating around the rotated axes, not the original axes.
+
+
+Car frame
+------
+Device frame is aligned with the road-facing camera used by openpilot. However, when controlling the vehicle it is helpful to think in a reference frame aligned with the vehicle. These two reference frames can be different.
+
+The orientation of car frame is defined to be aligned with the car's direction of travel and the road plane when the vehicle is driving on a flat road and not turning. The origin of car frame is defined to be directly below device frame (in car frame), such that it is on the road plane. The position and orientation of this frame is not necessarily always aligned with the direction of travel or the road plane due to suspension movements and other effects.
+
+
+Calibrated frame
+------
+It is helpful for openpilot's driving model to take in images that look similar when mounted differently in different cars. To achieve this we "calibrate" the images by transforming it into calibrated frame. Calibrated frame is defined to be aligned with car frame in pitch and yaw, and aligned with device frame in roll. It also has the same origin as device frame.
+
+
+Example
+------
+To transform global Mesh3D positions and orientations (positions_ecef, quats_ecef) into the local frame described by the
+first position and orientation from Mesh3D one would do:
+```
+ecef_from_local = rot_from_quat(quats_ecef[0])
+local_from_ecef = ecef_from_local.T
+positions_local = np.einsum('ij,kj->ki', local_from_ecef, postions_ecef - positions_ecef[0])
+rotations_global = rot_from_quat(quats_ecef)
+rotations_local = np.einsum('ij,kjl->kil', local_from_ecef, rotations_global)
+eulers_local = euler_from_rot(rotations_local)
+```

common/transformations/camera.py

@@ -1,55 +1,74 @@
+import itertools
 import numpy as np
+from dataclasses import dataclass
 
 import openpilot.common.transformations.orientation as orient
 
 ## -- hardcoded hardware params --
-eon_f_focal_length = 910.0
-eon_d_focal_length = 650.0
-tici_f_focal_length = 2648.0
-tici_e_focal_length = tici_d_focal_length = 567.0 # probably wrong? magnification is not consistent across frame
+@dataclass(frozen=True)
+class CameraConfig:
+  width: int
+  height: int
+  focal_length: float
 
-eon_f_frame_size = (1164, 874)
-eon_d_frame_size = (816, 612)
-tici_f_frame_size = tici_e_frame_size = tici_d_frame_size = (1928, 1208)
+  @property
+  def size(self):
+    return (self.width, self.height)
 
-# aka 'K' aka camera_frame_from_view_frame
-eon_fcam_intrinsics = np.array([
-  [eon_f_focal_length,  0.0,  float(eon_f_frame_size[0])/2],
-  [0.0,  eon_f_focal_length,  float(eon_f_frame_size[1])/2],
-  [0.0,  0.0,                                          1.0]])
-eon_intrinsics = eon_fcam_intrinsics # xx
+  @property
+  def intrinsics(self):
+    # aka 'K' aka camera_frame_from_view_frame
+    return np.array([
+      [self.focal_length,  0.0, float(self.width)/2],
+      [0.0, self.focal_length, float(self.height)/2],
+      [0.0,  0.0, 1.0]
+    ])
 
-eon_dcam_intrinsics = np.array([
-  [eon_d_focal_length,  0.0,  float(eon_d_frame_size[0])/2],
-  [0.0,  eon_d_focal_length,  float(eon_d_frame_size[1])/2],
-  [0.0,  0.0,                                          1.0]])
+  @property
+  def intrinsics_inv(self):
+    # aka 'K_inv' aka view_frame_from_camera_frame
+    return np.linalg.inv(self.intrinsics)
 
-tici_fcam_intrinsics = np.array([
-  [tici_f_focal_length,  0.0,  float(tici_f_frame_size[0])/2],
-  [0.0,  tici_f_focal_length,  float(tici_f_frame_size[1])/2],
-  [0.0,  0.0,                                            1.0]])
+@dataclass(frozen=True)
+class _NoneCameraConfig(CameraConfig):
+  width: int = 0
+  height: int = 0
+  focal_length: float = 0
 
-tici_dcam_intrinsics = np.array([
-  [tici_d_focal_length,  0.0,  float(tici_d_frame_size[0])/2],
-  [0.0,  tici_d_focal_length,  float(tici_d_frame_size[1])/2],
-  [0.0,  0.0,                                            1.0]])
+@dataclass(frozen=True)
+class DeviceCameraConfig:
+  fcam: CameraConfig
+  dcam: CameraConfig
+  ecam: CameraConfig
 
-tici_ecam_intrinsics = tici_dcam_intrinsics
+  def all_cams(self):
+    for cam in ['fcam', 'dcam', 'ecam']:
+      if not isinstance(getattr(self, cam), _NoneCameraConfig):
+        yield cam, getattr(self, cam)
 
-# aka 'K_inv' aka view_frame_from_camera_frame
-eon_fcam_intrinsics_inv = np.linalg.inv(eon_fcam_intrinsics)
-eon_intrinsics_inv = eon_fcam_intrinsics_inv # xx
+_ar_ox_fisheye = CameraConfig(1928, 1208, 567.0)  # focal length probably wrong? magnification is not consistent across frame
+_os_fisheye = CameraConfig(2688, 1520, 567.0 / 2 * 3)
+_ar_ox_config = DeviceCameraConfig(CameraConfig(1928, 1208, 2648.0), _ar_ox_fisheye, _ar_ox_fisheye)
+_os_config = DeviceCameraConfig(CameraConfig(2688, 1520, 2648.0 * 2 / 3), _os_fisheye, _os_fisheye)
+_neo_config = DeviceCameraConfig(CameraConfig(1164, 874, 910.0), CameraConfig(816, 612, 650.0), _NoneCameraConfig())
 
-tici_fcam_intrinsics_inv = np.linalg.inv(tici_fcam_intrinsics)
-tici_ecam_intrinsics_inv = np.linalg.inv(tici_ecam_intrinsics)
+DEVICE_CAMERAS = {
+  # A "device camera" is defined by a device type and sensor
 
+  # sensor type was never set on eon/neo/two
+  ("neo", "unknown"): _neo_config,
+  # unknown here is AR0231, field was added with OX03C10 support
+  ("tici", "unknown"): _ar_ox_config,
 
-FULL_FRAME_SIZE = tici_f_frame_size
-FOCAL = tici_f_focal_length
-fcam_intrinsics = tici_fcam_intrinsics
-
-W, H = FULL_FRAME_SIZE[0], FULL_FRAME_SIZE[1]
+  # before deviceState.deviceType was set, assume tici AR config
+  ("unknown", "ar0231"): _ar_ox_config,
+  ("unknown", "ox03c10"): _ar_ox_config,
 
+  # simulator (emulates a tici)
+  ("pc", "unknown"): _ar_ox_config,
+}
+prods = itertools.product(('tici', 'tizi', 'mici'), (('ar0231', _ar_ox_config), ('ox03c10', _ar_ox_config), ('os04c10', _os_config)))
+DEVICE_CAMERAS.update({(d, c[0]): c[1] for d, c in prods})
 
 # device/mesh : x->forward, y-> right, z->down
 # view : x->right, y->down, z->forward
@@ -93,7 +112,7 @@ def roll_from_ke(m):
                     -(m[0, 0] - m[0, 1] * m[2, 0] / m[2, 1]))
 
 
-def normalize(img_pts, intrinsics=fcam_intrinsics):
+def normalize(img_pts, intrinsics):
   # normalizes image coordinates
   # accepts single pt or array of pts
   intrinsics_inv = np.linalg.inv(intrinsics)
@@ -106,7 +125,7 @@ def normalize(img_pts, intrinsics=fcam_intrinsics):
   return img_pts_normalized[:, :2].reshape(input_shape)
 
 
-def denormalize(img_pts, intrinsics=fcam_intrinsics, width=np.inf, height=np.inf):
+def denormalize(img_pts, intrinsics, width=np.inf, height=np.inf):
   # denormalizes image coordinates
   # accepts single pt or array of pts
   img_pts = np.array(img_pts)
@@ -123,7 +142,7 @@ def denormalize(img_pts, intrinsics=fcam_intrinsics, width=np.inf, height=np.inf
   return img_pts_denormalized[:, :2].reshape(input_shape)
 
 
-def get_calib_from_vp(vp, intrinsics=fcam_intrinsics):
+def get_calib_from_vp(vp, intrinsics):
   vp_norm = normalize(vp, intrinsics)
   yaw_calib = np.arctan(vp_norm[0])
   pitch_calib = -np.arctan(vp_norm[1]*np.cos(yaw_calib))

common/transformations/model.py

@@ -1,19 +1,11 @@
 import numpy as np
 
 from openpilot.common.transformations.orientation import rot_from_euler
-from openpilot.common.transformations.camera import (
-  FULL_FRAME_SIZE, get_view_frame_from_calib_frame, view_frame_from_device_frame,
-  eon_fcam_intrinsics, tici_ecam_intrinsics, tici_fcam_intrinsics)
+from openpilot.common.transformations.camera import get_view_frame_from_calib_frame, view_frame_from_device_frame
 
 # segnet
 SEGNET_SIZE = (512, 384)
 
-def get_segnet_frame_from_camera_frame(segnet_size=SEGNET_SIZE, full_frame_size=FULL_FRAME_SIZE):
-  return np.array([[float(segnet_size[0]) / full_frame_size[0],  0.0],
-                   [0.0,  float(segnet_size[1]) / full_frame_size[1]]])
-segnet_frame_from_camera_frame = get_segnet_frame_from_camera_frame() # xx
-
-
 # MED model
 MEDMODEL_INPUT_SIZE = (512, 256)
 MEDMODEL_YUV_SIZE = (MEDMODEL_INPUT_SIZE[0], MEDMODEL_INPUT_SIZE[1] * 3 // 2)
@@ -63,16 +55,9 @@ calib_from_medmodel = np.linalg.inv(medmodel_frame_from_calib_frame[:, :3])
 calib_from_sbigmodel = np.linalg.inv(sbigmodel_frame_from_calib_frame[:, :3])
 
 # This function is verified to give similar results to xx.uncommon.utils.transform_img
-def get_warp_matrix(device_from_calib_euler: np.ndarray, wide_camera: bool = False, bigmodel_frame: bool = False, tici: bool = True) -> np.ndarray:
-  if tici and wide_camera:
-    cam_intrinsics = tici_ecam_intrinsics
-  elif tici:
-    cam_intrinsics = tici_fcam_intrinsics
-  else:
-    cam_intrinsics = eon_fcam_intrinsics
-
+def get_warp_matrix(device_from_calib_euler: np.ndarray, intrinsics: np.ndarray, bigmodel_frame: bool = False) -> np.ndarray:
   calib_from_model = calib_from_sbigmodel if bigmodel_frame else calib_from_medmodel
   device_from_calib = rot_from_euler(device_from_calib_euler)
-  camera_from_calib = cam_intrinsics @ view_frame_from_device_frame @ device_from_calib
+  camera_from_calib = intrinsics @ view_frame_from_device_frame @ device_from_calib
   warp_matrix: np.ndarray = camera_from_calib @ calib_from_model
   return warp_matrix

common/transformations/orientation.py

@@ -1,5 +1,5 @@
 import numpy as np
-from typing import Callable
+from collections.abc import Callable
 
 from openpilot.common.transformations.transformations import (ecef_euler_from_ned_single,
                                                     euler2quat_single,

common/transformations/tests/test_coordinates.py

@@ -0,0 +1,104 @@
+import numpy as np
+
+import openpilot.common.transformations.coordinates as coord
+
+geodetic_positions = np.array([[37.7610403, -122.4778699, 115],
+                                 [27.4840915, -68.5867592, 2380],
+                                 [32.4916858, -113.652821, -6],
+                                 [15.1392514, 103.6976037, 24],
+                                 [24.2302229, 44.2835412, 1650]])
+
+ecef_positions = np.array([[-2711076.55270557, -4259167.14692758,  3884579.87669935],
+                          [ 2068042.69652729, -5273435.40316622,  2927004.89190746],
+                          [-2160412.60461669, -4932588.89873832,  3406542.29652851],
+                          [-1458247.92550567,  5983060.87496612,  1654984.6099885 ],
+                          [ 4167239.10867871,  4064301.90363223,  2602234.6065749 ]])
+
+ecef_positions_offset = np.array([[-2711004.46961115, -4259099.33540613,  3884605.16002147],
+                                  [ 2068074.30639499, -5273413.78835412,  2927012.48741131],
+                                  [-2160344.53748176, -4932586.20092211,  3406636.2962545 ],
+                                  [-1458211.98517094,  5983151.11161276,  1655077.02698447],
+                                  [ 4167271.20055269,  4064398.22619263,  2602238.95265847]])
+
+
+ned_offsets = np.array([[78.722153649976391, 24.396208657446344, 60.343017506838436],
+                       [10.699003365155221, 37.319278617604269, 4.1084100025050407],
+                       [95.282646251726959, 61.266689955574428, -25.376506058505054],
+                       [68.535769283630003, -56.285970011848889, -100.54840137956515],
+                       [-33.066609321880179, 46.549821994306861, -84.062540548335591]])
+
+ecef_init_batch = np.array([2068042.69652729, -5273435.40316622,  2927004.89190746])
+ecef_positions_offset_batch = np.array([[ 2068089.41454771, -5273434.46829148,  2927074.04783672],
+                                        [ 2068103.31628647, -5273393.92275431,  2927102.08725987],
+                                        [ 2068108.49939636, -5273359.27047121,  2927045.07091581],
+                                        [ 2068075.12395611, -5273381.69432566,  2927041.08207992],
+                                        [ 2068060.72033399, -5273430.6061505,  2927094.54928305]])
+
+ned_offsets_batch = np.array([[  53.88103168,   43.83445935,  -46.27488057],
+                              [  93.83378995,   71.57943024,  -30.23113187],
+                              [  57.26725796,   89.05602684,   23.02265814],
+                              [  49.71775195,   49.79767572,   17.15351015],
+                              [  78.56272609,   18.53100158,  -43.25290759]])
+
+
+class TestNED:
+  def test_small_distances(self):
+    start_geodetic = np.array([33.8042184, -117.888593, 0.0])
+    local_coord = coord.LocalCoord.from_geodetic(start_geodetic)
+
+    start_ned = local_coord.geodetic2ned(start_geodetic)
+    np.testing.assert_array_equal(start_ned, np.zeros(3,))
+
+    west_geodetic = start_geodetic + [0, -0.0005, 0]
+    west_ned = local_coord.geodetic2ned(west_geodetic)
+    assert np.abs(west_ned[0]) < 1e-3
+    assert west_ned[1] < 0
+
+    southwest_geodetic = start_geodetic + [-0.0005, -0.002, 0]
+    southwest_ned = local_coord.geodetic2ned(southwest_geodetic)
+    assert southwest_ned[0] < 0
+    assert southwest_ned[1] < 0
+
+  def test_ecef_geodetic(self):
+    # testing single
+    np.testing.assert_allclose(ecef_positions[0], coord.geodetic2ecef(geodetic_positions[0]), rtol=1e-9)
+    np.testing.assert_allclose(geodetic_positions[0, :2], coord.ecef2geodetic(ecef_positions[0])[:2], rtol=1e-9)
+    np.testing.assert_allclose(geodetic_positions[0, 2], coord.ecef2geodetic(ecef_positions[0])[2], rtol=1e-9, atol=1e-4)
+
+    np.testing.assert_allclose(geodetic_positions[:, :2], coord.ecef2geodetic(ecef_positions)[:, :2], rtol=1e-9)
+    np.testing.assert_allclose(geodetic_positions[:, 2], coord.ecef2geodetic(ecef_positions)[:, 2], rtol=1e-9, atol=1e-4)
+    np.testing.assert_allclose(ecef_positions, coord.geodetic2ecef(geodetic_positions), rtol=1e-9)
+
+
+  def test_ned(self):
+    for ecef_pos in ecef_positions:
+      converter = coord.LocalCoord.from_ecef(ecef_pos)
+      ecef_pos_moved = ecef_pos + [25, -25, 25]
+      ecef_pos_moved_double_converted = converter.ned2ecef(converter.ecef2ned(ecef_pos_moved))
+      np.testing.assert_allclose(ecef_pos_moved, ecef_pos_moved_double_converted, rtol=1e-9)
+
+    for geo_pos in geodetic_positions:
+      converter = coord.LocalCoord.from_geodetic(geo_pos)
+      geo_pos_moved = geo_pos + np.array([0, 0, 10])
+      geo_pos_double_converted_moved = converter.ned2geodetic(converter.geodetic2ned(geo_pos) + np.array([0, 0, -10]))
+      np.testing.assert_allclose(geo_pos_moved[:2], geo_pos_double_converted_moved[:2], rtol=1e-9, atol=1e-6)
+      np.testing.assert_allclose(geo_pos_moved[2], geo_pos_double_converted_moved[2], rtol=1e-9, atol=1e-4)
+
+  def test_ned_saved_results(self):
+    for i, ecef_pos in enumerate(ecef_positions):
+      converter = coord.LocalCoord.from_ecef(ecef_pos)
+      np.testing.assert_allclose(converter.ned2ecef(ned_offsets[i]),
+                                 ecef_positions_offset[i],
+                                 rtol=1e-9, atol=1e-4)
+      np.testing.assert_allclose(converter.ecef2ned(ecef_positions_offset[i]),
+                                 ned_offsets[i],
+                                 rtol=1e-9, atol=1e-4)
+
+  def test_ned_batch(self):
+    converter = coord.LocalCoord.from_ecef(ecef_init_batch)
+    np.testing.assert_allclose(converter.ecef2ned(ecef_positions_offset_batch),
+                                                           ned_offsets_batch,
+                                                           rtol=1e-9, atol=1e-7)
+    np.testing.assert_allclose(converter.ned2ecef(ned_offsets_batch),
+                                                           ecef_positions_offset_batch,
+                                                           rtol=1e-9, atol=1e-7)

common/transformations/tests/test_orientation.py

@@ -0,0 +1,61 @@
+import numpy as np
+
+from openpilot.common.transformations.orientation import euler2quat, quat2euler, euler2rot, rot2euler, \
+                                               rot2quat, quat2rot, \
+                                               ned_euler_from_ecef
+
+eulers = np.array([[ 1.46520501,  2.78688383,  2.92780854],
+       [ 4.86909526,  3.60618161,  4.30648981],
+       [ 3.72175965,  2.68763705,  5.43895988],
+       [ 5.92306687,  5.69573614,  0.81100357],
+       [ 0.67838374,  5.02402037,  2.47106426]])
+
+quats = np.array([[ 0.66855182, -0.71500939,  0.19539353,  0.06017818],
+       [ 0.43163717,  0.70013301,  0.28209145,  0.49389021],
+       [ 0.44121991, -0.08252646,  0.34257534,  0.82532207],
+       [ 0.88578382, -0.04515356, -0.32936046,  0.32383617],
+       [ 0.06578165,  0.61282835,  0.07126891,  0.78424163]])
+
+ecef_positions = np.array([[-2711076.55270557, -4259167.14692758,  3884579.87669935],
+                          [ 2068042.69652729, -5273435.40316622,  2927004.89190746],
+                          [-2160412.60461669, -4932588.89873832,  3406542.29652851],
+                          [-1458247.92550567,  5983060.87496612,  1654984.6099885 ],
+                          [ 4167239.10867871,  4064301.90363223,  2602234.6065749 ]])
+
+ned_eulers = np.array([[ 0.46806039, -0.4881889 ,  1.65697808],
+                    [-2.14525969, -0.36533066,  0.73813479],
+                    [-1.39523364, -0.58540761, -1.77376356],
+                    [-1.84220435,  0.61828016, -1.03310421],
+                    [ 2.50450101,  0.36304151,  0.33136365]])
+
+
+class TestOrientation:
+  def test_quat_euler(self):
+    for i, eul in enumerate(eulers):
+      np.testing.assert_allclose(quats[i], euler2quat(eul), rtol=1e-7)
+      np.testing.assert_allclose(quats[i], euler2quat(quat2euler(quats[i])), rtol=1e-6)
+    for i, eul in enumerate(eulers):
+      np.testing.assert_allclose(quats[i], euler2quat(list(eul)), rtol=1e-7)
+      np.testing.assert_allclose(quats[i], euler2quat(quat2euler(list(quats[i]))), rtol=1e-6)
+    np.testing.assert_allclose(quats, euler2quat(eulers), rtol=1e-7)
+    np.testing.assert_allclose(quats, euler2quat(quat2euler(quats)), rtol=1e-6)
+
+  def test_rot_euler(self):
+    for eul in eulers:
+      np.testing.assert_allclose(euler2quat(eul), euler2quat(rot2euler(euler2rot(eul))), rtol=1e-7)
+    for eul in eulers:
+      np.testing.assert_allclose(euler2quat(eul), euler2quat(rot2euler(euler2rot(list(eul)))), rtol=1e-7)
+    np.testing.assert_allclose(euler2quat(eulers), euler2quat(rot2euler(euler2rot(eulers))), rtol=1e-7)
+
+  def test_rot_quat(self):
+    for quat in quats:
+      np.testing.assert_allclose(quat, rot2quat(quat2rot(quat)), rtol=1e-7)
+    for quat in quats:
+      np.testing.assert_allclose(quat, rot2quat(quat2rot(list(quat))), rtol=1e-7)
+    np.testing.assert_allclose(quats, rot2quat(quat2rot(quats)), rtol=1e-7)
+
+  def test_euler_ned(self):
+    for i in range(len(eulers)):
+      np.testing.assert_allclose(ned_eulers[i], ned_euler_from_ecef(ecef_positions[i], eulers[i]), rtol=1e-7)
+      #np.testing.assert_allclose(eulers[i], ecef_euler_from_ned(ecef_positions[i], ned_eulers[i]), rtol=1e-7)
+    # np.testing.assert_allclose(ned_eulers, ned_euler_from_ecef(ecef_positions, eulers), rtol=1e-7)

common/util.cc

@@ -246,18 +246,13 @@ std::string random_string(std::string::size_type length) {
   return s;
 }
 
-std::string dir_name(std::string const &path) {
-  size_t pos = path.find_last_of("/");
-  if (pos == std::string::npos) return "";
-  return path.substr(0, pos);
-}
-
 bool starts_with(const std::string &s1, const std::string &s2) {
   return strncmp(s1.c_str(), s2.c_str(), s2.size()) == 0;
 }
 
-bool ends_with(const std::string &s1, const std::string &s2) {
-  return strcmp(s1.c_str() + (s1.size() - s2.size()), s2.c_str()) == 0;
+bool ends_with(const std::string& s, const std::string& suffix) {
+  return s.size() >= suffix.size() &&
+         strcmp(s.c_str() + (s.size() - suffix.size()), suffix.c_str()) == 0;
 }
 
 std::string check_output(const std::string& command) {
@@ -276,20 +271,4 @@ std::string check_output(const std::string& command) {
   return result;
 }
 
-struct tm get_time() {
-  time_t rawtime;
-  time(&rawtime);
-
-  struct tm sys_time;
-  gmtime_r(&rawtime, &sys_time);
-
-  return sys_time;
-}
-
-bool time_valid(struct tm sys_time) {
-  int year = 1900 + sys_time.tm_year;
-  int month = 1 + sys_time.tm_mon;
-  return (year > 2023) || (year == 2023 && month >= 6);
-}
-
 }  // namespace util

common/util.h

@@ -8,7 +8,6 @@
 #include <atomic>
 #include <chrono>
 #include <csignal>
-#include <ctime>
 #include <map>
 #include <memory>
 #include <mutex>
@@ -45,10 +44,6 @@ int set_realtime_priority(int level);
 int set_core_affinity(std::vector<int> cores);
 int set_file_descriptor_limit(uint64_t limit);
 
-// ***** Time helpers *****
-struct tm get_time();
-bool time_valid(struct tm sys_time);
-
 // ***** math helpers *****
 
 // map x from [a1, a2] to [b1, b2]
@@ -75,9 +70,8 @@ int getenv(const char* key, int default_val);
 float getenv(const char* key, float default_val);
 
 std::string hexdump(const uint8_t* in, const size_t size);
-std::string dir_name(std::string const& path);
 bool starts_with(const std::string &s1, const std::string &s2);
-bool ends_with(const std::string &s1, const std::string &s2);
+bool ends_with(const std::string &s, const std::string &suffix);
 
 // ***** random helpers *****
 int random_int(int min, int max);
@@ -179,3 +173,10 @@ void update_max_atomic(std::atomic<T>& max, T const& value) {
   T prev = max;
   while (prev < value && !max.compare_exchange_weak(prev, value)) {}
 }
+
+typedef struct Rect {
+  int x;
+  int y;
+  int w;
+  int h;
+} Rect;

common/utils.py

@@ -0,0 +1,11 @@
+class Freezable:
+  _frozen: bool = False
+
+  def freeze(self):
+    if not self._frozen:
+      self._frozen = True
+
+  def __setattr__(self, *args, **kwargs):
+    if self._frozen:
+      raise Exception("cannot modify frozen object")
+    super().__setattr__(*args, **kwargs)

common/version.h

@@ -1 +1 @@
-#define COMMA_VERSION "0.9.6"
+#define COMMA_VERSION "0.9.7"