Implemented local connection.

CMakeLists.txt

@@ -3,44 +3,13 @@ cmake_minimum_required(VERSION 3.13)
 project(Sophiar2)
 set(CMAKE_CXX_STANDARD 20)
 
-add_compile_options(-march=native)
-add_compile_options(-mno-avx) # enable avx will cause some stack pointer alignment error with Eigen
-#add_compile_definitions(CORO_SIGNAL2_USE_TIMER)
+add_subdirectory(./src)
 
-include_directories(./src)
+add_executable(${PROJECT_NAME} app/main.cpp)
 
-find_package(Boost REQUIRED COMPONENTS iostreams)
-list(APPEND BASIC_LIBS ${Boost_LIBRARIES})
-include_directories(${Boost_INCLUDE_DIRS})
+target_include_directories(${PROJECT_NAME} PRIVATE ./src)
 
-find_package(fmt REQUIRED)
-list(APPEND BASIC_LIBS fmt::fmt)
-
-find_package(spdlog REQUIRED)
-list(APPEND BASIC_LIBS spdlog::spdlog)
-add_compile_definitions(SPDLOG_ACTIVE_LEVEL=SPDLOG_LEVEL_TRACE)
-
-find_package(Eigen3 REQUIRED)
-list(APPEND BASIC_LIBS Eigen3::Eigen)
-
-find_package(nlohmann_json REQUIRED)
-list(APPEND BASIC_LIBS nlohmann_json::nlohmann_json)
-
-IF (WIN32)
-    list(APPEND BASIC_LIBS ws2_32 wsock32 winmm bcrypt)
-ELSEIF (UNIX)
-    list(APPEND BASIC_LIBS crypt)
-ENDIF ()
-
-include(src/core/CMakeLists.txt)
-include(src/algorithm/CMakeLists.txt)
-include(src/sensor/CMakeLists.txt)
-include(src/robot/CMakeLists.txt)
-
-add_executable(${PROJECT_NAME} src/main.cpp)
-
-target_link_libraries(${PROJECT_NAME} ${BASIC_LIBS})
-target_link_libraries(${PROJECT_NAME} ${SOPHIAR_LIBS})
+target_link_libraries(${PROJECT_NAME} Sophiar2Lib)
 
 add_subdirectory(app)
 add_subdirectory(tests)

app/CMakeLists.txt

@@ -1,2 +1,2 @@
 add_executable(kuka_calibration kuka_calibration.cpp)
-target_link_libraries(kuka_calibration ${BASIC_LIBS} ${SOPHIAR_LIBS})
+target_link_libraries(kuka_calibration Sophiar2Lib)

app/data/tka_ar_navigation.json

