third_party/acados: Remove cython and import dynamically instead

selfdrive/controls/lib/acados_setup.py

@@ -0,0 +1,63 @@
+import os
+import sys
+import ctypes
+import platform
+
+def get_acados_dir():
+  # current file: openpilot/selfdrive/controls/lib/acados_setup.py
+  # acados is at openpilot/third_party/acados
+  current_dir = os.path.dirname(os.path.abspath(__file__))
+  op_dir = os.path.abspath(os.path.join(current_dir, '..', '..', '..'))
+  return os.path.join(op_dir, 'third_party', 'acados')
+
+def get_acados_lib_path():
+  acados_dir = get_acados_dir()
+  if sys.platform.startswith('linux'):
+    machine = platform.machine()
+    if machine == 'x86_64':
+      return os.path.join(acados_dir, 'x86_64', 'lib')
+    elif machine == 'aarch64':
+      return os.path.join(acados_dir, 'larch64', 'lib')
+  elif sys.platform.startswith('darwin'):
+    return os.path.join(acados_dir, 'Darwin', 'lib')
+
+  # Fallback
+  return os.path.join(acados_dir, 'x86_64', 'lib')
+
+def acados_preload():
+  lib_path = get_acados_lib_path()
+  if not os.path.exists(lib_path):
+    return
+
+  if sys.platform.startswith('linux'):
+    libs = ['libblasfeo.so', 'libhpipm.so', 'libqpOASES_e.so.3.1']
+    mode = ctypes.RTLD_GLOBAL
+  elif sys.platform.startswith('darwin'):
+    libs = ['libblasfeo.dylib', 'libhpipm.dylib', 'libqpOASES_e.3.1.dylib']
+    mode = ctypes.RTLD_GLOBAL
+  else:
+    libs = []
+    mode = 0
+
+  for lib in libs:
+    full_path = os.path.join(lib_path, lib)
+    if os.path.exists(full_path):
+      try:
+        ctypes.CDLL(full_path, mode=mode)
+      except OSError:
+        pass
+
+def prepare_acados_ocp_json(json_file):
+  import json
+  import tempfile
+
+  with open(json_file) as f:
+    data = json.load(f)
+
+  data['acados_lib_path'] = get_acados_lib_path()
+
+  fd, path = tempfile.mkstemp(suffix='.json', text=True)
+  with os.fdopen(fd, 'w') as f:
+    json.dump(data, f, indent=4)
+
+  return path

selfdrive/controls/lib/lateral_mpc_lib/SConscript

@@ -1,4 +1,4 @@
-Import('env', 'envCython', 'arch', 'msgq_python', 'common_python', 'np_version')
+Import('env', 'arch', 'msgq_python', 'common_python', 'np_version')
 
 gen = "c_generated_code"
 
@@ -75,23 +75,3 @@ else:
 lib_solver = lenv.SharedLibrary(f"{gen}/acados_ocp_solver_lat",
                                 build_files,
                                 LIBS=['m', 'acados', 'hpipm', 'blasfeo', 'qpOASES_e'])
-
-# generate cython stuff
-acados_ocp_solver_pyx = File("#third_party/acados/acados_template/acados_ocp_solver_pyx.pyx")
-acados_ocp_solver_common = File("#third_party/acados/acados_template/acados_solver_common.pxd")
-libacados_ocp_solver_pxd = File(f'{gen}/acados_solver.pxd')
-libacados_ocp_solver_c = File(f'{gen}/acados_ocp_solver_pyx.c')
-
-lenv2 = envCython.Clone()
-lenv2["LIBPATH"] += [lib_solver[0].dir.abspath]
-lenv2["RPATH"] += [lenv2.Literal('\\$$ORIGIN')]
-lenv2.Command(libacados_ocp_solver_c,
-  [acados_ocp_solver_pyx, acados_ocp_solver_common, libacados_ocp_solver_pxd],
-  f'cython' + \
-  f' -o {libacados_ocp_solver_c.get_labspath()}' + \
-  f' -I {libacados_ocp_solver_pxd.get_dir().get_labspath()}' + \
-  f' -I {acados_ocp_solver_common.get_dir().get_labspath()}' + \
-  f' {acados_ocp_solver_pyx.get_labspath()}')
-lib_cython = lenv2.Program(f'{gen}/acados_ocp_solver_pyx.so', [libacados_ocp_solver_c], LIBS=['acados_ocp_solver_lat'])
-lenv2.Depends(lib_cython, lib_solver)
-lenv2.Depends(libacados_ocp_solver_c, np_version)

