diff --git a/examples/lst_sq_computer.py b/examples/lst_sq_computer.py index 3c74eaa..4f779de 100755 --- a/examples/lst_sq_computer.py +++ b/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 diff --git a/examples/test_kinematic_kf.py b/examples/test_kinematic_kf.py index 2d6c5e5..9f7c908 100644 --- a/examples/test_kinematic_kf.py +++ b/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 diff --git a/rednose/SConscript b/rednose/SConscript index e246438..5346989 100644 --- a/rednose/SConscript +++ b/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') diff --git a/rednose/helpers/__init__.py b/rednose/helpers/__init__.py index 12cce34..b2beede 100644 --- a/rednose/helpers/__init__.py +++ b/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: diff --git a/rednose/helpers/common_ekf.cc b/rednose/helpers/common_ekf.cc deleted file mode 100644 index 1e63902..0000000 --- a/rednose/helpers/common_ekf.cc +++ /dev/null @@ -1,19 +0,0 @@ -#include "common_ekf.h" - -std::vector& get_ekfs() { - static std::vector vec; - return vec; -} - -void ekf_register(const EKF* ekf) { - get_ekfs().push_back(ekf); -} - -const EKF* ekf_lookup(const std::string& ekf_name) { - for (const auto& ekfi : get_ekfs()) { - if (ekf_name == ekfi->name) { - return ekfi; - } - } - return NULL; -} diff --git a/rednose/helpers/common_ekf.h b/rednose/helpers/ekf.h similarity index 81% rename from rednose/helpers/common_ekf.h rename to rednose/helpers/ekf.h index 5dfdd44..a9ecb35 100644 --- a/rednose/helpers/common_ekf.h +++ b/rednose/helpers/ekf.h @@ -31,13 +31,3 @@ struct EKF { std::unordered_map sets = {}; std::unordered_map extra_routines = {}; }; - -std::vector& 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); \ -} diff --git a/rednose/helpers/ekf_load.cc b/rednose/helpers/ekf_load.cc new file mode 100644 index 0000000..f647854 --- /dev/null +++ b/rednose/helpers/ekf_load.cc @@ -0,0 +1,39 @@ +#include "ekf_load.h" +#include + +std::vector& ekf_get_all() { + static std::vector 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); +} \ No newline at end of file diff --git a/rednose/helpers/ekf_load.h b/rednose/helpers/ekf_load.h new file mode 100644 index 0000000..89180b8 --- /dev/null +++ b/rednose/helpers/ekf_load.h @@ -0,0 +1,9 @@ +#include +#include + +#include "ekf.h" + +std::vector& 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); diff --git a/rednose/helpers/ekf_sym.cc b/rednose/helpers/ekf_sym.cc index cf3c88f..38d419d 100644 --- a/rednose/helpers/ekf_sym.cc +++ b/rednose/helpers/ekf_sym.cc @@ -9,7 +9,6 @@ EKFSym::EKFSym(std::string name, Map Q, Map x_initial, Map< std::vector quaternion_idxs, std::vector global_vars, double max_rewind_age) { // TODO: add logger - this->ekf = ekf_lookup(name); assert(this->ekf); diff --git a/rednose/helpers/ekf_sym.h b/rednose/helpers/ekf_sym.h index 86d6ca1..b83d7f3 100644 --- a/rednose/helpers/ekf_sym.h +++ b/rednose/helpers/ekf_sym.h @@ -12,7 +12,8 @@ #include -#include "common_ekf.h" +#include "ekf.h" +#include "ekf_load.h" #define REWIND_TO_KEEP 512 diff --git a/rednose/helpers/ekf_sym.py b/rednose/helpers/ekf_sym.py index de68991..7107d97 100644 --- a/rednose/helpers/ekf_sym.py +++ b/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_': diff --git a/rednose/helpers/ekf_sym_pyx.pyx b/rednose/helpers/ekf_sym_pyx.pyx index 31bded3..4549526 100644 --- a/rednose/helpers/ekf_sym_pyx.pyx +++ b/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 "" namespace "std" nogil: @@ -20,11 +22,8 @@ cdef extern from "" 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": @@ -87,12 +86,6 @@ cdef np.ndarray[np.float64_t, ndim=1, mode="c"] vector_to_numpy(VectorXd arr): cdef double[:] mem_view = 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, @@ -100,7 +93,7 @@ cdef class EKF_sym_pyx: 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) diff --git a/site_scons/site_tools/rednose_filter.py b/site_scons/site_tools/rednose_filter.py index 46e33c1..b6e69c4 100644 --- a/site_scons/site_tools/rednose_filter.py +++ b/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")