@@ -244,10 +244,10 @@
       "type": "ndi_interface",
       "name": "ndi",
       "init_config": {
-        "address_type": "ethernet",
+        "address_type": "serial",
         "ip": "10.0.0.5",
         "tcp_port": 8765,
-        "com_port": "COM3",
+        "com_port": "/dev/ttyUSB0",
         "tool_list": [
           {
             "rom_path": "/home/tpx/data/roms/GlassProbe_4Ball_4.rom",
@@ -281,7 +281,7 @@
       }
     },
     {
-      "type": "transform_obj_validity_watcher",
+      "type": "object_validity_watcher",
       "name": "probe_visibility_watcher",
       "start_config": {
         "variable_name": "probe_in_tracker"

src/CMakeLists.txt

@@ -0,0 +1,43 @@
+cmake_minimum_required(VERSION 3.13)
+
+project(Sophiar2Lib)
+set(CMAKE_CXX_STANDARD 20)
+
+add_compile_options(-march=native)
+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(.)
+
+find_package(Boost REQUIRED COMPONENTS iostreams)
+list(APPEND BASIC_LIBS ${Boost_LIBRARIES})
+include_directories(${Boost_INCLUDE_DIRS})
+
+find_package(fmt REQUIRED)
+list(APPEND BASIC_LIBS fmt::fmt)
+
+find_package(spdlog REQUIRED)
+list(APPEND BASIC_LIBS spdlog::spdlog)
+add_compile_definitions(SPDLOG_ACTIVE_LEVEL=SPDLOG_LEVEL_TRACE)
+
+find_package(Eigen3 REQUIRED)
+list(APPEND BASIC_LIBS Eigen3::Eigen)
+
+find_package(nlohmann_json REQUIRED)
+list(APPEND BASIC_LIBS nlohmann_json::nlohmann_json)
+
+IF (WIN32)
+    list(APPEND BASIC_LIBS ws2_32 wsock32 winmm bcrypt)
+ELSEIF (UNIX)
+    list(APPEND BASIC_LIBS crypt)
+ENDIF ()
+
+include(core/CMakeLists.txt)
+include(algorithm/CMakeLists.txt)
+include(sensor/CMakeLists.txt)
+include(robot/CMakeLists.txt)
+
+add_library(${PROJECT_NAME} main_empty.cpp)
+
+target_link_libraries(${PROJECT_NAME} ${BASIC_LIBS})
+target_link_libraries(${PROJECT_NAME} ${SOPHIAR_LIBS})

src/core/local_connection.cpp

@@ -0,0 +1,290 @@
+#include "local_connection.h"
+#include "core/basic_obj_types.hpp"
+#include "core/global_defs.h"
+#include "core/sophiar_manager.h"
+#include "core/sophiar_pool.h"
+
+#include <boost/asio/awaitable.hpp>
+#include <boost/asio/co_spawn.hpp>
+#include <boost/asio/detached.hpp>
+#include <boost/asio/post.hpp>
+
+#include <fmt/ostream.h>
+#include <spdlog/spdlog.h>
+
+#include <atomic>
+#include <condition_variable>
+#include <coroutine>
+#include <map>
+#include <thread>
+
+namespace sophiar {
+
+    namespace local_connection_impl {
+
+        enum command_type {
+            INIT, START, STOP, RESET, QUERY, INIT_CONF, START_CONF,
+            QUERY_VARIABLE, REGISTER_CALLBACK, ATTACH_CALLBACK
+        };
+
+        // https://en.cppreference.com/w/cpp/atomic/atomic_flag
+        struct spin_lock {
+
+            void lock() {
+                while (flag.test_and_set(std::memory_order_acquire)) // acquire lock
+                {
+#if defined(__cpp_lib_atomic_flag_test)
+                    while (flag.test(std::memory_order_relaxed)) // test lock
+#endif
+                        ; // spin
+                }
+            }
+
+            void unlock() {
+                flag.clear(std::memory_order_release); // release lock
+            }
+
+        private:
+            std::atomic_flag flag = ATOMIC_FLAG_INIT;
+        };
+
+    }
+
+    using namespace local_connection_impl;
+
+    using boost::asio::awaitable;
+    using boost::asio::co_spawn;
+    using boost::asio::detached;
+    using boost::asio::post;
+
+    using callback_token_type = sophiar_pool::callback_token_type;
+    using attach_token_type = sophiar_pool::attach_token_type;
+
+    struct local_connection::impl {
+
+        // for command
+
+        struct attach_info {
+            variable_index_type index;
+            callback_token_type cb_token;
+        };
+
+        std::mutex mu;
+        std::condition_variable cv;
+        command_type cmd;
+        std::any cmd_param, cmd_result;
+        bool cmd_finish = true;
+
+        // for variable
+
+        struct variable_store_info {
+            using variable_info = sophiar_pool::variable_info;
+
+            std::any value;
+            variable_info var_info;
+            spin_lock mu;
+            callback_token_type cb_token;
+            attach_token_type bind_token;
+        };
+
+        std::map<std::string, variable_store_info> variable_pool;
+
+        awaitable<void> process_command() {
+//            SPDLOG_INFO("Process in thread: {}.", std::this_thread::get_id());
+            {
+                auto lock = std::lock_guard{mu};
+                assert(!cmd_finish);
+                if (cmd < INIT_CONF) {
+                    auto obj_name = std::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));
+                            break;
+                        }
+                        case START: {
+                            cmd_result = co_await global_sophiar_manager->start_object(
+                                    std::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));
+                            break;
+                        }
+                        case RESET: {
+                            cmd_result = co_await global_sophiar_manager->reset_object(
+                                    std::any_cast<std::string>(cmd_param));
+                            break;
+                        }
+                        case QUERY: {
+                            cmd_result = global_sophiar_manager->query_object_state(
+                                    std::any_cast<std::string>(cmd_param));
+                            break;
+                        }
+                        default: {
+                            assert(false);
+                        }
+                    }
+                } else if (cmd < QUERY_VARIABLE) {
+                    auto patch = std::any_cast<config_patch_type>(cmd_param);
+                    switch (cmd) {
+                        case INIT_CONF: {
+                            cmd_result = global_sophiar_manager->patch_init_config(
+                                    patch.name, patch.config);
+                            break;
+                        }
+                        case START_CONF: {
+                            cmd_result = global_sophiar_manager->patch_start_config(
+                                    patch.name, patch.config);
+                            break;
+                        }
+                        default: {
+                            assert(false);
+                        }
+                    }
+                } else if (cmd == QUERY_VARIABLE) {
+                    auto var_name = std::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);
+                    cmd_result = REGISTER_CALLBACK(std::move(func));
+                } else if (cmd == ATTACH_CALLBACK) {
+                    auto info = std::any_cast<attach_info>(cmd_param);
+                    cmd_result = ATTACH_CALLBACK(info.index, info.cb_token);
+                }
+                cmd_finish = true;
+            }
+            cv.notify_one();
+            co_return;
+        }
+
+        void send_and_wait_command(command_type _cmd, const std::any &_param) {
+            // copy command information
+            {
+                auto lock = std::lock_guard{mu};
+                assert(cmd_finish);
+                cmd = _cmd;
+                cmd_param = _param;
+                cmd_finish = false;
+            }
+            co_spawn(*global_context, process_command(), detached);
+            auto lock = std::unique_lock{mu};
+            cv.wait(lock, [this] { return cmd_finish; });
+        }
+
+        std::any command_helper(command_type _cmd, const std::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>(
+                    command_helper(QUERY_VARIABLE, name));
+            auto [iter, ok] = variable_pool.emplace(std::piecewise_construct,
+                                                    std::forward_as_tuple(name),
+                                                    std::forward_as_tuple());
+            assert(ok);
+            auto store_ptr = &iter->second;
+            store_ptr->var_info = var_info;
+            auto var_type_index = var_info.type_index;
+            auto raw_ptr = var_info.placeholder;
+            std::function<void()> cb_func = [=] {
+                auto lock = std::lock_guard{store_ptr->mu};
+                LOOP_BASIC_VAR_TYPE(
+                        {
+                            if (real_ptr == nullptr) {
+                                store_ptr->value = {};
+                            } else {
+                                store_ptr->value = real_ptr->value;
+                            }
+                            return;
+                        })
+            };
+            store_ptr->cb_token = std::any_cast<callback_token_type>(
+                    command_helper(REGISTER_CALLBACK, cb_func));
+            store_ptr->bind_token = std::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
+        }
+
+        variable_store_info *get_variable_store(const std::string &name) {
+            if (!variable_pool.contains(name)) [[unlikely]] {
+                register_variable(name);
+            }
+            auto iter = variable_pool.find(name);
+            assert(iter != variable_pool.end());
+            return &iter->second;
+        }
+
+        std::any query_variable(const std::string &name) {
+            auto store = get_variable_store(name);
+            auto lock = std::lock_guard{store->mu};
+            return store->value;
+        }
+
+        void update_variable(const std::string &name, const std::any &value) {
+            auto store = get_variable_store(name);
+            auto var_type_index = store->var_info.type_index;
+            auto raw_ptr = store->var_info.placeholder;
+            post(*global_context, [=] {
+                LOOP_BASIC_VAR_TYPE(
+                        {
+                            using ValueType = decltype(real_ptr->value);
+                            if (!value.has_value()) [[unlikely]] {
+                                real_ptr = nullptr;
+                                return;
+                            }
+                            if (real_ptr == nullptr) [[unlikely]] {
+                                real_ptr = real_ptr->new_instance();
+                            }
+                            real_ptr->value = std::any_cast<ValueType>(value);
+                            return;
+                        })
+            });
+        }
+
+    };
+
+    local_connection::local_connection()
+            : pimpl(std::make_unique<impl>()) {}
+
+    local_connection::~local_connection() = default;
+
+    bool local_connection::init_object(const std::string &name) {
+        return std::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));
+    }
+
+    bool local_connection::stop_object(const std::string &name) {
+        return std::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));
+    }
+
+    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));
+    }
+
+    bool local_connection::patch_init_config(const config_patch_type &patch) {
+        return std::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));
+    }
+
+    std::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) {
+        pimpl->update_variable(name, value);
+    }
+
+}