selfdrive/controls/lib/lateral_mpc_lib/lat_mpc.py

@@ -7,10 +7,8 @@
 # WARNING: imports outside of constants will not trigger a rebuild
 from openpilot.selfdrive.modeld.constants import ModelConstants
 
-if __name__ == '__main__':  # generating code
-  from openpilot.third_party.acados.acados_template import AcadosModel, AcadosOcp, AcadosOcpSolver
-else:
-  from openpilot.selfdrive.controls.lib.lateral_mpc_lib.c_generated_code.acados_ocp_solver_pyx import AcadosOcpSolverCython
+from openpilot.third_party.acados.acados_template import AcadosModel, AcadosOcp, AcadosOcpSolver
+from openpilot.selfdrive.controls.lib.acados_setup import acados_preload, prepare_acados_ocp_json
 
 LAT_MPC_DIR = os.path.dirname(os.path.abspath(__file__))
 EXPORT_DIR = os.path.join(LAT_MPC_DIR, "c_generated_code")
@@ -132,7 +130,14 @@ class LateralMpc:
   def __init__(self, x0=None):
     if x0 is None:
       x0 = np.zeros(X_DIM)
-    self.solver = AcadosOcpSolverCython(MODEL_NAME, ACADOS_SOLVER_TYPE, N)
+    acados_preload()
+    # Fixup JSON for current architecture
+    json_path = prepare_acados_ocp_json(JSON_FILE)
+    try:
+      self.solver = AcadosOcpSolver(gen_lat_ocp(), json_file=json_path, generate=False, build=False)
+    finally:
+      if os.path.exists(json_path):
+        os.remove(json_path)
     self.reset(x0)
 
   def reset(self, x0=None):
@@ -196,4 +201,4 @@ def run(self, x0, p, y_pts, heading_pts, yaw_rate_pts):
 if __name__ == "__main__":
   ocp = gen_lat_ocp()
   AcadosOcpSolver.generate(ocp, json_file=JSON_FILE)
-  # AcadosOcpSolver.build(ocp.code_export_directory, with_cython=True)
+

selfdrive/controls/lib/longitudinal_mpc_lib/SConscript

@@ -1,4 +1,4 @@
-Import('env', 'envCython', 'arch', 'msgq_python', 'common_python', 'pandad_python', 'np_version')
+Import('env', 'arch', 'msgq_python', 'common_python', 'pandad_python', 'np_version')
 
 gen = "c_generated_code"
 
@@ -80,23 +80,3 @@ else:
 lib_solver = lenv.SharedLibrary(f"{gen}/acados_ocp_solver_long",
                                 build_files,
                                 LIBS=['m', 'acados', 'hpipm', 'blasfeo', 'qpOASES_e'])
-
-# generate cython stuff
-acados_ocp_solver_pyx = File("#third_party/acados/acados_template/acados_ocp_solver_pyx.pyx")
-acados_ocp_solver_common = File("#third_party/acados/acados_template/acados_solver_common.pxd")
-libacados_ocp_solver_pxd = File(f'{gen}/acados_solver.pxd')
-libacados_ocp_solver_c = File(f'{gen}/acados_ocp_solver_pyx.c')
-
-lenv2 = envCython.Clone()
-lenv2["LIBPATH"] += [lib_solver[0].dir.abspath]
-lenv2["RPATH"] += [lenv2.Literal('\\$$ORIGIN')]
-lenv2.Command(libacados_ocp_solver_c,
-  [acados_ocp_solver_pyx, acados_ocp_solver_common, libacados_ocp_solver_pxd],
-  f'cython' + \
-  f' -o {libacados_ocp_solver_c.get_labspath()}' + \
-  f' -I {libacados_ocp_solver_pxd.get_dir().get_labspath()}' + \
-  f' -I {acados_ocp_solver_common.get_dir().get_labspath()}' + \
-  f' {acados_ocp_solver_pyx.get_labspath()}')
-lib_cython = lenv2.Program(f'{gen}/acados_ocp_solver_pyx.so', [libacados_ocp_solver_c], LIBS=['acados_ocp_solver_long'])
-lenv2.Depends(lib_cython, lib_solver)
-lenv2.Depends(libacados_ocp_solver_c, np_version)

selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py

@@ -10,10 +10,8 @@
 from openpilot.selfdrive.modeld.constants import index_function
 from openpilot.selfdrive.controls.radard import _LEAD_ACCEL_TAU
 
-if __name__ == '__main__':  # generating code
-  from openpilot.third_party.acados.acados_template import AcadosModel, AcadosOcp, AcadosOcpSolver
-else:
-  from openpilot.selfdrive.controls.lib.longitudinal_mpc_lib.c_generated_code.acados_ocp_solver_pyx import AcadosOcpSolverCython
+from openpilot.third_party.acados.acados_template import AcadosModel, AcadosOcp, AcadosOcpSolver
+from openpilot.selfdrive.controls.lib.acados_setup import acados_preload, prepare_acados_ocp_json
 
 from casadi import SX, vertcat
 
@@ -225,12 +223,17 @@ class LongitudinalMpc:
   def __init__(self, mode='acc', dt=DT_MDL):
     self.mode = mode
     self.dt = dt
-    self.solver = AcadosOcpSolverCython(MODEL_NAME, ACADOS_SOLVER_TYPE, N)
+    acados_preload()
+    json_path = prepare_acados_ocp_json(JSON_FILE)
+    try:
+      self.solver = AcadosOcpSolver(gen_long_ocp(), json_file=json_path, generate=False, build=False)
+    finally:
+      if os.path.exists(json_path):
+        os.remove(json_path)
     self.reset()
     self.source = SOURCES[2]
 
   def reset(self):
-    # self.solver = AcadosOcpSolverCython(MODEL_NAME, ACADOS_SOLVER_TYPE, N)
     self.solver.reset()
     # self.solver.options_set('print_level', 2)
     self.v_solution = np.zeros(N+1)
@@ -454,4 +457,3 @@ def run(self):
 if __name__ == "__main__":
   ocp = gen_long_ocp()
   AcadosOcpSolver.generate(ocp, json_file=JSON_FILE)
-  # AcadosOcpSolver.build(ocp.code_export_directory, with_cython=True)

selfdrive/controls/tests/test_acados_setup.py

@@ -0,0 +1,21 @@
+import os
+import sys
+import platform
+from openpilot.selfdrive.controls.lib.acados_setup import get_acados_lib_path, get_acados_dir
+
+def test_get_acados_lib_path_linux_x86_64(monkeypatch):
+  monkeypatch.setattr(sys, 'platform', 'linux')
+  monkeypatch.setattr(platform, 'machine', lambda: 'x86_64')
+  expected = os.path.join(get_acados_dir(), 'x86_64', 'lib')
+  assert get_acados_lib_path() == expected
+
+def test_get_acados_lib_path_linux_aarch64(monkeypatch):
+  monkeypatch.setattr(sys, 'platform', 'linux')
+  monkeypatch.setattr(platform, 'machine', lambda: 'aarch64')
+  expected = os.path.join(get_acados_dir(), 'larch64', 'lib')
+  assert get_acados_lib_path() == expected
+
+def test_get_acados_lib_path_darwin(monkeypatch):
+  monkeypatch.setattr(sys, 'platform', 'darwin')
+  expected = os.path.join(get_acados_dir(), 'Darwin', 'lib')
+  assert get_acados_lib_path() == expected