Restructure. Remove common_ekf. Load libs at runtime

examples/lst_sq_computer.py

@@ -50,7 +50,7 @@ def generate_code(generated_dir, K=4):
     sympy_functions = generate_residual(K)
     header, sympy_code = sympy_into_c(sympy_functions)
 
-    code = "\n#include \"rednose/helpers/common_ekf.h\"\n"
+    code = "\n#include \"rednose/helpers/ekf.h\"\n"
     code += "\n#define KDIM %d\n" % K
     code += "extern \"C\" {\n"
     code += sympy_code

examples/test_kinematic_kf.py

@@ -38,11 +38,11 @@ def test_kinematic_kf(self):
 
       # Retrieve kf values
       state = kf.x
-      xs_kf.append(float(state[States.POSITION]))
-      vs_kf.append(float(state[States.VELOCITY]))
+      xs_kf.append(float(state[States.POSITION].item()))
+      vs_kf.append(float(state[States.VELOCITY].item()))
       std = np.sqrt(kf.P)
-      xs_kf_std.append(float(std[States.POSITION, States.POSITION]))
-      vs_kf_std.append(float(std[States.VELOCITY, States.VELOCITY]))
+      xs_kf_std.append(float(std[States.POSITION, States.POSITION].item()))
+      vs_kf_std.append(float(std[States.VELOCITY, States.VELOCITY].item()))
 
       # Update simulation
       x += v * dt

rednose/SConscript

@@ -1,7 +1,7 @@
 Import('env', 'envCython', 'common')
 
 cc_sources = [
-  "helpers/common_ekf.cc",
+  "helpers/ekf_load.cc",
   "helpers/ekf_sym.cc",
 ]
 libs = []
@@ -10,7 +10,8 @@ if common != "":
   libs = ['zmq', common]
 
 ekf_objects = env.SharedObject(cc_sources)
-rednose = env.Library("helpers/ekf_sym", ekf_objects, LIBS=libs)
-rednose_python = envCython.Program("helpers/ekf_sym_pyx.so", ["helpers/ekf_sym_pyx.pyx", ekf_objects], LIBS=libs + envCython["LIBS"])
+rednose = env.SharedLibrary("helpers/ekf_sym", ekf_objects, LIBS=libs)
+rednose_python = envCython.Program("helpers/ekf_sym_pyx.so", ["helpers/ekf_sym_pyx.pyx", ekf_objects],
+                                   LIBS=libs + envCython["LIBS"])
 
 Export('rednose', 'rednose_python')

rednose/helpers/__init__.py

@@ -13,11 +13,9 @@ def write_code(folder, name, code, header):
   open(os.path.join(folder, f"{name}.h"), 'w', encoding='utf-8').write(header)
 
 
-def load_code(folder, name, lib_name=None):
-  if lib_name is None:
-    lib_name = name
+def load_code(folder, name):
   shared_ext = "dylib" if platform.system() == "Darwin" else "so"
-  shared_fn = os.path.join(folder, f"lib{lib_name}.{shared_ext}")
+  shared_fn = os.path.join(folder, f"lib{name}.{shared_ext}")
   header_fn = os.path.join(folder, f"{name}.h")
 
   with open(header_fn, encoding='utf-8') as f:

rednose/helpers/common_ekf.h → rednose/helpers/ekf.h

@@ -31,13 +31,3 @@ struct EKF {
   std::unordered_map<std::string, void (*)(double)> sets = {};
   std::unordered_map<std::string, extra_routine_t> extra_routines = {};
 };
-
-std::vector<const EKF*>& get_ekfs();
-const EKF* ekf_lookup(const std::string& ekf_name);
-
-void ekf_register(const EKF* ekf);
-
-#define ekf_init(ekf) \
-static void __attribute__((constructor)) do_ekf_init_ ## ekf(void) { \
-  ekf_register(&ekf); \
-}

rednose/helpers/ekf_load.cc

@@ -0,0 +1,39 @@
+#include "ekf_load.h"
+#include <dlfcn.h>
+
+std::vector<const EKF*>& ekf_get_all() {
+  static std::vector<const EKF*> vec;
+  return vec;
+}
+
+void ekf_register(const EKF* ekf) {
+  ekf_get_all().push_back(ekf);
+}
+
+const EKF* ekf_lookup(const std::string& ekf_name) {
+  for (const auto& ekfi : ekf_get_all()) {
+    if (ekf_name == ekfi->name) {
+      return ekfi;
+    }
+  }
+  return NULL;
+}
+
+void ekf_load_and_register(const std::string& ekf_directory, const std::string& ekf_name) {
+  if (ekf_lookup(ekf_name)) {
+    return;
+  }
+
+#ifdef __APPLE__
+  std::string dylib_ext = ".dylib";
+#else
+  std::string dylib_ext = ".so";
+#endif
+  std::string ekf_path = ekf_directory + "/lib" + ekf_name + dylib_ext;
+  void* handle = dlopen(ekf_path.c_str(), RTLD_NOW);
+  assert(handle);
+  void* (*ekf_get)() = (void*(*)())dlsym(handle, "ekf_get");
+  assert(ekf_get != NULL);
+  const EKF* ekf = (const EKF*)ekf_get();
+  ekf_register(ekf);
+}

rednose/helpers/ekf_load.h

@@ -0,0 +1,9 @@
+#include <vector>
+#include <string>
+
+#include "ekf.h"
+
+std::vector<const EKF*>& ekf_get_all();
+const EKF* ekf_lookup(const std::string& ekf_name);
+void ekf_register(const EKF* ekf);
+void ekf_load_and_register(const std::string& ekf_directory, const std::string& ekf_name);

rednose/helpers/ekf_sym.cc