src/core/local_connection.h

@@ -0,0 +1,97 @@
+#ifndef SOPHIAR2_LOCAL_CONNECTION_H
+#define SOPHIAR2_LOCAL_CONNECTION_H
+
+#include "tristate_obj.h"
+
+#include <Eigen/Geometry>
+
+#include <nlohmann/json.hpp>
+
+#include <any>
+#include <memory>
+#include <optional>
+
+namespace sophiar {
+
+    class local_connection {
+    public:
+
+        local_connection();
+
+        ~local_connection();
+
+        // object related
+
+        bool init_object(const std::string &name);
+
+        bool start_object(const std::string &name);
+
+        bool stop_object(const std::string &name);
+
+        bool reset_object(const std::string &name);
+
+        tristate_obj::state_type query_object(const std::string &name);
+
+        struct config_patch_type {
+            std::string name;
+            nlohmann::json config;
+        };
+
+        bool patch_init_config(const config_patch_type &patch);
+
+        bool patch_start_config(const config_patch_type &patch);
+
+        // variable related
+
+        using bool_obj_value_type = bool;
+        using u64int_obj_value_type = uint64_t;
+        using double_obj_value_type = double;
+        using scalarxyz_obj_value_type = Eigen::Vector3d;
+        using transform_obj_value_type = Eigen::Isometry3d;
+        using array6_obj_value_type = std::array<double, 6>;
+        using array7_obj_value_type = std::array<double, 7>;
+
+#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); \
+        } \
+        void update_##type##_variable(const std::string &name, \
+                                  const std::optional<type##_obj_value_type> &value) { \
+            if (value.has_value()) { \
+                update_variable_impl(name, value); \
+            } else { \
+                update_variable_impl(name, {}); \
+            } \
+        }
+
+        VARIABLE_FUNC(bool)
+
+        VARIABLE_FUNC(u64int)
+
+        VARIABLE_FUNC(double)
+
+        VARIABLE_FUNC(scalarxyz)
+
+        VARIABLE_FUNC(transform)
+
+        VARIABLE_FUNC(array6)
+
+        VARIABLE_FUNC(array7)
+
+#undef VARIABLE_FUNC
+
+    private:
+        struct impl;
+        std::unique_ptr<impl> pimpl;
+
+        std::any query_variable_impl(const std::string &name);
+
+        void update_variable_impl(const std::string &name, const std::any &value);
+
+    };
+
+}
+
+#endif //SOPHIAR2_LOCAL_CONNECTION_H

