Implemented NDI Aurora navigation.

src/CMakeLists.txt

@@ -4,7 +4,7 @@ project(Sophiar2Lib)
 set(CMAKE_CXX_STANDARD 23)
 
 add_compile_options(-march=native)
-add_compile_options(-mno-avx) # enable avx will cause some stack pointer alignment error with Eigen
+#add_compile_options(-mno-avx) # enable avx will cause some stack pointer alignment error with Eigen
 #add_compile_definitions(CORO_SIGNAL2_USE_TIMER)
 
 include_directories(.)

src/core/basic_obj_types.hpp

@@ -13,12 +13,25 @@
 namespace sophiar {
 
     template<typename T>
-    struct small_obj_wrapper : public small_obj<small_obj_wrapper<T>> {
+    struct is_eigen_type : std::is_base_of<Eigen::EigenBase<T>, T> {
+    };
+
+    template<typename T>
+    inline constexpr size_t small_obj_align_bytes() {
+        if constexpr (is_eigen_type<T>::value) { return EIGEN_MAX_ALIGN_BYTES; }
+        return alignof(T);
+    }
+
+    template<typename T>
+    struct small_obj_wrapper : public small_obj<small_obj_wrapper<T>,
+            aligned_allocator<small_obj_align_bytes<T>()>> {
 
         using this_type = small_obj_wrapper<T>;
         using value_type = T;
 
-        T value;
+        static constexpr auto value_align_bytes = small_obj_align_bytes<T>();
+
+        alignas(value_align_bytes) T value;
 
         small_obj_wrapper() = default;
 
@@ -56,6 +69,8 @@ namespace sophiar {
             return sizeof(T);
         }
 
+    public:
+        EIGEN_MAKE_ALIGNED_OPERATOR_NEW
     };
 
     template<size_t Length>

src/core/local_connection.cpp

@@ -78,7 +78,7 @@ namespace sophiar {
         std::mutex mu;
         std::condition_variable cv;
         command_type cmd;
-        std::any cmd_param, cmd_result;
+        boost::any cmd_param, cmd_result;
         bool cmd_finish = true;
 
         // for variable
@@ -86,7 +86,7 @@ namespace sophiar {
         struct variable_store_info {
             using variable_info = sophiar_pool::variable_info;
 
-            std::any value;
+            boost::any value;
             variable_info var_info;
             spin_lock mu;
             callback_token_type cb_token;
@@ -102,31 +102,31 @@ namespace sophiar {
                 auto lock = std::lock_guard{mu};
                 assert(!cmd_finish);
                 if (cmd < INIT_CONF) {
-                    auto obj_name = std::any_cast<std::string>(cmd_param);
+                    auto obj_name = boost::any_cast<std::string>(cmd_param);
                     switch (cmd) {
                         case INIT: {
                             cmd_result = co_await global_sophiar_manager->init_object(
-                                    std::any_cast<std::string>(cmd_param));
+                                    boost::any_cast<std::string>(cmd_param));
                             break;
                         }
                         case START: {
                             cmd_result = co_await global_sophiar_manager->start_object(
-                                    std::any_cast<std::string>(cmd_param));
+                                    boost::any_cast<std::string>(cmd_param));
                             break;
                         }
                         case STOP: {
                             cmd_result = co_await global_sophiar_manager->stop_object(
-                                    std::any_cast<std::string>(cmd_param));
+                                    boost::any_cast<std::string>(cmd_param));
                             break;
                         }
                         case RESET: {
                             cmd_result = co_await global_sophiar_manager->reset_object(
-                                    std::any_cast<std::string>(cmd_param));
+                                    boost::any_cast<std::string>(cmd_param));
                             break;
                         }
                         case QUERY: {
                             cmd_result = global_sophiar_manager->query_object_state(
-                                    std::any_cast<std::string>(cmd_param));
+                                    boost::any_cast<std::string>(cmd_param));
                             break;
                         }
                         default: {
@@ -134,7 +134,7 @@ namespace sophiar {
                         }
                     }
                 } else if (cmd < QUERY_VARIABLE) {
-                    auto patch = std::any_cast<config_patch_type>(cmd_param);
+                    auto patch = boost::any_cast<config_patch_type>(cmd_param);
                     switch (cmd) {
                         case INIT_CONF: {
                             cmd_result = global_sophiar_manager->patch_init_config(
@@ -151,13 +151,13 @@ namespace sophiar {
                         }
                     }
                 } else if (cmd == QUERY_VARIABLE) {
-                    auto var_name = std::any_cast<std::string>(cmd_param);
+                    auto var_name = boost::any_cast<std::string>(cmd_param);
                     cmd_result = global_sophiar_pool->query_variable_information(var_name);
                 } else if (cmd == REGISTER_CALLBACK) {
-                    auto func = std::any_cast<std::function<void()>>(cmd_param);
+                    auto func = boost::any_cast<std::function<void()>>(cmd_param);
                     cmd_result = REGISTER_CALLBACK(std::move(func));
                 } else if (cmd == ATTACH_CALLBACK) {
-                    auto info = std::any_cast<attach_info>(cmd_param);
+                    auto info = boost::any_cast<attach_info>(cmd_param);
                     cmd_result = ATTACH_CALLBACK(info.index, info.cb_token);
                 }
                 cmd_finish = true;
@@ -166,7 +166,7 @@ namespace sophiar {
             co_return;
         }
 
-        void send_and_wait_command(command_type _cmd, const std::any &_param) {
+        void send_and_wait_command(command_type _cmd, const boost::any &_param) {
             // copy command information
             {
                 auto lock = std::lock_guard{mu};
@@ -180,13 +180,13 @@ namespace sophiar {
             cv.wait(lock, [this] { return cmd_finish; });
         }
 
-        std::any command_helper(command_type _cmd, const std::any &_param) {
+        boost::any command_helper(command_type _cmd, const boost::any &_param) {
             send_and_wait_command(_cmd, _param);
             return cmd_result;
         }
 
         void register_variable(const std::string &name) {
-            auto var_info = std::any_cast<sophiar_pool::variable_info>(
+            auto var_info = boost::any_cast<sophiar_pool::variable_info>(
                     command_helper(QUERY_VARIABLE, name));
             auto [iter, ok] = variable_pool.emplace(std::piecewise_construct,
                                                     std::forward_as_tuple(name),
@@ -208,9 +208,9 @@ namespace sophiar {
                             return;
                         })
             };
-            store_ptr->cb_token = std::any_cast<callback_token_type>(
+            store_ptr->cb_token = boost::any_cast<callback_token_type>(
                     command_helper(REGISTER_CALLBACK, cb_func));
-            store_ptr->bind_token = std::any_cast<attach_token_type>(
+            store_ptr->bind_token = boost::any_cast<attach_token_type>(
                     command_helper(ATTACH_CALLBACK, attach_info{var_info.index, store_ptr->cb_token}));
             post(*global_context, cb_func); // query current value
         }
@@ -224,9 +224,9 @@ namespace sophiar {
             return &iter->second;
         }
 
-        std::any query_variable(const std::string &name) {
+        boost::any query_variable(const std::string &name) {
             auto store = get_variable_store(name);
-            std::any ret;
+            boost::any ret;
             {
                 auto lock = std::lock_guard{store->mu};
                 ret = store->value;
@@ -237,7 +237,7 @@ namespace sophiar {
             return ret;
         }
 
-        void update_variable(const std::string &name, const std::any &value) {
+        void update_variable(const std::string &name, const boost::any &value) {
             auto store = get_variable_store(name);
             auto var_type_index = store->var_info.type_index;
             auto raw_ptr = store->var_info.placeholder;
@@ -246,14 +246,14 @@ namespace sophiar {
                         {
                             using ValueType = decltype(real_ptr->value);
                             using ObjType = std::remove_cvref_t<decltype(*real_ptr)>;
-                            if (!value.has_value()) [[unlikely]] {
+                            if (value.empty()) [[unlikely]] {
                                 real_ptr = nullptr;
                                 return;
                             }
                             if (real_ptr == nullptr) [[unlikely]] {
                                 real_ptr = ObjType::new_instance();
                             }
-                            real_ptr->value = std::any_cast<ValueType>(value);
+                            real_ptr->value = boost::any_cast<ValueType>(value);
                             return;
                         })
             });
@@ -272,38 +272,38 @@ namespace sophiar {
     local_connection::~local_connection() = default;
 
     bool local_connection::init_object(const std::string &name) {
-        return std::any_cast<bool>(pimpl->command_helper(INIT, name));
+        return boost::any_cast<bool>(pimpl->command_helper(INIT, name));
     }
 
     bool local_connection::start_object(const std::string &name) {
-        return std::any_cast<bool>(pimpl->command_helper(START, name));
+        return boost::any_cast<bool>(pimpl->command_helper(START, name));
     }
 
     bool local_connection::stop_object(const std::string &name) {
-        return std::any_cast<bool>(pimpl->command_helper(STOP, name));
+        return boost::any_cast<bool>(pimpl->command_helper(STOP, name));
     }
 
     bool local_connection::reset_object(const std::string &name) {
-        return std::any_cast<bool>(pimpl->command_helper(RESET, name));
+        return boost::any_cast<bool>(pimpl->command_helper(RESET, name));
     }
 
     tristate_obj::state_type local_connection::query_object(const std::string &name) {
-        return std::any_cast<tristate_obj::state_type>(pimpl->command_helper(QUERY, name));
+        return boost::any_cast<tristate_obj::state_type>(pimpl->command_helper(QUERY, name));
     }
 
     bool local_connection::patch_init_config(const config_patch_type &patch) {
-        return std::any_cast<bool>(pimpl->command_helper(INIT_CONF, patch));
+        return boost::any_cast<bool>(pimpl->command_helper(INIT_CONF, patch));
     }
 
     bool local_connection::patch_start_config(const config_patch_type &patch) {
-        return std::any_cast<bool>(pimpl->command_helper(START_CONF, patch));
+        return boost::any_cast<bool>(pimpl->command_helper(START_CONF, patch));
     }
 
-    std::any local_connection::query_variable_impl(const std::string &name) {
+    boost::any local_connection::query_variable_impl(const std::string &name) {
         return pimpl->query_variable(name);
     }
 
-    void local_connection::update_variable_impl(const std::string &name, const std::any &value) {
+    void local_connection::update_variable_impl(const std::string &name, const boost::any &value) {
         pimpl->update_variable(name, value);
     }
 
@@ -314,7 +314,7 @@ namespace sophiar {
     void run_sophiar(const std::string &config_path) {
         auto config_file = std::ifstream{config_path};
         assert(config_file.is_open());
-        auto config = nlohmann::json::parse(config_file);
+        auto config = nlohmann::json::parse(config_file, nullptr, true, true); // ignore comments
         auto ok = initialize(config);
         assert(ok);
         global_context->run();

src/core/local_connection.h

@@ -5,9 +5,10 @@
 
 #include <Eigen/Geometry>
 
+#include <boost/any.hpp>
+
 #include <nlohmann/json.hpp>
 
-#include <any>
 #include <memory>
 #include <optional>
 
@@ -60,8 +61,8 @@ namespace sophiar {
 #define VARIABLE_FUNC(type) \
         std::optional<type##_obj_value_type> query_##type##_variable(const std::string &name) { \
             auto ret = query_variable_impl(name); \
-            if (!ret.has_value()) return {}; \
-            return std::any_cast<type##_obj_value_type>(ret); \
+            if (ret.empty()) return {}; \
+            return boost::any_cast<type##_obj_value_type>(ret); \
         } \
         void update_##type##_variable(const std::string &name, \
                                   const std::optional<type##_obj_value_type> &value) { \
@@ -95,9 +96,9 @@ namespace sophiar {
         struct impl;
         std::unique_ptr<impl> pimpl;
 
-        std::any query_variable_impl(const std::string &name);
+        boost::any query_variable_impl(const std::string &name);
 
-        void update_variable_impl(const std::string &name, const std::any &value);
+        void update_variable_impl(const std::string &name, const boost::any &value);
 
     };
 

src/core/small_obj.hpp

@@ -27,11 +27,27 @@ namespace sophiar {
         }
     }
 
-    template<typename DeriveT>
+    template<size_t Align>
+    struct aligned_allocator {
+        using size_type = size_t;
+        using difference_type = ptrdiff_t;
+
+        static char *malloc(size_type n) {
+            return (char *) aligned_alloc(Align, n);
+        }
+
+        static void free(void *block) {
+            ::free(block);
+        }
+    };
+
+    template<typename DeriveT,
+            typename Allocator = boost::default_user_allocator_new_delete>
     struct small_obj : public small_obj_ref_counter<DeriveT> {
 
         using pointer = boost::intrusive_ptr<DeriveT>;
-        using allocator_type = boost::object_pool<DeriveT>;
+
+        using allocator_type = boost::object_pool<DeriveT, Allocator>;
 
 //      static allocator_type allocator;
         inline static allocator_type allocator;

src/sensor/ndi/ndi_interface.cpp

@@ -26,6 +26,7 @@
 #include <boost/integer.hpp>
 #include <boost/iostreams/device/mapped_file.hpp>
 #include <boost/lexical_cast.hpp>
+#include <boost/predef.h>
 #include <boost/smart_ptr/scoped_ptr.hpp>
 #include <boost/system/error_code.hpp>
 

tests/data/ndi_interface_config.json

@@ -19,13 +19,13 @@
       "type": "ndi_interface",
       "name": "ndi",
       "init_config": {
-        "address_type": "ethernet",
+        "address_type": "serial",
         "ip": "10.0.0.5",
         "tcp_port": 8765,
         "com_port": "/dev/ttyUSB0",
         "tool_list": [
           {
-            "rom_path": "/home/tpx/data/roms/Glass_4Ball_2.rom",
+            "rom_path": "/home/tpx/data/roms/Glass_4Ball_1.rom",
             "outputs": {
               "transform": "probe_in_tracker",
               "marker_uncertainty": "probe_uncertainty"