@@ -9,7 +9,6 @@ EKFSym::EKFSym(std::string name, Map<MatrixXdr> Q, Map<VectorXd> x_initial, Map<
     std::vector<int> quaternion_idxs, std::vector<std::string> global_vars, double max_rewind_age)
 {
   // TODO: add logger
-
   this->ekf = ekf_lookup(name);
   assert(this->ekf);
 

rednose/helpers/ekf_sym.h

@@ -12,7 +12,8 @@
 
 #include <eigen3/Eigen/Dense>
 
-#include "common_ekf.h"
+#include "ekf.h"
+#include "ekf_load.h"
 
 #define REWIND_TO_KEEP 512
 

rednose/helpers/ekf_sym.py

@@ -116,7 +116,7 @@ def gen_code(folder, name, f_sym, dt_sym, x_sym, obs_eqs, dim_x, dim_err, eskf_p
   sympy_header, code = sympy_into_c(sympy_functions, global_vars)
 
   header = "#pragma once\n"
-  header += "#include \"rednose/helpers/common_ekf.h\"\n"
+  header += "#include \"rednose/helpers/ekf.h\"\n"
   header += "extern \"C\" {\n"
 
   pre_code = f"#include \"{name}.h\"\n"
@@ -200,7 +200,11 @@ def gen_code(folder, name, f_sym, dt_sym, x_sym, obs_eqs, dim_x, dim_err, eskf_p
     post_code += f"    {{ \"{f}\", {name}_{f} }},\n"
   post_code += "  },\n"
   post_code += "};\n\n"
-  post_code += f"ekf_init({name});\n"
+  post_code += "extern \"C\" {\n"
+  post_code += "  void* ekf_get() {\n"
+  post_code += f"    return (void*)&{name};\n"
+  post_code += "  }\n"
+  post_code += "}\n\n"
 
   # merge code blocks
   header += "}"
@@ -252,7 +256,7 @@ def __init__(self, folder, name, Q, x_initial, P_initial, dim_main, dim_main_err
     self.rewind_obscache = []
     self.init_state(x_initial, P_initial, None)
 
-    ffi, lib = load_code(folder, name, "kf")
+    ffi, lib = load_code(folder, name)
     kinds, self.feature_track_kinds = [], []
     for func in dir(lib):
       if func[:len(name) + 3] == f'{name}_h_':

rednose/helpers/ekf_sym_pyx.pyx

@@ -7,11 +7,13 @@ cimport cython
 from libcpp.string cimport string
 from libcpp.vector cimport vector
 from libcpp cimport bool
+from posix.dlfcn cimport dlopen, dlsym, dlerror, RTLD_NOW, RTLD_GLOBAL
 cimport numpy as np
 
 import numpy as np
 
-from rednose.helpers import load_code
+import os
+import platform
 
 
 cdef extern from "<optional>" namespace "std" nogil:
@@ -20,11 +22,8 @@ cdef extern from "<optional>" namespace "std" nogil:
     bool has_value()
     T& value()
 
-cdef extern from "rednose/helpers/common_ekf.h":
-  cdef cppclass EKF:
-    pass
-
-  cdef EKF* ekf_lookup(string name)
+cdef extern from "rednose/helpers/ekf_load.h":
+  cdef void ekf_load_and_register(string directory, string name)
 
 cdef extern from "rednose/helpers/ekf_sym.h" namespace "EKFS":
   cdef cppclass MapVectorXd "Eigen::Map<Eigen::VectorXd>":
@@ -87,20 +86,14 @@ cdef np.ndarray[np.float64_t, ndim=1, mode="c"] vector_to_numpy(VectorXd arr):
   cdef double[:] mem_view = <double[:arr.rows()]>arr.data()
   return np.copy(np.asarray(mem_view, dtype=np.double, order="C"))
 
-cdef ekf_load_code_if_needed(str directory, str name):
-  if ekf_lookup(name.encode('utf8')) is not cython.NULL:
-    return
-
-  load_code(directory, name)
-
 cdef class EKF_sym_pyx:
   cdef EKFSym* ekf
   def __cinit__(self, str gen_dir, str name, np.ndarray[np.float64_t, ndim=2] Q,
       np.ndarray[np.float64_t, ndim=1] x_initial, np.ndarray[np.float64_t, ndim=2] P_initial, int dim_main,
       int dim_main_err, int N=0, int dim_augment=0, int dim_augment_err=0, list maha_test_kinds=[],
       list quaternion_idxs=[], list global_vars=[], double max_rewind_age=1.0, logger=None):
     # TODO logger
-    ekf_load_code_if_needed(gen_dir, name)
+    ekf_load_and_register(gen_dir.encode('utf8'), name.encode('utf8'))
 
     cdef np.ndarray[np.float64_t, ndim=2, mode='c'] Q_b = np.ascontiguousarray(Q, dtype=np.double)
     cdef np.ndarray[np.float64_t, ndim=1, mode='c'] x_initial_b = np.ascontiguousarray(x_initial, dtype=np.double)

site_scons/site_tools/rednose_filter.py

@@ -34,10 +34,9 @@ def generate(env):
   templates = Glob(f"{rednose_dir}/rednose/templates/*")
   sympy_helpers = File(f"{rednose_dir}/rednose/helpers/sympy_helpers.py")
   ekf_sym = File(f"{rednose_dir}/rednose/helpers/ekf_sym.py")
-  common_ekf = File(f"{rednose_dir}/rednose/helpers/common_ekf.cc")
 
   gen_script_deps = templates + [sympy_helpers, ekf_sym]
-  filter_lib_deps = [common_ekf]
+  filter_lib_deps = []
 
   env.AddMethod(CompileFilterMethod(gen_script_deps, filter_lib_deps), "RednoseCompileFilter")