src/core/sophiar_manager.h

@@ -46,9 +46,9 @@ namespace sophiar {
 
 #ifdef SOPHIAR_TEST // for test unit only
 
-    public:
+        public:
 
-        sophiar_obj *get_object(std::string_view obj_name) const;
+            sophiar_obj *get_object(std::string_view obj_name) const;
 
 #endif // SOPHIAR_TEST
 
@@ -59,15 +59,17 @@ namespace sophiar {
 
         friend class external_controller;
 
+        friend class local_connection;
+
         void register_object_type_impl(std::string_view type_name, obj_factory_func_type func);
 
 #ifdef SOPHIAR_TEST
 
-    public:
+        public:
 
 #else
 
-        private:
+    private:
 
 #endif // SOPHIAR_TEST
 

src/core/sophiar_pool.h

@@ -73,6 +73,7 @@ namespace sophiar {
         callback_token_type register_callback(function_callback_type &&callback,
                                               function_callback_type &&exit_func = empty_callback);
 
+        [[deprecated("Not recommended to use.")]]
         callback_token_type register_coro_callback(coroutine_callback_type &&callback,
                                                    function_callback_type &&exit_func = empty_callback);
 

src/utility/variable_utility2.cpp

@@ -91,7 +91,7 @@ namespace sophiar {
 
     protected:
         awaitable<bool> on_start(const nlohmann::json &config) noexcept override {
-            create_callback(config);
+            register_variables(config);
             co_return true;
         }
 
@@ -102,12 +102,12 @@ namespace sophiar {
             auto cb_func = [=]() mutable {
                 if (raw_pointer_is_valid(var_info.placeholder, var_info.type_index)) {
                     if (is_valid)[[likely]] return;
-                    SPDLOG_DEBUG("{} becomes invalid.", var_name);
-                    is_valid = false;
-                } else {
-                    if (!is_valid)[[likely]] return;
                     SPDLOG_DEBUG("{} becomes valid.", var_name);
                     is_valid = true;
+                } else {
+                    if (!is_valid)[[likely]] return;
+                    SPDLOG_DEBUG("{} becomes invalid.", var_name);
+                    is_valid = false;
                 }
             };
             auto token = REGISTER_CALLBACK(cb_func);
@@ -116,6 +116,8 @@ namespace sophiar {
         }
     };
 
+
+
     void register_variable_utility() {
         REGISTER_TYPE2(variable_debug_watcher, "object_watcher");
         REGISTER_TYPE2(variable_validity_watcher, "object_validity_watcher");