初步实现了 sophiar_manager, 有 bug

CMakeLists.txt

@@ -20,9 +20,8 @@ list(APPEND EXTRA_LIBS spdlog::spdlog)
 find_package(Eigen3 REQUIRED)
 list(APPEND EXTRA_LIBS Eigen3::Eigen)
 
-find_package(yaml-cpp REQUIRED)
-list(APPEND EXTRA_LIBS ${YAML_CPP_LIBRARIES})
-include_directories(${YAML_CPP_INCLUDE_DIR})
+find_package(nlohmann_json REQUIRED)
+list(APPEND EXTRA_LIBS nlohmann_json::nlohmann_json)
 
 file(GLOB_RECURSE SRC_FILES ./src/*.cpp)
 add_executable(${PROJECT_NAME} ${SRC_FILES})

src/core/datanode_base.cpp

@@ -1,7 +1,10 @@
 #include "datanode_base.h"
 
+#include "core/sophiar_manager.h"
+#include "third_party/static_block.hpp"
 #include "utility/bit_operations.hpp"
 #include "utility/coro_signal.hpp"
+#include "utility/name_translator.hpp"
 #include "utility/statistic_timer.hpp"
 #include "utility/tiny_signal.hpp"
 
@@ -27,28 +30,48 @@ namespace sophiar {
 
     using namespace boost::asio::experimental::awaitable_operators;
 
-    template<typename FreqTag>
-    struct datanode_base<FreqTag>::impl {
+    name_translator<datanode_base::trigger_mode_type> trigger_mode_translator;
 
-        using state_type = typename tristate_obj<FreqTag>::state_type;
-        using connection_signal_type = tiny_signal<typename data_packet::pointer>;
+    static_block {
+        using mode_type = datanode_base::trigger_mode_type;
+        trigger_mode_translator.register_item("manual", mode_type::MANUAL);
+        trigger_mode_translator.register_item("input", mode_type::INPUT);
+        trigger_mode_translator.register_item("periodic", mode_type::PERIODIC);
+    };
+
+    struct datanode_base::impl {
+
+        using update_signal_type = tiny_signal<data_pointer_type>;
+        using update_slot_type = update_signal_type::slot_type;
+        using trigger_slot_type = tiny_signal<>::slot_type;
 
-        struct connection_node_type : public connection_signal_type::listener_type {
-            impl *p_this;
+        struct update_slot_impl_type : public update_slot_type {
+            impl *p_this = nullptr;
             uint8_t channel_index;
 
-            void on_signal_received(typename data_packet::pointer packet) override {
-                p_this->update_input_data(channel_index, packet);
+            void on_signal_received(data_pointer_type packet) override {
+                assert(p_this != nullptr);
+                p_this->update_input_data(channel_index, std::move(packet));
+            }
+        };
+
+        struct trigger_slot_impl_type : public trigger_slot_type {
+            impl *p_this = nullptr;
+
+            void on_signal_received() override {
+                assert(p_this != nullptr);
+                co_spawn(get_context(), p_this->trigger(), detached);
             }
         };
 
         datanode_base *q_this = nullptr;
 
-        typename data_packet::pointer input_data[MAX_CHANNEL_CNT];
-        typename data_packet::pointer output_data[MAX_CHANNEL_CNT];
+        data_pointer_type input_data[MAX_CHANNEL_CNT];
+        data_pointer_type output_data[MAX_CHANNEL_CNT];
+
+        uint8_t input_total = 0, output_total = 0;
+        update_signal_type output_signal[MAX_CHANNEL_CNT];
 
-        connection_node_type channel_input_nodes[MAX_CHANNEL_CNT];
-        connection_signal_type channel_output_list[MAX_CHANNEL_CNT];
         channel_mask_type input_update_mask = 0;
         channel_mask_type output_updated_mask = 0;
 
@@ -74,12 +97,59 @@ namespace sophiar {
         bool exec_block = false;
 
         impl()
-                : trigger_timer(datanode_base::get_context()),
-                  exec_cancel_signal(datanode_base::get_context()),
-                  run_finished_signal(datanode_base::get_context()) {
-            for (uint8_t i = 0; i < MAX_CHANNEL_CNT; ++i) {
-                channel_input_nodes[i].p_this = this;
-                channel_input_nodes[i].channel_index = i;
+                : trigger_timer(get_context()),
+                  exec_cancel_signal(get_context()),
+                  run_finished_signal(get_context()) {}
+
+        void create_trigger_slot() {
+            auto trigger_slot = new trigger_slot_impl_type;
+            trigger_slot->p_this = this;
+            get_manager().register_slot<>(q_this, "trigger", *trigger_slot);
+        }
+
+        void create_input_slot(const std::string &name) {
+            assert(input_total + 1 <= MAX_CHANNEL_CNT);
+            auto slot_index = input_total++;
+            auto update_slot = new update_slot_impl_type;
+            update_slot->p_this = this;
+            update_slot->channel_index = slot_index;
+            get_manager().register_slot<data_pointer_type>(q_this, name, *update_slot);
+        }
+
+        void create_output_signal(const std::string &name) {
+            assert(output_total + 1 <= MAX_CHANNEL_CNT);
+            auto signal_index = output_total++;
+            get_manager().register_signal(q_this, name, output_signal[signal_index]);
+        }
+
+        void load_start_config(const nlohmann::json &config) {
+            assert(config.contains("trigger_mode"));
+            assert(config["trigger_mode"].is_string());
+            auto trigger_mode_str = config["trigger_mode"].get<std::string>();
+            trigger_mode = trigger_mode_translator.translate(trigger_mode_str);
+
+            switch (trigger_mode) {
+                case trigger_mode_type::INPUT: {
+                    assert(config.contains("input_mask"));
+                    assert(config["input_mask"].is_number_integer());
+                    trigger_input_mask = config["input_mask"].get<uint64_t>();
+                    break;
+                }
+                case trigger_mode_type::PERIODIC: {
+                    assert(config.contains("exec_interval_ms"));
+                    assert(config["exec_interval_ms"].is_number_integer());
+                    trigger_interval = std::chrono::milliseconds(
+                            config["exec_interval_ms"].get<uint64_t>());
+                    break;
+                }
+                default:
+                    break;
+            }
+
+            if (config.contains("minimal_exec_interval_ms")) {
+                assert(config["minimal_exec_interval_ms"].is_number_integer());
+                minimal_exec_interval = std::chrono::milliseconds(
+                        config["minimal_exec_interval_ms"].get<uint64_t>());
             }
         }
 
@@ -92,12 +162,15 @@ namespace sophiar {
         }
 
         void commit_output() {
-            for (uint8_t i = 0; i < MAX_CHANNEL_CNT; ++i) {
-                if (test_bit(output_updated_mask, i)) {
-                    channel_output_list[i].emit_signal(output_data[i]);
-                }
+            // https://lemire.me/blog/2018/02/21/iterating-over-set-bits-quickly/
+            auto &mask = output_updated_mask;
+            while (mask != 0) {
+                auto tmp = mask & -mask;
+                auto pos = std::countr_zero(tmp);
+                assert(pos < output_total);
+                output_signal[pos].emit(output_data[pos]);
+                mask ^= tmp;
             }
-            output_updated_mask = 0;
         }
 
         awaitable<bool> exec_impl(timestamp_type current_ts = current_timestamp()) {
@@ -151,7 +224,8 @@ namespace sophiar {
             co_return;
         }
 
-        awaitable<bool> on_start() {
+        awaitable<bool> on_start(const nlohmann::json &config) {
+            load_start_config(config);
             trigger_stimer.reset();
             exec_stimer.reset();
             exec_block = false;
@@ -161,7 +235,7 @@ namespace sophiar {
                 } else if (trigger_interval.count() == 0) {
                     // TODO show warning, running without interval
                 }
-                co_spawn(q_this->get_context(), periodic_run(), detached);
+                co_spawn(get_context(), periodic_run(), detached);
             }
             co_return true;
         }
@@ -178,14 +252,13 @@ namespace sophiar {
             co_return;
         }
 
-        void update_input_data(uint8_t channel_index,
-                               const typename data_packet::pointer &data) {
-            assert(channel_index < MAX_CHANNEL_CNT);
-            input_data[channel_index] = data;
+        void update_input_data(uint8_t channel_index, data_pointer_type data) {
+            assert(channel_index < input_total);
+            input_data[channel_index] = std::move(data);
             set_bit(input_update_mask, channel_index);
             if (check_is_running() && trigger_mode == trigger_mode_type::INPUT) {
                 if (check_input_trigger_condition()) {
-                    co_spawn(q_this->get_context(), trigger(), detached);
+                    co_spawn(get_context(), trigger(), detached);
                 }
             }
         }
@@ -224,134 +297,73 @@ namespace sophiar {
 
     };
 
-    template<typename FreqTag>
-    awaitable<bool> datanode_base<FreqTag>::on_start() {
-        return pimpl->on_start();
+    awaitable<bool> datanode_base::on_start(const nlohmann::json &config) {
+        return pimpl->on_start(config);
     }
 
-    template<typename FreqTag>
-    awaitable<void> datanode_base<FreqTag>::on_stop() {
+    awaitable<void> datanode_base::on_stop() {
         return pimpl->on_stop();
     }
 
-    template<typename FreqTag>
-    datanode_base<FreqTag>::datanode_base()
+    datanode_base::datanode_base()
             : pimpl(std::make_unique<impl>()) {
         pimpl->q_this = this;
     }
 
-    template<typename FreqTag>
-    void datanode_base<FreqTag>::update_input_data(uint8_t channel_index,
-                                                   const typename data_packet::pointer &data) {
-        pimpl->update_input_data(channel_index, data);
+    void datanode_base::load_construct_config(const nlohmann::json &json) {
+        pimpl->create_trigger_slot();
     }
 
-    template<typename FreqTag>
-    boost::asio::awaitable<bool> datanode_base<FreqTag>::trigger() {
-        return pimpl->trigger();
+    void datanode_base::register_input(const std::string &name) {
+        pimpl->create_input_slot(name);
     }
 
-    template<typename FreqTag>
-    bool datanode_base<FreqTag>::cancel_pending() {
-        return pimpl->cancel_pending();
+    void datanode_base::register_output(const std::string &name) {
+        pimpl->create_output_signal(name);
     }
 
-    template<typename FreqTag>
-    bool datanode_base<FreqTag>::set_trigger_mode(trigger_mode_type mode) {
-        if (pimpl->check_is_running()) return false;
-        pimpl->trigger_mode = mode;
-        return true;
+    bool datanode_base::cancel_pending() {
+        return pimpl->cancel_pending();
     }
 
-    template<typename FreqTag>
-    typename datanode_base<FreqTag>::data_packet::pointer
-    datanode_base<FreqTag>::get_input_data(uint8_t channel_index) {
-        assert(channel_index < MAX_CHANNEL_CNT);
+    datanode_base::data_pointer_type
+    datanode_base::get_input_data(uint8_t channel_index) {
+        assert(channel_index < pimpl->input_total);
         return pimpl->input_data[channel_index];
     }
 
-    template<typename FreqTag>
-    void datanode_base<FreqTag>::set_output_data(uint8_t channel_index,
-                                                 const typename data_packet::pointer &data) {
-        assert(channel_index < MAX_CHANNEL_CNT);
-        pimpl->output_data[channel_index] = data;
+    void datanode_base::set_output_data(uint8_t channel_index, data_pointer_type data) {
+        assert(channel_index < pimpl->output_total);
+        pimpl->output_data[channel_index] = std::move(data);
         set_bit(pimpl->output_updated_mask, channel_index);
     }
 
-    template<typename FreqTag>
-    typename datanode_base<FreqTag>::exec_state_type
-    datanode_base<FreqTag>::get_exec_state() const {
+    datanode_base::exec_state_type datanode_base::get_exec_state() const {
         return pimpl->exec_state;
     }
 
-    template<typename FreqTag>
-    timestamp_type datanode_base<FreqTag>::get_last_exec_ts() const {
+    timestamp_type datanode_base::get_last_exec_ts() const {
         return pimpl->last_exec_ts;
     }
 
-    template<typename FreqTag>
-    bool datanode_base<FreqTag>::is_last_exec_success() const {
+    bool datanode_base::is_last_exec_success() const {
         return pimpl->last_exec_success;
     }
 
-    template<typename FreqTag>
-    std::string datanode_base<FreqTag>::get_statistic_info_as_string() const {
-        return fmt::format("\nTrigger timer: {}\nExec timer: {}",
-                           pimpl->trigger_stimer.to_string(), pimpl->exec_stimer.to_string());
-    }
-
-    template<typename FreqTag>
-    void datanode_base<FreqTag>::connect(datanode_base<FreqTag> &sender, uint8_t sender_channel_index,
-                                         datanode_base<FreqTag> &receiver, uint8_t receiver_channel_index) {
-        assert(sender_channel_index < MAX_CHANNEL_CNT);
-        assert(receiver_channel_index < MAX_CHANNEL_CNT);
-        auto &receiver_channel_node = receiver.pimpl->channel_input_nodes[receiver_channel_index];
-        sender.pimpl->channel_output_list[sender_channel_index].add_listener(receiver_channel_node);
-    }
-
-    template<typename FreqTag>
-    void datanode_base<FreqTag>::disconnect(uint8_t input_channel_index) {
-        assert(input_channel_index < MAX_CHANNEL_CNT);
-        auto &input_node = pimpl->channel_input_nodes[input_channel_index];
-        if (input_node.is_linked()) {
-            input_node.unlink();
-        }
-    }
-
-    template<typename FreqTag>
-    void datanode_base<FreqTag>::set_trigger_input_mask(channel_mask_type mask) {
-        pimpl->trigger_input_mask = mask;
-    }
+//    template<typename FreqTag>
+//    std::string datanode_base<FreqTag>::get_statistic_info_as_string() const {
+//        return fmt::format("\nTrigger timer: {}\nExec timer: {}",
+//                           pimpl->trigger_stimer.to_string(), pimpl->exec_stimer.to_string());
+//    }
 
-    template<typename FreqTag>
-    void datanode_base<FreqTag>::set_trigger_interval(std::chrono::milliseconds interval) {
-        pimpl->trigger_interval = interval;
-    }
-
-    template<typename FreqTag>
-    void datanode_base<FreqTag>::set_minimal_exec_interval(std::chrono::milliseconds interval) {
-        pimpl->minimal_exec_interval = interval;
-    }
-
-    template<typename FreqTag>
-    void datanode_base<FreqTag>::enable_statistic_timer() {
+    void datanode_base::enable_statistic_timer() {
         pimpl->enable_stimer = true;
     }
 
-    template<typename FreqTag>
-    void datanode_base<FreqTag>::disable_statistic_timer() {
+    void datanode_base::disable_statistic_timer() {
         pimpl->enable_stimer = false;
     }
 
-    template<typename FreqTag>
-    datanode_base<FreqTag>::~datanode_base() = default;
-
-    // 显式实例化
-
-    template
-    class datanode_base<high_freq_tag>;
-
-    template
-    class datanode_base<low_freq_tag>;
+    datanode_base::~datanode_base() = default;
 
 }

src/core/datanode_base.h

@@ -12,19 +12,9 @@
 
 namespace sophiar {
 
-    template<typename FreqTag>
-    class datanode_base : public tristate_obj<FreqTag> {
+    class datanode_base : public tristate_obj {
     public:
 
-        struct data_packet : public versatile_data,
-                             public small_obj<data_packet> {
-
-            void copy_content(const versatile_data &other) {
-                std::memcpy(dynamic_cast<versatile_data *>(this), &other, sizeof(versatile_data));
-            }
-
-        };
-
         enum class exec_state_type {
             IDLE,
             PENDING,
@@ -37,21 +27,18 @@ namespace sophiar {
             PERIODIC
         };
 
-        static constexpr uint8_t MAX_CHANNEL_CNT = 8;
-        using channel_mask_type = uint8_t;
+        static constexpr auto MAX_CHANNEL_CNT = 64;
+        using channel_mask_type = uint64_t;
         static_assert(sizeof(channel_mask_type) * CHAR_BIT >= MAX_CHANNEL_CNT);
 
+        using data_pointer_type = versatile_obj::pointer;
+
         datanode_base();
 
         ~datanode_base() override;
 
-        bool set_trigger_mode(trigger_mode_type mode);
-
-        void set_trigger_input_mask(channel_mask_type mask);
-
-        void set_trigger_interval(std::chrono::milliseconds interval);
-
-        void set_minimal_exec_interval(std::chrono::milliseconds interval);
+        // 创建 trigger slot
+        void load_construct_config(const nlohmann::json &json) override;
 
         exec_state_type get_exec_state() const;
 
@@ -63,30 +50,25 @@ namespace sophiar {
 
         void disable_statistic_timer();
 
-        std::string get_statistic_info_as_string() const;
-
-        void update_input_data(uint8_t channel_index,
-                               const typename data_packet::pointer &data);
-
-        boost::asio::awaitable<bool> trigger();
+//        std::string get_statistic_info_as_string() const;
 
         bool cancel_pending();
 
-        static void connect(datanode_base &sender, uint8_t sender_channel_index,
-                            datanode_base &receiver, uint8_t receiver_channel_index);
+    protected: // for child class to use
 
-        void disconnect(uint8_t input_channel_index);
+        void register_input(const std::string &name);
 
-    protected:
+        void register_output(const std::string &name);
 
-        typename data_packet::pointer get_input_data(uint8_t channel_index);
+        data_pointer_type get_input_data(uint8_t channel_index);
 
-        void set_output_data(uint8_t channel_index,
-                             const typename data_packet::pointer &data);
+        void set_output_data(uint8_t channel_index, data_pointer_type data);
 
         virtual boost::asio::awaitable<bool> exec() = 0;
 
-        boost::asio::awaitable<bool> on_start() override;
+    protected: // implementation
+
+        boost::asio::awaitable<bool> on_start(const nlohmann::json &config) override;
 
         boost::asio::awaitable<void> on_stop() override;
 

src/core/global_io_context.hpp

@@ -1,31 +0,0 @@
-#ifndef SOPHIAR2_GLOBAL_IO_CONTEXT_HPP
-#define SOPHIAR2_GLOBAL_IO_CONTEXT_HPP
-
-#include <boost/asio/io_context.hpp>
-
-namespace sophiar {
-
-    extern boost::asio::io_context high_freq_context;
-    extern boost::asio::io_context low_freq_context;
-
-    struct high_freq_tag;
-    struct low_freq_tag;
-
-    template<typename FreqTag>
-    struct use_context {
-        static auto &get_context();
-    };
-
-    template<>
-    inline auto &use_context<high_freq_tag>::get_context() {
-        return high_freq_context;
-    }
-
-    template<>
-    inline auto &use_context<low_freq_tag>::get_context() {
-        return low_freq_context;
-    }
-
-}
-
-#endif //SOPHIAR2_GLOBAL_IO_CONTEXT_HPP

src/core/sophiar_manager.cpp

@@ -0,0 +1,516 @@
+#include "sophiar_manager.h"
+#include "core/sophiar_obj.hpp"
+#include "core/timestamp_helper.hpp"
+#include "core/tristate_obj.h"
+#include "utility/debug_utility.hpp"
+#include "utility/named_vector.hpp"
+
+#include <boost/iterator/counting_iterator.hpp>
+
+#include <fmt/format.h>
+
+#include <algorithm>
+#include <coroutine>
+#include <iterator>
+#include <list>
+#include <stack>
+#include <unordered_map>
+#include <unordered_set>
+#include <vector>
+#include <boost/asio/awaitable.hpp>
+
+namespace sophiar {
+
+    using boost::asio::awaitable;
+
+    struct sophiar_manager::impl {
+
+        using mode_index_type = uint8_t;
+
+        struct config_info {
+            nlohmann::json config;
+            timestamp_type update_ts;
+        };
+
+        using mode_config_pool_type = std::unordered_map<mode_index_type, config_info>;
+
+        using obj_type_index_type = uint8_t;
+        using obj_factory_func_pool_type = named_vector<obj_type_index_type, obj_factory_func_type>;
+
+        struct obj_info {
+            sophiar_obj *ptr;
+            timestamp_type last_init_config_ts;
+            timestamp_type last_start_config_ts;
+            mode_config_pool_type init_config_pool;
+            mode_config_pool_type start_config_pool;
+        };
+
+        using obj_index_type = uint16_t;
+        using obj_pool_type = named_vector<obj_index_type, obj_info>;
+        using obj_set_type = std::unordered_set<obj_index_type>;
+        using obj_ptr_index_map_type = std::unordered_map<sophiar_obj *, obj_index_type>;
+
+        using slot_list_type = std::list<tiny_slot_base *>;
+        using slot_iter_type = slot_list_type::iterator;
+
+        struct slot_info {
+            slot_demuxer_base *demuxer = nullptr;
+            tiny_slot_base *direct_slot = nullptr; // 真正的 slot
+            std::stack<tiny_slot_base *> free_slot_pool; // 从 demuxer 创建的，未使用的 slot
+            slot_list_type used_slot_pool; // 从 demuxer 创建的，正在使用的 slot
+        };
+
+        using signal_index_type = uint16_t;
+        using slot_index_type = uint16_t;
+        using signal_pool_type = named_vector<signal_index_type, tiny_signal_base *>;
+        using slot_pool_type = named_vector<slot_index_type, slot_info>;
+
+        struct connection_info {
+            bool is_connected = false;
+            bool is_direct_connected = false; // if false, slot_iter is valid
+            signal_index_type signal_index;
+            slot_index_type slot_index;
+            slot_iter_type slot_iter;
+        };
+
+        using connection_index_type = uint32_t;
+        using connection_pool_type = named_vector<connection_index_type, connection_info>;
+        using connection_set_type = std::unordered_set<connection_index_type>;
+
+        struct mode_info {
+            obj_set_type obj_set; // 需要运行的对象
+            connection_set_type connection_set; // 需要建立的连接
+            std::vector<mode_index_type> degrade_list; // 降级尝试列表
+        };
+
+        using mode_pool_type = named_vector<mode_index_type, mode_info>;
+
+        sophiar_manager *q_this = nullptr;
+
+        obj_factory_func_pool_type obj_factory_func_pool;
+        obj_pool_type obj_pool;
+        obj_ptr_index_map_type obj_ptr_index_map;
+
+        signal_pool_type signal_pool;
+        slot_pool_type slot_pool;
+
+        connection_pool_type connection_pool;
+
+        mode_index_type current_mode = 0; // all_down
+        mode_pool_type mode_pool;
+
+        std::string get_obj_name_by_ptr(sophiar_obj *obj) const {
+            assert(obj_ptr_index_map.contains(obj));
+            auto obj_index = obj_ptr_index_map.at(obj);
+            return obj_pool.to_name_by_index(obj_index);
+        }
+
+        auto get_tristate_ptr(obj_index_type obj_index) {
+            return dynamic_cast<tristate_obj *>(obj_pool[obj_index].ptr);
+        }
+
+        static auto get_signal_uri(const std::string &signal_obj_name,
+                                   const std::string &signal_name) {
+            return fmt::format("signal://{}/{}", signal_obj_name, signal_name);
+        }
+
+        static auto get_slot_uri(const std::string &slot_obj_name,
+                                 const std::string &slot_name) {
+            return fmt::format("slot://{}/{}", slot_obj_name, slot_name);
+        }
+
+        void enable_connection(connection_index_type conn_index) {
+            auto &conn_info = connection_pool[conn_index];
+            assert(!conn_info.is_connected);
+            auto signal = signal_pool[conn_info.signal_index];
+            auto &slot_info = slot_pool[conn_info.slot_index];
+            if (!slot_info.direct_slot->is_linked()) { // 直接连接
+                signal->add_slot_base(slot_info.direct_slot);
+                conn_info.is_direct_connected = true;
+            } else { // 从 muxer 创建的 slot 连接
+                if (slot_info.free_slot_pool.empty()) { // 获得一个新的 slot
+                    auto new_slot = slot_info.demuxer->new_slot();
+                    slot_info.free_slot_pool.push(new_slot);
+                }
+                auto target_slot = slot_info.free_slot_pool.top();
+                slot_info.free_slot_pool.pop();
+                signal->add_slot_base(target_slot);
+                slot_info.used_slot_pool.push_front(target_slot);
+                conn_info.is_direct_connected = false;
+                conn_info.slot_iter = slot_info.used_slot_pool.begin();
+            }
+            conn_info.is_connected = true;
+        }
+
+        void disable_connection(connection_index_type conn_index) {
+            auto &conn_info = connection_pool[conn_index];
+            assert(conn_info.is_connected);
+            auto &slot_info = slot_pool[conn_info.slot_index];
+            if (conn_info.is_direct_connected) {
+                slot_info.direct_slot->unlink();
+            } else {
+                auto target_slot = *(conn_info.slot_iter);
+                target_slot->unlink();
+                slot_info.used_slot_pool.erase(conn_info.slot_iter);
+                slot_info.free_slot_pool.push(target_slot);
+            }
+            conn_info.is_connected = false;
+        }
+
+        awaitable<bool> try_switch_mode(mode_index_type mode_index) { // 尝试切换模式
+            auto &mode_info = mode_pool[mode_index];
+            using state_type = tristate_obj::state_type;
+
+            // 停止不需要的对象
+            for (obj_index_type obj_index = 0; obj_index < obj_pool.size(); ++obj_index) {
+                auto tristate_ptr = get_tristate_ptr(obj_index);
+                if (tristate_ptr != nullptr && // not tristate_obj
+                    tristate_ptr->get_state() == state_type::RUNNING &&
+                    !mode_info.obj_set.contains(obj_index)) {
+
+                    co_await tristate_ptr->stop();
+                    assert(tristate_ptr->get_state() != state_type::RUNNING &&
+                           tristate_ptr->get_state() != state_type::STOPPING);
+                }
+            }
+
+            // 禁用不需要的连接
+            for (connection_index_type conn_index = 0; conn_index < connection_pool.size(); ++conn_index) {
+                auto &conn_info = connection_pool[conn_index];
+                if (conn_info.is_connected &&
+                    !mode_info.connection_set.contains(conn_index)) {
+
+                    disable_connection(conn_index);
+                    assert(conn_info.is_connected == false);
+                }
+            }
+
+            // 启用需要的连接
+            for (auto conn_index: mode_info.connection_set) {
+                auto &conn_info = connection_pool[conn_index];
+                if (!conn_info.is_connected) {
+                    enable_connection(conn_index);
+                    assert(conn_info.is_connected == true);
+                }
+            }
+
+            // 启动需要的对象
+            for (auto obj_index: mode_info.obj_set) {
+                auto &obj_info = obj_pool[obj_index];
+                auto tristate_ptr = get_tristate_ptr(obj_index);
+                if (tristate_ptr == nullptr) continue; // not tristate
+
+                if (!tristate_ptr->is_stable()) {
+                    // TODO show low, cannot switch mode because some obj is unstable.
+                    co_return false;
+                }
+
+                assert(obj_info.init_config_pool.contains(mode_index));
+                assert(obj_info.start_config_pool.contains(mode_index));
+                auto &init_config = obj_info.init_config_pool.at(mode_index);
+                auto &start_config = obj_info.start_config_pool.at(mode_index);
+
+                // check if start config is updated
+                if (tristate_ptr->get_state() == state_type::RUNNING) {
+                    assert(obj_info.last_start_config_ts <= start_config.update_ts);
+                    if (obj_info.last_start_config_ts < start_config.update_ts) {
+                        co_await tristate_ptr->stop();
+                        assert(tristate_ptr->is_stable());
+                    }
+                }
+
+                // check if init config is updated
+                if (tristate_ptr->get_state() != state_type::INITIAL) {
+                    assert(tristate_ptr->get_state() == state_type::PENDING ||
+                           tristate_ptr->get_state() == state_type::RUNNING);
+                    assert(obj_info.last_init_config_ts <= init_config.update_ts);
+                    if (obj_info.last_init_config_ts < init_config.update_ts) {
+                        co_await tristate_ptr->reset();
+                        assert(tristate_ptr->is_stable());
+                    }
+                }
+
+                // is still running, do nothing
+                if (tristate_ptr->get_state() == state_type::RUNNING) continue;
+
+                // if not initialized, make it to be pending
+                if (tristate_ptr->get_state() == state_type::INITIAL) {
+                    CO_ENSURE(tristate_ptr->init(init_config.config))
+                    obj_info.last_init_config_ts = init_config.update_ts;
+                }
+
+                assert(tristate_ptr->get_state() == state_type::PENDING);
+                CO_ENSURE(tristate_ptr->start(start_config.config))
+                obj_info.last_start_config_ts = start_config.update_ts;
+                assert(tristate_ptr->get_state() == state_type::RUNNING);
+            }
+
+            co_return true;
+        }
+
+        awaitable<bool> switch_mode_impl(mode_index_type target_mode) { // 尝试切换模式，如果失败就降级
+            bool ok = co_await try_switch_mode(target_mode);
+            if (ok) co_return true;
+            for (auto degrade_mode: mode_pool[target_mode].degrade_list) {
+                ok = co_await try_switch_mode(degrade_mode);
+                if (ok) break;
+            }
+            co_return false;
+        }
+
+        obj_index_type create_object(const std::string &type_name,
+                                     const std::string &obj_name) {
+
+            auto type_index = obj_factory_func_pool.to_index_by_name(type_name);
+            auto factory_func = obj_factory_func_pool[type_index];
+            auto obj_ptr = factory_func();
+
+            auto obj_index = obj_pool.new_elem(obj_name);
+            auto &obj_info = obj_pool[obj_index];
+            obj_info.ptr = obj_ptr;
+
+            assert(!obj_ptr_index_map.contains(obj_ptr));
+            obj_ptr_index_map[obj_ptr] = obj_index;
+            return obj_index;
+        }
+
+        void register_signal(sophiar_obj *obj,
+                             const std::string &signal_name,
+                             tiny_signal_base *signal_base) {
+            auto obj_name = get_obj_name_by_ptr(obj);
+            auto signal_uri = get_signal_uri(obj_name, signal_name);
+            auto signal_index = signal_pool.new_elem(signal_uri);
+            signal_pool[signal_index] = signal_base;
+        }
+
+        void register_slot(sophiar_obj *obj,
+                           const std::string &slot_name,
+                           tiny_slot_base *slot_base,
+                           slot_demuxer_base *demuxer_base) {
+            auto obj_name = get_obj_name_by_ptr(obj);
+            auto slot_uri = get_slot_uri(obj_name, slot_name);
+            auto slot_index = slot_pool.new_elem(slot_uri);
+            auto &slot_info = slot_pool[slot_index];
+            slot_info.demuxer = demuxer_base;
+            slot_info.direct_slot = slot_base;
+        }
+
+        std::vector<mode_index_type> get_valid_modes(const nlohmann::json &config) const {
+            if (config.is_string()) {
+                assert(config.get<std::string>() == "all");
+                return {boost::make_counting_iterator(static_cast<mode_index_type>(1)), // 0 is reserved for all_down
+                        boost::make_counting_iterator(mode_pool.size())};
+            }
+            std::vector<mode_index_type> ret;
+            assert(config.is_array());
+            for (auto &mode_json: config) {
+                assert(mode_json.is_string());
+                auto mode_name = mode_json.get<std::string>();
+                ret.push_back(mode_pool.to_index_by_name(mode_name));
+            }
+            return ret;
+        }
+
+        std::vector<mode_index_type> get_valid_modes_for_obj(const nlohmann::json &config,
+                                                             obj_index_type obj_index) const {
+            auto candidate = get_valid_modes(config);
+            std::vector<mode_index_type> ret;
+            std::copy_if(candidate.begin(), candidate.end(),
+                         std::back_inserter(ret),
+                         [=](mode_index_type mode_index) {
+                             return mode_pool[mode_index].obj_set.contains(obj_index);
+                         });
+            return ret;
+        }
+
+        void build_mode(const nlohmann::json &config) {
+            assert(config.contains("name"));
+            assert(config["name"].is_string());
+            auto mode_name = config["name"].get<std::string>();
+            auto mode_index = mode_pool.new_elem(mode_name);
+            auto &mode_info = mode_pool[mode_index];
+            if (config.contains("degrade_list")) {
+                assert(config["degrade_list"].is_array());
+                for (auto &degrade_mode_json: config["degrade_list"]) {
+                    assert(degrade_mode_json.is_string());
+                    auto degrade_mode_name = degrade_mode_json.get<std::string>();
+                    auto degrade_mode_index = mode_pool.to_index_by_name(degrade_mode_name);
+                    mode_info.degrade_list.push_back(degrade_mode_index);
+                }
+            }
+            mode_info.degrade_list.push_back(0); // all_down
+        }
+
+        void build_object(const nlohmann::json &config) {
+            assert(config.contains("type"));
+            assert(config.contains("name"));
+            assert(config["type"].is_string());
+            assert(config["name"].is_string());
+            auto type_name = config["type"].get<std::string>();
+            auto obj_name = config["name"].get<std::string>();
+            auto obj_index = create_object(type_name, obj_name);
+            auto &obj_info = obj_pool[obj_index];
+            auto obj_ptr = obj_info.ptr;
+
+            assert(config.contains("enabled_modes"));
+            auto enabled_modes = get_valid_modes(config["enabled_modes"]);
+            for (auto mode_index: enabled_modes) {
+                mode_pool[mode_index].obj_set.insert(obj_index);
+            }
+
+            assert(config.contains("construct_config"));
+            obj_ptr->load_construct_config(config["construct_config"]);
+
+            auto tristate_ptr = dynamic_cast<tristate_obj *>(obj_ptr);
+            if (tristate_ptr == nullptr) return; // not tristate obj
+
+            auto current_ts = current_timestamp();
+            assert(config.contains("init_configs"));
+            assert(config["init_configs"].is_array());
+            for (auto &config_json: config["init_configs"]) {
+                assert(config_json.contains("modes"));
+                auto mode_list = get_valid_modes_for_obj(config_json["modes"], obj_index);
+                if (mode_list.empty()) {
+                    // TODO show log, invalid mode
+                    continue;
+                }
+                assert(config_json.contains("config"));
+                auto &init_config = config_json["config"];
+                for (auto mode_index: mode_list) {
+                    auto &config_info = obj_info.init_config_pool[mode_index];
+                    config_info.update_ts = current_ts;
+                    config_info.config = init_config;
+                }
+            }
+
+            assert(config.contains("start_configs"));
+            assert(config["start_configs"].is_array());
+            for (auto &config_json: config["start_configs"]) {
+                assert(config_json.contains("modes"));
+                auto mode_list = get_valid_modes_for_obj(config_json["modes"], obj_index);
+                if (mode_list.empty()) {
+                    // TODO show log, invalid mode config
+                    continue;
+                }
+                assert(config_json.contains("config"));
+                auto &start_config = config_json["config"];
+                for (auto mode_index: mode_list) {
+                    auto &config_info = obj_info.start_config_pool[mode_index];
+                    config_info.update_ts = current_ts;
+                    config_info.config = start_config;
+                }
+            }
+
+            // fill default configs
+            for (auto mode_index: enabled_modes) {
+                if (!obj_info.init_config_pool.contains(mode_index)) {
+                    // TODO show log
+                    auto &config_info = obj_info.init_config_pool[mode_index];
+                    config_info.update_ts = current_ts;
+                    config_info.config = {};
+                }
+                if (!obj_info.start_config_pool.contains(mode_index)) {
+                    // TODO show log
+                    auto &config_info = obj_info.start_config_pool[mode_index];
+                    config_info.update_ts = current_ts;
+                    config_info.config = {};
+                }
+            }
+        }
+
+        void build_graph(const nlohmann::json &config) {
+            mode_pool.new_elem("all_down");
+            assert(mode_pool.to_index_by_name("all_down") == 0);
+
+            assert(config.contains("mode_list"));
+            assert(config["mode_list"].is_array());
+            for (auto &mode_json: config["mode_list"]) {
+                build_mode(mode_json);
+            }
+
+            assert(config.contains("object_list"));
+            assert(config["object_list"].is_array());
+            for (auto &obj_json: config["object_list"]) {
+                build_object(obj_json);
+            }
+
+            assert(config.contains("connection_list"));
+            assert(config["connection_list"].is_array());
+            for (auto &part_json: config["connection_list"]) {
+                assert(part_json.contains("modes"));
+                auto mode_list = get_valid_modes(part_json["modes"]);
+                assert(part_json.contains("connections"));
+                assert(part_json["connections"].is_array());
+                for (auto &conn_json: part_json["connections"]) {
+                    assert(conn_json.contains("signal_object"));
+                    assert(conn_json.contains("signal_name"));
+                    assert(conn_json.contains("slot_object"));
+                    assert(conn_json.contains("slot_name"));
+                    assert(conn_json["signal_object"].is_string());
+                    assert(conn_json["signal_name"].is_string());
+                    assert(conn_json["slot_object"].is_string());
+                    assert(conn_json["slot_name"].is_string());
+                    auto signal_obj_name = conn_json["signal_object"].get<std::string>();
+                    auto signal_name = conn_json["signal_name"].get<std::string>();
+                    auto slot_obj_name = conn_json["slot_object"].get<std::string>();
+                    auto slot_name = conn_json["slot_name"].get<std::string>();
+                    auto signal_uri = get_signal_uri(signal_obj_name, signal_name);
+                    auto slot_uri = get_slot_uri(slot_obj_name, slot_name);
+                    auto signal_index = signal_pool.to_index_by_name(signal_uri);
+                    auto slot_index = slot_pool.to_index_by_name(slot_uri);
+                    auto conn_uri = fmt::format("connection://{}/{}", signal_index, slot_index);
+                    auto conn_index = connection_pool.contains(conn_uri) ?
+                                      connection_pool.to_index_by_name(conn_uri) :
+                                      connection_pool.new_elem(conn_uri);
+                    auto &conn_info = connection_pool[conn_index];
+                    conn_info.signal_index = signal_index;
+                    conn_info.slot_index = slot_index;
+                    for (auto mode_index: mode_list) {
+                        mode_pool[mode_index].connection_set.insert(conn_index);
+                    }
+                }
+            }
+        }
+
+    };
+
+    sophiar_manager::sophiar_manager()
+            : pimpl(std::make_unique<impl>()) {
+        pimpl->q_this = this;
+    }
+
+    void sophiar_manager::register_object_type_impl(const std::string &type_name,
+                                                    obj_factory_func_type func) {
+        auto index = pimpl->obj_factory_func_pool.new_elem(type_name);
+        pimpl->obj_factory_func_pool[index] = func;
+    }
+
+    void sophiar_manager::register_signal_impl(sophiar_obj *obj,
+                                               const std::string &signal_name,
+                                               tiny_signal_base *signal_base) {
+        pimpl->register_signal(obj, signal_name, signal_base);
+    }
+
+    void sophiar_manager::register_slot_impl(sophiar_obj *obj,
+                                             const std::string &slot_name,
+                                             tiny_slot_base *slot_base,
+                                             slot_demuxer_base *demuxer_base) {
+        pimpl->register_slot(obj, slot_name, slot_base, demuxer_base);
+    }
+
+    void sophiar_manager::build_from_config(const nlohmann::json &config) {
+        pimpl->build_graph(config);
+    }
+
+    boost::asio::awaitable<bool> sophiar_manager::switch_mode(const std::string &mode_name) {
+        if (!pimpl->mode_pool.contains(mode_name)) {
+            // TODO show log
+            co_return false;
+        }
+        auto mode_index = pimpl->mode_pool.to_index_by_name(mode_name);
+        co_return co_await pimpl->switch_mode_impl(mode_index);
+    }
+
+    sophiar_manager::~sophiar_manager() = default;
+
+}

src/core/sophiar_manager.h

@@ -0,0 +1,90 @@
+#ifndef SOPHIAR2_SOPHIAR_MANAGER_H
+#define SOPHIAR2_SOPHIAR_MANAGER_H
+
+#include "utility/tiny_signal.hpp"
+#include "utility/signal_muxer.hpp"
+#include "utility/slot_demuxer.hpp"
+
+#include <boost/asio/awaitable.hpp>
+
+#include <fmt/format.h>
+
+#include <nlohmann/json.hpp>
+
+#include <cassert>
+#include <exception>
+#include <memory>
+#include <string>
+#include <type_traits>
+
+namespace sophiar {
+
+    class sophiar_obj;
+
+    class sophiar_manager {
+    public:
+
+        using obj_factory_func_type = sophiar_obj *(*)();
+
+        void build_from_config(const nlohmann::json &config);
+
+        boost::asio::awaitable<bool> switch_mode(const std::string &mode_name);
+
+//        object_index_type get_object_index(sophiar_obj *obj);
+
+        template<typename Derived>
+        void register_object_type(const std::string &type_name) {
+            static_assert(std::is_convertible_v<decltype(Derived::new_instance()), sophiar_obj *>);
+            register_object_type_impl(type_name, &Derived::new_instance);
+        }
+
+        template<typename ...Args>
+        void register_signal(sophiar_obj *obj,
+                             const std::string &signal_name,
+                             tiny_signal<Args...> &signal) {
+            tiny_signal_base *base_ptr = &signal;
+            register_signal_impl(obj, signal_name, base_ptr);
+        }
+
+        // 将套用 slot_demuxer
+        template<typename ...Args>
+        void register_slot(sophiar_obj *obj,
+                           const std::string &slot_name,
+                           typename tiny_signal<Args...>::slot_type &slot) {
+            tiny_slot_base *slot_base = &slot;
+            slot_demuxer_base *demuxer_base = new slot_demuxer<Args...>(slot);
+            register_slot_impl(obj, slot_name, slot_base, demuxer_base);
+        }
+
+        sophiar_manager();
+
+        ~sophiar_manager();
+
+    private:
+
+        struct impl;
+        std::unique_ptr<impl> pimpl;
+
+        void register_object_type_impl(const std::string &type_name, obj_factory_func_type func);
+
+        // 记录 signal 的归属信息
+        void register_signal_impl(sophiar_obj *obj,
+                                  const std::string &signal_name,
+                                  tiny_signal_base *signal_base);
+
+        // 记录 slot 的归属信息
+        void register_slot_impl(sophiar_obj *obj,
+                                const std::string &slot_name,
+                                tiny_slot_base *slot_base,
+                                slot_demuxer_base *demuxer_base);
+
+    };
+
+    extern sophiar_manager global_sophiar_manager;
+
+#define REGISTER_TYPE(DerivedT) \
+    global_sophiar_manager.register_object_type<DerivedT>(#DerivedT)
+
+}
+
+#endif //SOPHIAR2_SOPHIAR_MANAGER_H

src/core/sophiar_obj.hpp

@@ -0,0 +1,39 @@
+#ifndef SOPHIAR2_SOPHIAR_OBJ_HPP
+#define SOPHIAR2_SOPHIAR_OBJ_HPP
+
+#include "core/sophiar_manager.h"
+
+#include <boost/asio/io_context.hpp>
+
+#include <nlohmann/json.hpp>
+
+namespace sophiar {
+
+    extern boost::asio::io_context global_context;
+
+    class sophiar_obj {
+    public:
+
+        virtual ~sophiar_obj() = default;
+
+        // 创建所有的 slot 和 signal
+        virtual void load_construct_config(const nlohmann::json &config) {};
+
+        static constexpr auto &get_context() {
+            return global_context;
+        }
+
+        static constexpr auto &get_manager() {
+            return global_sophiar_manager;
+        }
+
+    };
+
+#define DEFAULT_NEW_INSTANCE(DerivedT) \
+    static sophiar_obj *new_instance() { \
+        return new DerivedT{}; \
+    }
+
+}
+
+#endif //SOPHIAR2_SOPHIAR_OBJ_HPP

src/core/transform_tree.cpp

@@ -1,19 +1,182 @@
 #include "transform_tree.h"
 
+#include "core/sophiar_manager.h"
+#include "utility/named_vector.hpp"
+#include "utility/signal_muxer.hpp"
+
+#include <fmt/format.h>
+
+#include <cassert>
+#include <string>
+#include <vector>
+
 namespace sophiar {
 
     struct transform_tree::impl {
 
+        using trans_update_slot_type = tiny_signal<trans_obj::pointer>::slot_type;
+        using trans_updated_signal_type = tiny_signal<>;
+
         struct node_type {
+            index_type parent = -1, depth = 0; // 父亲的编号, 节点深度
+            bool is_valid = true; // 变换矩阵是否有效
+            trans_type trans; // 在父亲下观察自己的结果
+            trans_updated_signal_type updated_signal; // 当发生有效变换时发送信号
+        };
+
+        struct trans_update_slot_impl_type : public trans_update_slot_type {
 
+            node_type *node = nullptr;
+
+            void on_signal_received(trans_obj::pointer obj) override {
+                assert(node != nullptr);
+                coordinate_pose_transform_tree trans;
+                *obj >> trans;
+                bool need_emit_updated_signal = false;
+                if (!node->trans.isApprox(trans.view_as_base())) {
+                    need_emit_updated_signal = true;
+                    node->trans = trans.view_as_base();
+                }
+                if (node->is_valid != trans.is_valid) {
+                    need_emit_updated_signal = true;
+                    node->is_valid = trans.is_valid;
+                }
+                if (need_emit_updated_signal) {
+                    node->updated_signal.emit();
+                }
+            }
         };
 
-        node_type nodes[MAX_NODE_CNT];
+        transform_tree *q_this = nullptr;
+        named_vector<index_type, node_type> nodes;
+
+        std::tuple<bool, trans_type> query_transform(index_type observer, index_type target) {
+            if (observer == target) [[unlikely]] {
+                return {true, trans_type::Identity()};
+            }
+
+            if (nodes[target].parent == observer) [[unlikely]] {
+                return {nodes[target].is_valid, nodes[target].trans};
+            }
+
+            if (nodes[observer].parent == target) [[unlikely]] {
+                return {nodes[observer].is_valid, nodes[observer].trans.inverse()};
+            }
+
+            auto observer_trans = trans_type::Identity();
+            auto target_trans = trans_type::Identity();
+            auto is_valid = true;
+            while (observer != target) {
+                bool is_target_lower = nodes[target].depth > nodes[observer].depth;
+                auto &lower = is_target_lower ? target : observer;
+                auto &lower_trans = is_target_lower ? target_trans : observer_trans;
+                lower_trans = nodes[lower].trans * lower_trans;
+                is_valid &= nodes[lower].is_valid;
+                lower = nodes[lower].parent;
+                assert(lower != -1);
+            }
+
+            return {is_valid, observer_trans.inverse() * target_trans};
+        }
+
+        trans_updated_signal_type *create_watch(index_type observer, index_type target) {
+            auto signal = new signal_muxer<>(); // 因为只会调用有限次，所以无视内存泄漏的问题
+            while (observer != target) {
+                bool is_target_lower = nodes[target].depth > nodes[observer].depth;
+                auto &lower = is_target_lower ? target : observer;
+                auto lower_name = nodes.to_name_by_index(lower);
+                signal->add_upstream_signal(lower_name, nodes[lower].updated_signal);
+                lower = nodes[lower].parent;
+                assert(lower != -1);
+            }
+            return signal;
+        }
+
+        void load_nodes(const nlohmann::json &config) {
+            assert(config.contains("node_list"));
+            assert(config["node_list"].is_array());
+            for (auto &node: config["node_list"]) {
+
+                assert(node.contains("name"));
+                assert(node["name"].is_string());
+                auto name = node["name"].get<std::string>();
+                auto index = nodes.new_elem(name);
+
+                index_type parent_index = -1;
+                if (node.contains("parent")) {
+                    assert(node["parent"].is_string());
+                    auto parent_name = node["parent"].get<std::string>();
+                    parent_index = nodes.to_index_by_name(parent_name);
+                }
+
+                trans_type default_trans = trans_type::Identity();
+                if (node.contains("transform")) {
+                    coordinate_pose_compact compact_trans;
+                    compact_trans.fill_from_json(node["transform"]);
+                    default_trans = compact_trans.view_as_base();
+                }
+
+                bool is_valid = true;
+                if (node.contains("valid")) {
+                    assert(node["valid"].is_boolean());
+                    is_valid = node["valid"].get<bool>();
+                }
+
+                auto &tree_node = nodes[index];
+                tree_node.parent = parent_index;
+                tree_node.is_valid = is_valid;
+                tree_node.trans = default_trans;
+                if (parent_index != -1) {
+                    tree_node.depth = nodes[parent_index].depth + 1;
+                }
+
+                auto trans_update_slot = new trans_update_slot_impl_type;
+                trans_update_slot->node = &tree_node;
+                get_manager().register_slot<trans_obj::pointer>(q_this, name, *trans_update_slot);
+            }
+        }
+
+        void load_watches(const nlohmann::json &config) {
+            if (!config.contains("watch_list")) return;
+            for (auto &watch_config: config["watch_list"]) {
+                assert(watch_config.contains("target"));
+                assert(watch_config["target"].is_string());
+                assert(watch_config.contains("observer"));
+                assert(watch_config["observer"].is_string());
+                auto target_name = watch_config["target"].get<std::string>();
+                auto observer_name = watch_config["observer"].get<std::string>();
+                auto target_index = nodes.to_index_by_name(target_name);
+                auto observer_index = nodes.to_index_by_name(observer_name);
+                auto watch_signal = create_watch(observer_index, target_index);
+                auto watch_name = fmt::format("track_{}_{}", observer_name, target_name);
+                if (watch_config.contains("name")) {
+                    assert(watch_config["name"].is_string());
+                    watch_name = watch_config["name"].get<std::string>();
+                }
+                get_manager().register_signal(q_this, watch_name, *watch_signal);
+            }
+        }
 
     };
 
     transform_tree::transform_tree()
-            : pimpl(std::make_unique<impl>()) {}
+            : pimpl(std::make_unique<impl>()) {
+        pimpl->q_this = this;
+    }
+
+    std::tuple<bool, transform_tree::trans_type>
+    transform_tree::query_transform(index_type observer, index_type target) {
+        return pimpl->query_transform(observer, target);
+    }
+
+    void transform_tree::load_construct_config(const nlohmann::json &config) {
+        pimpl->load_nodes(config);
+        pimpl->load_watches(config);
+    }
+
+    transform_tree::index_type transform_tree::get_index_by_name(const std::string &name) {
+        return pimpl->nodes.to_index_by_name(name);
+    }
 
     transform_tree::~transform_tree() = default;
 

src/core/transform_tree.h

@@ -1,22 +1,60 @@
 #ifndef SOPHIAR2_TRANSFORM_TREE_H
 #define SOPHIAR2_TRANSFORM_TREE_H
 
+#include "core/small_obj.hpp"
+#include "core/sophiar_obj.hpp"
+#include "core/types/geometry_types.hpp"
+
 #include <Eigen/Geometry>
 
+#include <nlohmann/json.hpp>
+
+#include <limits>
 #include <memory>
+#include <tuple>
 
 namespace sophiar {
 
-    class transform_tree {
+    struct coordinate_pose_transform_tree : public coordinate_pose {
+
+        static constexpr auto type_id = (uint32_t) versatile_type_id::COORDINATE_POSE_TRANSFORM_TREE;
+
+        bool is_valid = false;
+
+    protected:
+
+        void fill_from_versatile_data(const versatile_data &data) {
+            is_valid = data.extra_data[0];
+            coordinate_pose::fill_from_versatile_data(data);
+        }
+
+        void write_to_versatile_data(versatile_data &data) const {
+            data.extra_data[0] = is_valid;
+            coordinate_pose::write_to_versatile_data(data);
+        }
+
+    private:
+
+        friend class versatile_data;
+
+    };
+
+    class transform_tree : public sophiar_obj {
     public:
 
+        using index_type = int8_t;
         using trans_type = Eigen::Isometry3d;
-
-        static constexpr auto MAX_NODE_CNT = 32;
+        using trans_obj = versatile_obj;
 
         transform_tree();
 
-        ~transform_tree();
+        ~transform_tree() override;
+
+        std::tuple<bool, trans_type> query_transform(index_type observer, index_type target);
+
+        void load_construct_config(const nlohmann::json &config) override;
+
+        index_type get_index_by_name(const std::string &name);
 
     private:
 

src/core/tristate_obj.cpp

@@ -12,8 +12,7 @@ namespace sophiar {
 
     using namespace boost::asio::experimental::awaitable_operators;
 
-    template<typename FreqTag>
-    struct tristate_obj<FreqTag>::impl {
+    struct tristate_obj::impl {
 
         tristate_obj *q_this = nullptr;
 
@@ -26,14 +25,14 @@ namespace sophiar {
         coro_signal reset_finished_signal;
 
         impl()
-                : init_cancel_signal(tristate_obj::get_context()),
-                  start_cancel_signal(tristate_obj::get_context()),
-                  init_finished_signal(tristate_obj::get_context()),
-                  start_finished_signal(tristate_obj::get_context()),
-                  stop_finished_signal(tristate_obj::get_context()),
-                  reset_finished_signal(tristate_obj::get_context()) {}
-
-        awaitable<bool> init() {
+                : init_cancel_signal(get_context()),
+                  start_cancel_signal(get_context()),
+                  init_finished_signal(get_context()),
+                  start_finished_signal(get_context()),
+                  stop_finished_signal(get_context()),
+                  reset_finished_signal(get_context()) {}
+
+        awaitable<bool> init(const nlohmann::json &config) {
             if (state == state_type::INITIALIZING) {
                 auto result = co_await (init_finished_signal.coro_wait() ||
                                         reset_finished_signal.coro_wait());
@@ -45,7 +44,7 @@ namespace sophiar {
             }
             if (state != state_type::INITIAL) co_return true; // >= PENDING
             state = state_type::INITIALIZING;
-            auto result = co_await (q_this->on_init() || init_cancel_signal.coro_wait());
+            auto result = co_await (q_this->on_init(config) || init_cancel_signal.coro_wait());
             if (result.index() == 0 && std::get<0>(result) == true) { // succeeded
                 state = state_type::PENDING;
                 init_finished_signal.try_notify_all();
@@ -58,7 +57,7 @@ namespace sophiar {
             }
         }
 
-        awaitable<bool> start() {
+        awaitable<bool> start(const nlohmann::json &config) {
             if (state == state_type::STARTING) {
                 auto result = co_await (start_finished_signal.coro_wait() ||
                                         stop_finished_signal.coro_wait());
@@ -71,7 +70,7 @@ namespace sophiar {
             if (state == state_type::RUNNING) co_return true;
             if (state != state_type::PENDING) co_return false; // INITIAL, INITIALIZING, RESETTING
             state = state_type::STARTING;
-            auto result = co_await (q_this->on_start() || start_cancel_signal.coro_wait());
+            auto result = co_await (q_this->on_start(config) || start_cancel_signal.coro_wait());
             if (result.index() == 0 && std::get<0>(result) == true) { // succeeded
                 state = state_type::RUNNING;
                 start_finished_signal.try_notify_all();
@@ -124,46 +123,31 @@ namespace sophiar {
 
     };
 
-    template<typename FreqTag>
-    typename tristate_obj<FreqTag>::state_type tristate_obj<FreqTag>::get_state() const {
+    tristate_obj::state_type tristate_obj::get_state() const {
         return pimpl->state;
     }
 
-    template<typename FreqTag>
-    awaitable<bool> tristate_obj<FreqTag>::init() {
-        return pimpl->init();
+    awaitable<bool> tristate_obj::init(const nlohmann::json &config) {
+        return pimpl->init(config);
     }
 
-    template<typename FreqTag>
-    awaitable<bool> tristate_obj<FreqTag>::start() {
-        return pimpl->start();;
+    awaitable<bool> tristate_obj::start(const nlohmann::json &config) {
+        return pimpl->start(config);
     }
 
-    template<typename FreqTag>
-    awaitable<void> tristate_obj<FreqTag>::stop() {
+    awaitable<void> tristate_obj::stop() {
         return pimpl->stop();
     }
 
-    template<typename FreqTag>
-    awaitable<void> tristate_obj<FreqTag>::reset() {
+    awaitable<void> tristate_obj::reset() {
         return pimpl->reset();
     }
 
-    template<typename FreqTag>
-    tristate_obj<FreqTag>::tristate_obj()
+    tristate_obj::tristate_obj()
             : pimpl(std::make_unique<impl>()) {
         pimpl->q_this = this;
     }
 
-    template<typename FreqTag>
-    tristate_obj<FreqTag>::~tristate_obj() = default;
-
-    // 显式实例化
-
-    template
-    class tristate_obj<high_freq_tag>;
-
-    template
-    class tristate_obj<low_freq_tag>;
+    tristate_obj::~tristate_obj() = default;
 
 }

src/core/tristate_obj.h

@@ -1,7 +1,9 @@
 #ifndef SOPHIAR2_TRISTATE_OBJ_H
 #define SOPHIAR2_TRISTATE_OBJ_H
 
-#include "core/global_io_context.hpp"
+#include "core/sophiar_obj.hpp"
+
+#include <nlohmann/json.hpp>
 
 #include <coroutine>
 #include <memory>
@@ -11,8 +13,8 @@
 
 namespace sophiar {
 
-    template<typename FreqTag>
-    class tristate_obj : public use_context<FreqTag>, private boost::noncopyable {
+    class tristate_obj : public sophiar_obj,
+                         private boost::noncopyable {
     public:
 
         enum class state_type {
@@ -27,11 +29,11 @@ namespace sophiar {
 
         tristate_obj();
 
-        virtual ~tristate_obj();
+        ~tristate_obj() override;
 
-        boost::asio::awaitable<bool> init();
+        boost::asio::awaitable<bool> init(const nlohmann::json &config);
 
-        boost::asio::awaitable<bool> start();
+        boost::asio::awaitable<bool> start(const nlohmann::json &config);
 
         boost::asio::awaitable<void> stop();
 
@@ -39,11 +41,22 @@ namespace sophiar {
 
         state_type get_state() const;
 
+        bool is_stable() const { // 是否处于稳定状态
+            auto cur_state = get_state();
+            return cur_state == state_type::INITIAL ||
+                   cur_state == state_type::PENDING ||
+                   cur_state == state_type::RUNNING;
+        }
+
     protected:
 
-        virtual boost::asio::awaitable<bool> on_init() { co_return true; } // TODO add no_throw
+        virtual boost::asio::awaitable<bool> on_init(const nlohmann::json &config) {
+            co_return true;
+        }
 
-        virtual boost::asio::awaitable<bool> on_start() { co_return true; }
+        virtual boost::asio::awaitable<bool> on_start(const nlohmann::json &config) {
+            co_return true;
+        }
 
         virtual boost::asio::awaitable<void> on_stop() { co_return; }
 

src/core/types/geometry_types.hpp

@@ -5,6 +5,8 @@
 
 #include <Eigen/Geometry>
 
+#include <nlohmann/json.hpp>
+
 #include <cassert>
 
 namespace sophiar {
@@ -46,6 +48,34 @@ namespace sophiar {
 
         static constexpr auto type_id = (uint32_t) versatile_type_id::COORDINATE_POSE_COMPACT;
 
+        void fill_from_json(const nlohmann::json &data) {
+            assert(data.is_array());
+            assert(data.size() == 7);
+            for (int i = 0; i < 7; ++i) {
+                assert(data[i].is_number());
+            }
+            auto trans_part = Eigen::Translation3d(data[0].get<double>(),
+                                                   data[1].get<double>(),
+                                                   data[2].get<double>());
+            auto quat_part = Eigen::Quaterniond(data[3].get<double>(),
+                                                data[4].get<double>(),
+                                                data[5].get<double>(),
+                                                data[6].get<double>());
+            this->view_as_base() = trans_part * quat_part;
+        }
+
+        void write_to_json(nlohmann::json &data) {
+            auto trans_part = this->translation();
+            auto quat_part = Eigen::Quaterniond(this->rotation());
+            data.push_back(trans_part.x());
+            data.push_back(trans_part.y());
+            data.push_back(trans_part.z());
+            data.push_back(quat_part.w());
+            data.push_back(quat_part.x());
+            data.push_back(quat_part.y());
+            data.push_back(quat_part.z());
+        }
+
     protected:
 
         void fill_from_versatile_data(const versatile_data &data) {
@@ -99,13 +129,7 @@ namespace sophiar {
 
     };
 
-    auto make_skew(const Eigen::Vector3d &vec) { // TODO 不知道怎么优化
-        return Eigen::Matrix3d{{0,       -vec(2), vec(1)},
-                               {vec(2),  0,       -vec(0)},
-                               {-vec(1), vec(0),  0}};
-    }
-
-    auto operator*(const Eigen::Isometry3d &trans, const coordinate_twist &old_twist) { // TODO 不知道怎么优化
+    inline auto operator*(const Eigen::Isometry3d &trans, const coordinate_twist &old_twist) { // TODO 不知道怎么优化
         coordinate_twist new_twist;
         auto rot_matrix = trans.linear();
         auto trans_vec = trans.translation();
@@ -127,7 +151,7 @@ namespace sophiar {
             versatile_type_id::POINT_VELOCITY, 3, 1> {
     };
 
-    auto operator*(const Eigen::Isometry3d &trans, const point_velocity &old_vec) {
+    inline auto operator*(const Eigen::Isometry3d &trans, const point_velocity &old_vec) {
         point_velocity new_vec;
         new_vec.noalias() = trans.linear() * old_vec;
         return new_vec;

src/core/types/versatile_data.hpp

@@ -31,6 +31,10 @@ namespace sophiar {
             timestamp = current_timestamp();
         }
 
+        void copy_from(const versatile_data &other) {
+            std::memcpy(this, &other, sizeof(*this));
+        }
+
         double &operator[](size_t index) {
             assert(index < FLOAT_FILED_LENGTH);
             return floats[index];
@@ -72,14 +76,14 @@ namespace sophiar {
     static_assert(sizeof(versatile_data) == 128);
     static_assert(std::is_trivial_v<versatile_data>);
 
-    template<typename FreqTag>
     struct versatile_obj : public versatile_data,
-                           public small_obj<versatile_obj<FreqTag>> {
+                           public small_obj<versatile_obj> {
     };
 
     enum class versatile_type_id : uint32_t {
         COORDINATE_POSE,
         COORDINATE_POSE_COMPACT, // 平移 + 四元数
+        COORDINATE_POSE_TRANSFORM_TREE,
         COORDINATE_TWIST,
         POINT,
         POINT_VELOCITY,

src/extern_defs/core/sophiar_manager.cpp

@@ -0,0 +1,7 @@
+#include "core/sophiar_manager.h"
+
+namespace sophiar {
+
+    sophiar_manager global_sophiar_manager;
+
+}

src/extern_defs/core/global_io_context.cpp → src/extern_defs/core/sophiar_obj.cpp

@@ -1,4 +1,4 @@
-#include "core/global_io_context.hpp"
+#include "core/sophiar_obj.hpp"
 #include "third_party/static_block.hpp"
 
 #include <boost/predef.h>
@@ -12,8 +12,7 @@
 
 namespace sophiar {
 
-    boost::asio::io_context high_freq_context;
-    boost::asio::io_context low_freq_context;
+    boost::asio::io_context global_context;
 
 #ifdef BOOST_OS_WINDOWS_AVAILABLE
 

src/extern_defs/core/types/versatile_data.cpp

@@ -2,12 +2,16 @@
 
 namespace sophiar {
 
-    bool _is_compatible(uint32_t given_type, uint32_t requested_type) {
+    bool _is_compatible(uint32_t _given_type, uint32_t _requested_type) {
+        auto given_type = (versatile_type_id) _given_type;
+        auto requested_type = (versatile_type_id) _requested_type;
         if (given_type == requested_type) return true;
         switch (given_type) {
-
+            case versatile_type_id::COORDINATE_POSE_TRANSFORM_TREE:
+                if (requested_type == versatile_type_id::COORDINATE_POSE) return true;
+            default:
+                return false;
         }
-        return false;
     }
 
 }

src/main.cpp

@@ -8,10 +8,9 @@
 
 #include <boost/asio/co_spawn.hpp>
 #include <boost/asio/detached.hpp>
+#include <boost/property_tree/ptree.hpp>
 #include <boost/uuid/uuid_generators.hpp>
 
-#include <Eigen/Geometry>
-
 using boost::asio::co_spawn;
 using boost::asio::detached;
 
@@ -22,14 +21,6 @@ auto test_func(const Eigen::MatrixBase<Derived> &m) {
     return sizeof(std::remove_cv<decltype(m)>);
 }
 
-struct fake_state_listener : public ur_status_signal_type::listener_type {
-    void on_signal_received(ur_status::pointer status) override {
-        ++cnt;
-    }
-
-    size_t cnt = 0;
-};
-
 int main() {
 //    ur_interface ur;
 //    fake_state_listener listener;
@@ -52,7 +43,7 @@ int main() {
 //    ur.get_context().run();
 //    std::cout << listener.cnt << std::endl;
 
-    auto trans = to_matrix_obj<high_freq_tag>(Eigen::Isometry3d().matrix());
+    std::cout << sizeof(std::string) << std::endl;
 
     return 0;
 }

src/robot/ur/ur_interface.cpp

@@ -223,7 +223,11 @@ namespace sophiar {
             }
         };
 
-        struct ur_command_handler_type : public ur_command_listener_type {
+        using ur_command_signal_type = tiny_signal<ur_command::pointer>;
+        using ur_status_signal_type = tiny_signal<ur_status::pointer>;
+        using ur_command_slot_type = ur_command_signal_type::slot_type;
+
+        struct ur_command_slot_impl_type : public ur_command_slot_type {
             impl *p_this = nullptr;
 
             void on_signal_received(ur_command::pointer cmd) override {
@@ -237,8 +241,6 @@ namespace sophiar {
             }
         };
 
-        using ur_command_muxer_type = signal_muxer<ur_command::pointer>;
-
         std::vector<std::string> input_variable_list = {
                 "input_int_register_24", // Move Type
                 "input_int_register_25", // Cmd Id
@@ -288,11 +290,9 @@ namespace sophiar {
 
         ip_address_type ur_ip;
         scoped_ptr<tcp::socket> ur_socket; // https://stackoverflow.com/questions/3062803/
-        Vector6d ur_tcp = {0, 0, 0, 0, 0, 0}; // TODO 让外部代码自己提供 tcp_pose
 
         ur_status_signal_type ur_status_signal;
-        ur_command_handler_type ur_command_handler;
-        ur_command_muxer_type ur_command_muxer;
+        ur_command_slot_impl_type ur_command_handler;
         coro_signal stop_requested_signal;
         coro_signal handle_command_request_finished_signal;
         coro_signal handle_packages_finished_signal;
@@ -434,10 +434,11 @@ namespace sophiar {
 
         awaitable<void> install_program() {
             // stop moving
-            auto no_motion = ur_command::new_stop_motion();
+            auto no_motion = ur_command::new_instance();
+            no_motion->set_motion_type(ur_move_mode::NO_MOTION);
             co_await ur_command_queue.async_send(error_code{}, std::move(no_motion), use_awaitable);
             // open urscript socket
-            tcp::socket urscript_socket(q_this->get_context());
+            tcp::socket urscript_socket(get_context());
             co_await urscript_socket.async_connect({ur_ip, urscript_port}, use_awaitable);
             // send script file
             auto script_file = mapped_file(urscript_path, boost::iostreams::mapped_file::readonly);
@@ -458,7 +459,6 @@ namespace sophiar {
             // fill data
             static int32_t last_cmd_id = 0;
             cmd->cmd_id = last_cmd_id++;
-            cmd->tcp_pose = ur_tcp; // TODO 让外部提供 tcp_pose
             // send command
             auto command_content_ptr = static_cast<ur_command_content *>(cmd.get());
             auto command_wrapper_ptr = reinterpret_cast<ur_command_wrapper *>(command_content_ptr);
@@ -478,7 +478,7 @@ namespace sophiar {
             };
             buffer >> data_packet;
             assert(data_packet.recipe_id == outputs_recipe_id);
-            ur_status_signal.emit_signal(status);
+            ur_status_signal.emit(status);
         }
 
         void handle_text_message(versatile_buffer &buffer) {
@@ -553,8 +553,22 @@ namespace sophiar {
             co_return;
         }
 
+        void register_signal_slot() {
+            get_manager().register_signal(q_this, "status", ur_status_signal);
+            get_manager().register_slot<ur_command::pointer>(q_this, "command", ur_command_handler);
+        }
+
+        void load_init_config(const nlohmann::json &config) {
+            assert(config.contains("address"));
+            assert(config["address"].is_string());
+            ur_ip = boost::asio::ip::make_address(config["address"].get<std::string>());
+            assert(config.contains("report_frequency"));
+            assert(config["report_frequency"].is_number());
+            report_frequency = config["report_frequency"].get<double>();
+        }
+
         awaitable<bool> on_init_impl() {
-            ur_socket.reset(new tcp::socket(q_this->get_context()));
+            ur_socket.reset(new tcp::socket(get_context()));
             co_await ur_socket->async_connect({ur_ip, rtde_port}, use_awaitable);
             // decrease delay
             tcp::no_delay no_delay_option(true);
@@ -599,48 +613,17 @@ namespace sophiar {
         }
 
         impl()
-                : ur_command_queue(ur_interface::get_context(), ur_command_queue_size),
-                  ur_command_muxer(ur_command_handler),
-                  stop_requested_signal(ur_interface::get_context()),
-                  handle_packages_finished_signal(ur_interface::get_context()),
-                  handle_command_request_finished_signal(ur_interface::get_context()) {
+                : ur_command_queue(get_context(), ur_command_queue_size),
+                  stop_requested_signal(get_context()),
+                  handle_packages_finished_signal(get_context()),
+                  handle_command_request_finished_signal(get_context()) {
             ur_command_handler.p_this = this;
         }
 
     };
 
-    void ur_interface::set_ur_ip(const ip_address_type &ip) {
-        assert(get_state() == state_type::INITIAL);
-        pimpl->ur_ip = ip;
-    }
-
-    void ur_interface::set_report_frequency(double freq) {
-        assert(get_state() != state_type::RUNNING);
-        pimpl->report_frequency = freq;
-    }
-
-    void ur_interface::set_tcp(const Vector6d &tcp) {
-        pimpl->ur_tcp = tcp;
-    }
-
-    void ur_interface::add_status_listener(ur_status_listener_type &node) {
-        pimpl->ur_status_signal.add_listener(node);
-    }
-
-    ur_command_listener_type &ur_interface::get_command_listener(switchable_muxer::eid_type eid) {
-        return pimpl->ur_command_muxer.get_listener(eid);
-    }
-
-    switchable_muxer *ur_interface::get_signal_switcher(uint8_t sid) {
-        switch (sid) {
-            case 0:
-                return &pimpl->ur_command_muxer;
-            default:
-                return nullptr;
-        }
-    }
-
-    awaitable<bool> ur_interface::on_init() {
+    awaitable<bool> ur_interface::on_init(const nlohmann::json &config) {
+        pimpl->load_init_config(config);
         try {
             CO_ENSURE(pimpl->on_init_impl())
         } catch (std::exception &e) {
@@ -648,10 +631,10 @@ namespace sophiar {
             co_return false;
         }
 
-        co_return co_await tristate_obj::on_init();
+        co_return co_await tristate_obj::on_init(config);
     }
 
-    awaitable<bool> ur_interface::on_start() {
+    awaitable<bool> ur_interface::on_start(const nlohmann::json &config) {
         try {
             CO_ENSURE(pimpl->on_start_impl())
         } catch (std::exception &e) {
@@ -659,7 +642,7 @@ namespace sophiar {
             co_return false;
         }
 
-        co_return co_await tristate_obj::on_start();
+        co_return co_await tristate_obj::on_start(config);
     }
 
     boost::asio::awaitable<void> ur_interface::on_stop() {
@@ -679,6 +662,11 @@ namespace sophiar {
         pimpl->q_this = this;
     }
 
+    bool ur_interface::load_construct_config(const nlohmann::json &config) {
+        assert(config.empty());
+        pimpl->register_signal_slot();
+    }
+
     ur_interface::~ur_interface() = default;
 
 }

src/robot/ur/ur_interface.h

@@ -1,7 +1,6 @@
 #ifndef SOPHIAR2_UR_INTERFACE_H
 #define SOPHIAR2_UR_INTERFACE_H
 
-#include "core/global_io_context.hpp"
 #include "core/small_obj.hpp"
 #include "core/tristate_obj.h"
 #include "core/types/versatile_data.hpp"
@@ -12,12 +11,11 @@
 
 #include <boost/asio/ip/address.hpp>
 
+#include <cassert>
 #include <memory>
 
 namespace sophiar {
 
-    using ur_freq_tag = high_freq_tag;
-
     namespace ur_runtime_status {
         using ur_runtime_status_type = uint32_t;
         static constexpr ur_runtime_status_type STOPPING = 0;
@@ -57,6 +55,15 @@ namespace sophiar {
         static constexpr ur_safety_mode_type UNDEFINED_SAFETY_MODE = 11;
     }
 
+    namespace ur_move_mode {
+        using ur_move_mode_type = int32_t;
+        static constexpr ur_move_mode_type NO_MOTION = 0;
+        static constexpr ur_move_mode_type JOINT_ANGLE = 1;
+        static constexpr ur_move_mode_type JOINT_SPEED = 2;
+        static constexpr ur_move_mode_type TOOL_POSE = 3;
+        static constexpr ur_move_mode_type TOOL_SPEED = 4;
+    }
+
     struct ur_status_content {
         Eigen::Vector6d target_q, target_qd, target_qdd;
         Eigen::Vector6d actual_q, actual_qd;
@@ -74,104 +81,112 @@ namespace sophiar {
                        public small_obj<ur_status> {
     };
 
+    class ur_interface;
+
     // tcp_pose and cmd_id will be filled by ur_interface
     struct ur_command_content {
-        int32_t move_type, cmd_id;
-        Eigen::Vector6d tcp_pose, target;
-        double param_a, param_v;
-    };
-
-    struct ur_command : public ur_command_content,
-                        public small_obj<ur_command> {
+    public:
 
         static constexpr double a_joint_default = 1.4; // rad/s^2
         static constexpr double a_tool_default = 1.2; // m/s^2
         static constexpr double v_joint_default = 1.05; // rad/s
         static constexpr double v_tool_default = 0.25; // m/s
 
-        static pointer new_stop_motion() {
-            auto ret = new_instance();
-            ret->move_type = 0;
-            ret->param_a = 0;
-            ret->param_v = 0;
-            return ret;
-        }
+    protected:
+
+        int32_t move_type = 0, cmd_id = -1;
+        Eigen::Vector6d tcp_pose = Eigen::Vector6d::Zero();
+        Eigen::Vector6d target;
+        double param_a = 0, param_v = 0;
+
+    private:
+
+        friend class ur_interface;
+
+    };
+
+    struct ur_command : public ur_command_content,
+                        public small_obj<ur_command> {
 
-        static pointer new_joint_angle(const Eigen::Vector6d &angle,
-                                       double ja = a_joint_default,
-                                       double jv = v_joint_default) {
-            auto ret = new_instance();
-            ret->move_type = 1;
-            ret->target = angle;
-            ret->param_a = ja;
-            ret->param_v = jv;
-            return ret;
+        ur_command &set_motion_type(ur_move_mode::ur_move_mode_type move_type) {
+            assert(move_type >= 0 && move_type <= 4);
+            this->move_type = move_type;
+            return *this;
         }
 
-        static pointer new_joint_speed(const Eigen::Vector6d &speed,
-                                       double ja = a_joint_default) {
-            auto ret = new_instance();
-            ret->move_type = 3;
-            ret->target = speed;
-            ret->param_a = ja;
-            ret->param_v = 0;
-            return ret;
+        ur_command &set_tcp_pose(const Eigen::Vector6d &tcp) {
+            assert(move_type != ur_move_mode::NO_MOTION); // != no motion
+            this->tcp_pose = tcp;
+            return *this;
         }
 
-        static pointer new_tool_pose(const Eigen::Vector6d &pose,
-                                     double ta = a_tool_default,
-                                     double tv = v_tool_default) {
-            auto ret = new_instance();
-            ret->move_type = 2;
-            ret->target = pose;
-            ret->param_a = ta;
-            ret->param_v = tv;
-            return ret;
+        ur_command &set_joint_angle(const Eigen::Vector6d &angles) {
+            assert(move_type == ur_move_mode::JOINT_ANGLE); // movej
+            this->target = angles;
+            return *this;
         }
 
-        static pointer new_tcp_speed(const Eigen::Vector6d &speed,
-                                     double ta = a_tool_default) {
-            auto ret = new_instance();
-            ret->move_type = 4;
-            ret->target = speed;
-            ret->param_a = ta;
-            ret->param_v = 0;
-            return ret;
+        ur_command &set_tool_pose(const Eigen::Vector6d &pose) {
+            assert(move_type == ur_move_mode::TOOL_POSE); // movel
+            this->target = pose;
+            return *this;
         }
-    };
 
-    using ur_status_signal_type = tiny_signal<ur_status::pointer>;
-    using ur_command_signal_type = tiny_signal<ur_command::pointer>;
-    using ur_status_listener_type = ur_status_signal_type::listener_type;
-    using ur_command_listener_type = ur_command_signal_type::listener_type;
+        ur_command &set_joint_speed(const Eigen::Vector6d &speeds) {
+            assert(move_type == ur_move_mode::JOINT_SPEED); // speedj
+            this->target = speeds;
+            return *this;
+        }
 
-    class ur_interface : public tristate_obj<ur_freq_tag> {
-    public:
+        ur_command &set_tool_speed(const Eigen::Vector6d &speed) {
+            assert(move_type == ur_move_mode::TOOL_SPEED); // speedl
+            this->target = speed;
+            return *this;
+        }
 
-        using ip_address_type = boost::asio::ip::address;
+        ur_command &set_joint_max_speed(double joint_speed) {
+            assert(move_type == ur_move_mode::JOINT_ANGLE); // movej
+            this->param_v = joint_speed;
+            return *this;
+        }
 
-        ur_interface();
+        ur_command &set_joint_max_acceleration(double joint_acceleration) {
+            assert(move_type == ur_move_mode::JOINT_ANGLE || // movej
+                   move_type == ur_move_mode::JOINT_SPEED);  // speedj
+            this->param_a = joint_acceleration;
+            return *this;
+        }
 
-        ~ur_interface() override;
+        ur_command &set_tool_max_speed(double tool_speed) {
+            assert(move_type == ur_move_mode::TOOL_POSE); // movel
+            this->param_v = tool_speed;
+            return *this;
+        }
 
-        void set_ur_ip(const ip_address_type &ip); // TODO be private
+        ur_command &set_tools_max_acceleration(double tool_acceleration) {
+            assert(move_type == ur_move_mode::TOOL_POSE || // movel
+                   move_type == ur_move_mode::TOOL_SPEED); // speedl
+            this->param_a = tool_acceleration;
+            return *this;
+        }
+    };
 
-        // default 125Hz
-        void set_report_frequency(double freq); // TODO be private
+    class ur_interface : public tristate_obj {
+    public:
 
-        void add_status_listener(ur_status_listener_type &node);
+        using ip_address_type = boost::asio::ip::address;
 
-        ur_command_listener_type &get_command_listener(switchable_muxer::eid_type eid);
+        ur_interface();
 
-        switchable_muxer *get_signal_switcher(uint8_t sid); // signal id
+        ~ur_interface() override;
 
-        void set_tcp(const Eigen::Vector6d &tcp); // TODO 让外部提供
+        bool load_construct_config(const nlohmann::json &config) override;
 
     protected:
 
-        boost::asio::awaitable<bool> on_init() override;
+        boost::asio::awaitable<bool> on_init(const nlohmann::json &config) override;
 
-        boost::asio::awaitable<bool> on_start() override;
+        boost::asio::awaitable<bool> on_start(const nlohmann::json &config) override;
 
         boost::asio::awaitable<void> on_stop() override;
 

src/utility/bit_operations.hpp

@@ -37,7 +37,7 @@ namespace sophiar {
     template<typename T>
     std::enable_if_t<std::is_arithmetic_v<T>>
     inline swap_net_loc_endian(T &val) {
-        if constexpr(boost::endian::order::native == boost::endian::order::big) {
+        if constexpr (boost::endian::order::native == boost::endian::order::big) {
             return;
         } else {
             boost::endian::endian_reverse_inplace(val);

src/utility/coro_signal.hpp

@@ -12,6 +12,7 @@
 
 namespace sophiar {
 
+    // TODO 为多线程改造
     class coro_signal : private boost::noncopyable {
     public:
 
@@ -29,7 +30,7 @@ namespace sophiar {
                 }
                 // handle leaking signal
                 if (signal.waiting_count == 0 &&
-                    signal.channel.ready()) {
+                        signal.channel.ready()) {
                     signal.channel.reset();
                     signal.is_notifying = false;
                 }

src/utility/name_translator.hpp

@@ -0,0 +1,32 @@
+#ifndef SOPHIAR2_NAME_TRANSLATOR_HPP
+#define SOPHIAR2_NAME_TRANSLATOR_HPP
+
+#include <cassert>
+#include <string>
+#include <unordered_map>
+
+namespace sophiar {
+
+    template<typename IndexType>
+    class name_translator {
+    public:
+
+        void register_item(const std::string &name, IndexType index) {
+            assert(!name_map.contains(name));
+            name_map[name] = index;
+        }
+
+        IndexType translate(const std::string &name) const {
+            assert(name_map.contains(name));
+            return name_map.at(name);
+        }
+
+    private:
+
+        std::unordered_map<std::string, IndexType> name_map;
+
+    };
+
+}
+
+#endif //SOPHIAR2_NAME_TRANSLATOR_HPP

src/utility/named_vector.hpp

@@ -0,0 +1,75 @@
+#ifndef SOPHIAR2_NAMED_VECTOR_HPP
+#define SOPHIAR2_NAMED_VECTOR_HPP
+
+#include <cassert>
+#include <climits>
+#include <unordered_map>
+#include <vector>
+
+namespace sophiar {
+
+    template<typename IndexType, typename ElemType = bool>
+    class named_vector {
+    public:
+
+        IndexType new_elem(const std::string &name) {
+            assert(!name_map.contains(name));
+            assert(name_list.size() <= std::numeric_limits<IndexType>::max());
+            IndexType new_index = name_list.size();
+            name_map[name] = new_index;
+            name_list.push_back(name);
+            elem_list.resize(new_index + 1);
+            return new_index;
+        }
+
+        std::string to_name_by_index(IndexType index) const {
+            assert(index >= 0 && index < name_list.size());
+            return name_list[index];
+        }
+
+        IndexType to_index_by_name(const std::string &name) const {
+            assert(name_map.contains(name));
+            return name_map.at(name);
+        }
+
+        ElemType &operator[](IndexType index) {
+            assert(index >= 0 && index < name_list.size());
+            return elem_list[index];
+        }
+
+        const ElemType &operator[](IndexType index) const {
+            assert(index >= 0 && index < name_list.size());
+            return elem_list[index];
+        }
+
+        ElemType &operator[](const std::string &name) {
+            return operator[](to_index_by_name(name));
+        }
+
+        const ElemType &operator[](const std::string &name) const {
+            return operator[](to_index_by_name(name));
+        }
+
+        IndexType size() const {
+            return name_list.size();
+        }
+
+        bool contains(const std::string &name) const {
+            return name_map.contains(name);
+        }
+
+        bool contains(IndexType index) const {
+            return index >= 0 && index < size();
+        }
+
+    private:
+
+        std::unordered_map<std::string, IndexType> name_map;
+        std::vector<std::string> name_list;
+        std::vector<ElemType> elem_list;
+
+    };
+
+}
+
+#endif //SOPHIAR2_NAMED_VECTOR_HPP

src/utility/signal_muxer.hpp

@@ -1,66 +1,39 @@
 #ifndef SOPHIAR2_SIGNAL_MUXER_HPP
 #define SOPHIAR2_SIGNAL_MUXER_HPP
 
+#include "utility/named_vector.hpp"
 #include "utility/tiny_signal.hpp"
 
-#include <typeindex>
-#include <unordered_map>
-
 namespace sophiar {
 
-    // 只做引用，所以不定义虚的析构函数
-    struct switchable_muxer {
-        using eid_type = uint8_t;
-
-        virtual void set_switch_state(eid_type eid, bool is_enabled) = 0;
-    };
-
     template<typename... Args>
-    class signal_muxer : public switchable_muxer {
+    class signal_muxer : public tiny_signal<Args...> {
     public:
 
         using signal_type = tiny_signal<Args...>;
-        using listener_type = typename signal_type::listener_type;
+        using slot_type = typename signal_type::slot_type;
 
-        class proxy_listener : public listener_type {
-        public:
-            proxy_listener() = default;
+        struct proxy_slot : public slot_type {
 
-            explicit proxy_listener(listener_type &target)
-                    : listener(&target) {}
+            signal_type *signal = nullptr;
 
             virtual void on_signal_received(Args... args) {
-                if (!is_enabled) return;
-                listener->on_signal_received(std::move(args)...);
+                assert(signal != nullptr);
+                signal->emit(std::move(args)...);
             }
 
-        private:
-            listener_type *listener;
-            bool is_enabled = true;
-
-            friend class signal_muxer<Args...>;
         };
 
-        explicit signal_muxer(listener_type &target)
-                : listener(target) {}
-
-        listener_type &get_listener(eid_type eid) {
-            if (!listener_map.contains(eid)) {
-                listener_map.emplace(std::piecewise_construct,
-                                     std::forward_as_tuple(eid),
-                                     std::forward_as_tuple(listener));
-            }
-            return listener_map[eid];
-        }
-
-        void set_switch_state(eid_type eid, bool is_enabled) override {
-            assert(listener_map.contains(eid));
-            listener_map[eid].is_enabled = is_enabled;
+        void add_upstream_signal(const std::string &name, signal_type &signal) {
+            slot_pool.new_elem(name);
+            auto slot = new proxy_slot; // 只调用有限次，无视内存泄漏
+            slot->signal = this;
+            signal.add_slot_base(slot);
         }
 
     private:
-        listener_type &listener;
-        std::unordered_map<eid_type, proxy_listener> listener_map;
+
+        named_vector<uint8_t> slot_pool;
 
     };
 

src/utility/slot_demuxer.hpp

@@ -0,0 +1,52 @@
+#ifndef SOPHIAR2_SLOT_DEMUXER_HPP
+#define SOPHIAR2_SLOT_DEMUXER_HPP
+
+#include "utility/named_vector.hpp"
+#include "utility/tiny_signal.hpp"
+
+namespace sophiar {
+
+    struct slot_demuxer_base {
+
+        virtual ~slot_demuxer_base() = default;
+
+        virtual tiny_slot_base *new_slot() = 0;
+
+    };
+
+    template<typename... Args>
+    class slot_demuxer : public slot_demuxer_base {
+    public:
+
+        using signal_type = tiny_signal<Args...>;
+        using slot_type = typename signal_type::slot_type;
+
+        struct proxy_slot : public slot_type {
+
+            slot_type *upstream_slot = nullptr;
+
+            virtual void on_signal_received(Args... args) {
+                assert(upstream_slot != nullptr);
+                upstream_slot->signal_received(std::move(args)...);
+            }
+
+        };
+
+        explicit slot_demuxer(slot_type &slot)
+                : upstream_slot(&slot) {}
+
+        tiny_slot_base *new_slot() override {
+            auto slot = new proxy_slot; // 只调用有限次，无视内存泄漏
+            slot->upstream_slot = this->upstream_slot;
+            return slot;
+        }
+
+    private:
+
+        slot_type *upstream_slot = nullptr;
+
+    };
+
+}
+
+#endif //SOPHIAR2_SLOT_DEMUXER_HPP

src/utility/tiny_signal.hpp

@@ -3,50 +3,77 @@
 
 #include <boost/intrusive/list.hpp>
 
+#include <cassert>
 #include <type_traits>
+#include <utility>
 
 namespace sophiar {
 
-    template<typename... Args>
-    class signal_muxer;
+    using slot_hook_type = boost::intrusive::list_base_hook<
+            boost::intrusive::link_mode<
+                    boost::intrusive::auto_unlink>>;
+
+    struct tiny_slot_base : public slot_hook_type {
+
+        bool is_enabled = true;
+
+        virtual ~tiny_slot_base() = default;
+
+        void disconnect() {
+            assert(this->is_linked());
+            this->unlink();
+        }
+    };
+
+    struct tiny_signal_base {
+
+        virtual ~tiny_signal_base() = default;
+
+        virtual void add_slot_base(tiny_slot_base *slot_base) = 0;
+    };
 
     template<typename... Args>
-    class tiny_signal {
+    class tiny_signal : public tiny_signal_base {
     public:
 
-        using muxer_type = signal_muxer<Args...>;
-
-        using hook_type = boost::intrusive::list_base_hook<
-                boost::intrusive::link_mode<
-                        boost::intrusive::auto_unlink>>;
+        class slot_type : public tiny_slot_base {
+        public:
 
-        // 其他对象不得持有 listener
-        // 所有调用应该以引用进行
-        struct listener_type : public hook_type {
+            void signal_received(Args... args) {
+                if (!is_enabled) return;
+                on_signal_received(std::move(args)...);
+            }
 
-            virtual ~listener_type() = default;
+        protected:
 
             virtual void on_signal_received(Args... args) = 0;
-        };
 
-        using list_type = boost::intrusive::list<
-                listener_type, boost::intrusive::constant_time_size<false>>;
+        };
 
-        void add_listener(listener_type &node) {
-            if (node.is_linked()) {
-                node.unlink();
-            }
-            node_list.push_back(node);
+        void add_slot_base(tiny_slot_base *slot_base) override {
+            auto slot_ptr = dynamic_cast<slot_type *>(slot_base);
+            assert(slot_ptr != nullptr);
+            slot_list.push_back(*slot_ptr);
         }
 
-        void emit_signal(Args... args) {
-            for (auto &node: node_list) {
-                node.on_signal_received(args...);
+        void emit(Args... args) {
+            auto iter = slot_list.begin(), iter_end = slot_list.end();
+            while (iter != iter_end) {
+                auto this_iter = iter++;
+                if (iter == iter_end) {
+                    this_iter->signal_received(std::move(args)...);
+                } else {
+                    this_iter->signal_received(args...);
+                }
             }
         }
 
     private:
-        list_type node_list;
+
+        using slot_list_type = boost::intrusive::list<
+                slot_type, boost::intrusive::constant_time_size<false>>;
+
+        slot_list_type slot_list;
 
     };
 

tests/CMakeLists.txt

@@ -3,9 +3,11 @@ find_package (Boost REQUIRED COMPONENTS unit_test_framework)
 include_directories (${Boost_INCLUDE_DIRS})
 
 add_executable (test_core
-        core/datanode_base.cpp
+        #        core/datanode_base.cpp
         core/geometry_types.cpp
         core/small_obj.cpp
+        core/sophiar_manager.cpp
+        #        core/transform_tree.cpp
         core/tristate_obj.cpp
         ${EXTERN_DEF_FILES}
         ${CORE_IMPL_FILES})

tests/core/sophiar_manager.cpp

@@ -0,0 +1,145 @@
+#define BOOST_TEST_DYN_LINK
+
+#include "core/sophiar_manager.h"
+#include "core/tristate_obj.h"
+#include "utility/debug_utility.hpp"
+
+#include <boost/asio/co_spawn.hpp>
+#include <boost/asio/detached.hpp>
+#include <boost/asio/high_resolution_timer.hpp>
+#include <boost/asio/this_coro.hpp>
+#include <boost/asio/use_awaitable.hpp>
+#include <boost/test/unit_test.hpp>
+
+#include <chrono>
+#include <fstream>
+#include <iostream>
+
+using boost::asio::awaitable;
+using boost::asio::co_spawn;
+using boost::asio::detached;
+using boost::asio::use_awaitable;
+
+using namespace sophiar;
+using namespace std::chrono_literals;
+
+struct source_node_type : public tristate_obj {
+    DEFAULT_NEW_INSTANCE(source_node_type)
+
+    void load_construct_config(const nlohmann::json &config) override {
+        BOOST_TEST(config.empty());
+        get_manager().register_signal(this, "output", output_signal);
+    };
+
+    boost::asio::awaitable<bool> on_init(const nlohmann::json &config) override {
+        assert(config.contains("init_value"));
+        assert(config["init_value"].is_number());
+        init_value = config["init_value"].get<int64_t>();
+        co_return true;
+    }
+
+    boost::asio::awaitable<bool> on_start(const nlohmann::json &config) override {
+        assert(config.contains("start_value"));
+        assert(config["start_value"].is_number());
+        start_value = config["start_value"].get<int64_t>();
+        co_spawn(get_context(), [=]() -> awaitable<void> {
+            co_await coro_sleep(50ms);
+            this->output_signal.emit(init_value * start_value);
+        }, detached);
+        co_return true;
+    }
+
+    using output_signal_type = tiny_signal<int64_t>;
+    output_signal_type output_signal;
+    int64_t init_value = -1, start_value = -1;
+
+};
+
+struct proxy_node_type : public tristate_obj {
+    DEFAULT_NEW_INSTANCE(proxy_node_type);
+
+    using signal_type = tiny_signal<int64_t>;
+    using slot_type = signal_type::slot_type;
+
+    struct slot_impl_type : public slot_type {
+        proxy_node_type *p_this = nullptr;
+
+        void on_signal_received(int64_t args) override {
+            assert(p_this != nullptr);
+            p_this->output_signal.emit(args * p_this->init_value);
+        }
+    };
+
+    void load_construct_config(const nlohmann::json &config) override {
+        BOOST_TEST(config.empty());
+        auto input_slot = new slot_impl_type();
+        input_slot->p_this = this;
+        get_manager().register_signal(this, "output", output_signal);
+        get_manager().register_slot<int64_t>(this, "input", *input_slot);
+    };
+
+    boost::asio::awaitable<bool> on_init(const nlohmann::json &config) override {
+        assert(config.contains("init_value"));
+        assert(config["init_value"].is_number());
+        init_value = config["init_value"].get<int64_t>();
+        co_return true;
+    }
+
+    boost::asio::awaitable<bool> on_start(const nlohmann::json &config) override {
+        BOOST_TEST(config.empty());
+        co_return true;
+    }
+
+    signal_type output_signal;
+    int64_t init_value = -1;
+};
+
+struct target_node_type : public tristate_obj {
+    DEFAULT_NEW_INSTANCE(target_node_type);
+
+    using signal_type = tiny_signal<int64_t>;
+    using slot_type = signal_type::slot_type;
+
+    struct slot_impl_type : public slot_type {
+        void on_signal_received(int64_t args) override {
+            std::cout << args << std::endl;
+        }
+    };
+
+    void load_construct_config(const nlohmann::json &config) override {
+        BOOST_TEST(config.empty());
+        get_manager().register_slot<int64_t>(this, "input", *(new slot_impl_type));
+    };
+
+    boost::asio::awaitable<bool> on_init(const nlohmann::json &config) override {
+        BOOST_TEST(config.empty());
+        co_return true;
+    }
+
+    boost::asio::awaitable<bool> on_start(const nlohmann::json &config) override {
+        BOOST_TEST(config.empty());
+        co_return true;
+    }
+
+};
+
+BOOST_AUTO_TEST_CASE(test_sophiar_manager) {
+
+    REGISTER_TYPE(source_node_type);
+    REGISTER_TYPE(proxy_node_type);
+    REGISTER_TYPE(target_node_type);
+
+    std::ifstream config_file("data/sophiar_manager_config.json");
+    BOOST_TEST(config_file.is_open());
+
+    global_sophiar_manager.build_from_config(nlohmann::json::parse(config_file));
+
+    co_spawn(global_context, []() -> awaitable<void> {
+        co_await global_sophiar_manager.switch_mode("mode_a");
+        co_await coro_sleep(100ms);
+        co_await global_sophiar_manager.switch_mode("mode_b");
+    }, detached);
+
+    global_context.run();
+
+}

tests/core/transform_tree.cpp

@@ -0,0 +1,35 @@
+#define BOOST_TEST_DYN_LINK
+
+#include "core/transform_tree.h"
+
+#include <boost/test/unit_test.hpp>
+
+#include <nlohmann/json.hpp>
+
+#include <fstream>
+
+using namespace nlohmann;
+using namespace sophiar;
+
+BOOST_AUTO_TEST_CASE(test_transform_tree) {
+
+    std::ifstream config_file("data/transform_tree_config.json");
+    BOOST_TEST(config_file.is_open());
+
+    transform_tree tree;
+    tree.load_construct_config(json::parse(config_file));
+
+    auto index_c = tree.get_index_by_name("C");
+    auto index_d = tree.get_index_by_name("D");
+    auto query_result = tree.query_transform(index_d, index_c);
+    BOOST_TEST(std::get<0>(query_result) == true);
+    BOOST_TEST(std::get<1>(query_result).isApprox(
+            (Eigen::Isometry3d) Eigen::Translation3d(3, 5, 7)));
+
+    auto index_e = tree.get_index_by_name("E");
+    query_result = tree.query_transform(index_e, index_c);
+    BOOST_TEST(std::get<0>(query_result) == false);
+    BOOST_TEST(std::get<1>(query_result).isApprox(
+            (Eigen::Isometry3d) Eigen::Translation3d(4, 6, 8)));
+
+}

tests/core/tristate_obj.cpp

@@ -22,15 +22,15 @@ using namespace sophiar;
 using namespace std::chrono_literals;
 
 awaitable<void> test_tristate_obj_1() {
-    struct type_a : public tristate_obj<high_freq_tag> {
+    struct type_a : public tristate_obj {
         unsigned int cnt = 0;
 
-        awaitable<bool> on_init() {
+        awaitable<bool> on_init(const nlohmann::json &) {
             set_bit(cnt, 0);
             co_return true;
         }
 
-        awaitable<bool> on_start() {
+        awaitable<bool> on_start(const nlohmann::json &) {
             set_bit(cnt, 1);
             co_return true;
         }
@@ -46,10 +46,10 @@ awaitable<void> test_tristate_obj_1() {
         }
     } node_a;
 
-    co_await node_a.init();
+    co_await node_a.init({});
     BOOST_TEST(node_a.cnt == 0b0001);
     BOOST_TEST((node_a.get_state() == type_a::state_type::PENDING));
-    co_await node_a.start();
+    co_await node_a.start({});
     BOOST_TEST(node_a.cnt == 0b0011);
     BOOST_TEST((node_a.get_state() == type_a::state_type::RUNNING));
     co_await node_a.stop();
@@ -60,11 +60,11 @@ awaitable<void> test_tristate_obj_1() {
     BOOST_TEST((node_a.get_state() == type_a::state_type::INITIAL));
 
     node_a.cnt = 0;
-    co_await node_a.start(); // test start before init
+    co_await node_a.start({}); // test start before init
     BOOST_TEST(node_a.cnt == 0b0000);
     BOOST_TEST((node_a.get_state() == type_a::state_type::INITIAL));
-    co_await node_a.init();
-    co_await node_a.start();
+    co_await node_a.init({});
+    co_await node_a.start({});
     BOOST_TEST(node_a.cnt == 0b0011);
     BOOST_TEST((node_a.get_state() == type_a::state_type::RUNNING));
     co_await node_a.reset(); // test force reset
@@ -75,21 +75,21 @@ awaitable<void> test_tristate_obj_1() {
 }
 
 awaitable<void> test_tristate_obj_2() {
-    struct type_a : public tristate_obj<high_freq_tag> {
+    struct type_a : public tristate_obj {
         int cnt = 0;
         boost::asio::high_resolution_timer timer;
 
         type_a()
                 : timer(get_context()) {}
 
-        awaitable<bool> on_init() {
+        awaitable<bool> on_init(const nlohmann::json &) {
             timer.expires_from_now(100ms);
             co_await timer.async_wait(use_awaitable);
             set_bit(cnt, 0);
             co_return true;
         }
 
-        awaitable<bool> on_start() {
+        awaitable<bool> on_start(const nlohmann::json &) {
             timer.expires_from_now(100ms);
             co_await timer.async_wait(use_awaitable);
             set_bit(cnt, 1);
@@ -116,19 +116,19 @@ awaitable<void> test_tristate_obj_2() {
         co_await node_a.reset();
         co_return;
     }, detached);
-    BOOST_TEST((co_await node_a.init() == false));
+    BOOST_TEST((co_await node_a.init({}) == false));
     BOOST_TEST(node_a.cnt == 0b1000);
     BOOST_TEST((node_a.get_state() == type_a::state_type::INITIAL));
 
     node_a.cnt = 0;
-    BOOST_TEST((co_await node_a.init() == true));
+    BOOST_TEST((co_await node_a.init({}) == true));
     BOOST_TEST((node_a.get_state() == type_a::state_type::PENDING));
     co_spawn(co_await boost::asio::this_coro::executor, [&]() -> awaitable<void> {
         co_await coro_sleep(50ms);
         co_await node_a.stop();
         co_return;
     }, detached);
-    BOOST_TEST((co_await node_a.start() == false));
+    BOOST_TEST((co_await node_a.start({}) == false));
     BOOST_TEST(node_a.cnt == 0b0101);
     BOOST_TEST((node_a.get_state() == type_a::state_type::PENDING));
 
@@ -138,7 +138,7 @@ awaitable<void> test_tristate_obj_2() {
         co_await node_a.reset();
         co_return;
     }, detached);
-    BOOST_TEST((co_await node_a.start() == false));
+    BOOST_TEST((co_await node_a.start({}) == false));
     BOOST_TEST(node_a.cnt = 0b0101);
     BOOST_TEST((node_a.get_state() == type_a::state_type::PENDING));
 
@@ -150,21 +150,21 @@ awaitable<void> test_tristate_obj_2() {
 }
 
 awaitable<void> test_tristate_obj_3() {
-    struct type_a : public tristate_obj<high_freq_tag> {
+    struct type_a : public tristate_obj {
         int cnt = 0;
         boost::asio::high_resolution_timer timer;
 
         type_a()
                 : timer(get_context()) {}
 
-        awaitable<bool> on_init() {
+        awaitable<bool> on_init(const nlohmann::json &) {
             timer.expires_from_now(100ms);
             co_await timer.async_wait(use_awaitable);
             ++cnt;
             co_return true;
         }
 
-        awaitable<bool> on_start() {
+        awaitable<bool> on_start(const nlohmann::json &) {
             timer.expires_from_now(100ms);
             co_await timer.async_wait(use_awaitable);
             ++cnt;
@@ -188,19 +188,19 @@ awaitable<void> test_tristate_obj_3() {
 
     node_a.cnt = 0;
     co_spawn(co_await boost::asio::this_coro::executor, [&]() -> awaitable<void> {
-        BOOST_TEST(co_await node_a.init() == true);
+        BOOST_TEST(co_await node_a.init({}) == true);
         BOOST_TEST(node_a.cnt == 1);
     }, detached);
-    BOOST_TEST(co_await node_a.init() == true);
+    BOOST_TEST(co_await node_a.init({}) == true);
     BOOST_TEST((node_a.get_state() == type_a::state_type::PENDING));
     BOOST_TEST(node_a.cnt == 1);
 
     node_a.cnt = 0;
     co_spawn(co_await boost::asio::this_coro::executor, [&]() -> awaitable<void> {
-        BOOST_TEST(co_await node_a.start() == true);
+        BOOST_TEST(co_await node_a.start({}) == true);
         BOOST_TEST(node_a.cnt == 1);
     }, detached);
-    BOOST_TEST(co_await node_a.start() == true);
+    BOOST_TEST(co_await node_a.start({}) == true);
     BOOST_TEST((node_a.get_state() == type_a::state_type::RUNNING));
     BOOST_TEST(node_a.cnt == 1);
 
@@ -229,14 +229,14 @@ awaitable<void> test_tristate_obj_3() {
         co_return;
     }, detached);
     co_spawn(co_await boost::asio::this_coro::executor, [&]() -> awaitable<void> {
-        BOOST_TEST(co_await node_a.init() == false);
+        BOOST_TEST(co_await node_a.init({}) == false);
         BOOST_TEST(node_a.cnt == 1);
     }, detached);
-    BOOST_TEST(co_await node_a.init() == false);
+    BOOST_TEST(co_await node_a.init({}) == false);
     BOOST_TEST((node_a.get_state() == type_a::state_type::INITIAL));
     BOOST_TEST(node_a.cnt == 1);
 
-    co_await node_a.init();
+    co_await node_a.init({});
     node_a.cnt = 0;
     co_spawn(co_await boost::asio::this_coro::executor, [&]() -> awaitable<void> {
         co_await coro_sleep(50ms);
@@ -244,17 +244,17 @@ awaitable<void> test_tristate_obj_3() {
         co_return;
     }, detached);
     co_spawn(co_await boost::asio::this_coro::executor, [&]() -> awaitable<void> {
-        BOOST_TEST(co_await node_a.start() == false);
+        BOOST_TEST(co_await node_a.start({}) == false);
         BOOST_TEST(node_a.cnt == 1);
     }, detached);
-    BOOST_TEST(co_await node_a.start() == false);
+    BOOST_TEST(co_await node_a.start({}) == false);
     BOOST_TEST((node_a.get_state() == type_a::state_type::PENDING));
     BOOST_TEST(node_a.cnt == 1);
 }
 
 BOOST_AUTO_TEST_CASE(test_tristate_obj) {
-    co_spawn(high_freq_context, test_tristate_obj_1(), detached);
-    co_spawn(high_freq_context, test_tristate_obj_2(), detached);
-    co_spawn(high_freq_context, test_tristate_obj_3(), detached);
-    high_freq_context.run();
+    co_spawn(global_context, test_tristate_obj_1(), detached);
+    co_spawn(global_context, test_tristate_obj_2(), detached);
+    co_spawn(global_context, test_tristate_obj_3(), detached);
+    global_context.run();
 }

tests/data/sophiar_manager_config.json

@@ -0,0 +1,131 @@
+{
+  "mode_list": [
+    {
+      "name": "mode_a"
+    },
+    {
+      "name": "mode_b",
+      "degrade_list": [
+        "mode_a"
+      ]
+    }
+  ],
+  "object_list": [
+    {
+      "type": "source_node_type",
+      "name": "source",
+      "enabled_modes": "all",
+      "construct_config": {},
+      "init_configs": [
+        {
+          "modes": [
+            "mode_a"
+          ],
+          "config": {
+            "init_value": 2
+          }
+        },
+        {
+          "modes": [
+            "mode_b"
+          ],
+          "config": {
+            "init_value": 3
+          }
+        }
+      ],
+      "start_configs": [
+        {
+          "modes": [
+            "mode_a"
+          ],
+          "config": {
+            "start_value": 3
+          }
+        },
+        {
+          "modes": [
+            "mode_b"
+          ],
+          "config": {
+            "start_value": 4
+          }
+        }
+      ]
+    },
+    {
+      "type": "proxy_node_type",
+      "name": "proxy",
+      "enabled_modes": [
+        "mode_b"
+      ],
+      "construct_config": {},
+      "init_configs": [
+        {
+          "modes": "all",
+          "config": {
+            "init_value": 5
+          }
+        }
+      ],
+      "start_configs": [
+        {
+          "modes": "all",
+          "config": {}
+        }
+      ]
+    },
+    {
+      "type": "target_node_type",
+      "name": "target",
+      "enabled_modes": "all",
+      "construct_config": {},
+      "init_configs": [
+        {
+          "modes": "all",
+          "config": {}
+        }
+      ],
+      "start_configs": [
+        {
+          "modes": "all",
+          "config": {}
+        }
+      ]
+    }
+  ],
+  "connection_list": [
+    {
+      "modes": [
+        "mode_a"
+      ],
+      "connections": [
+        {
+          "signal_object": "source",
+          "signal_name": "output",
+          "slot_object": "target",
+          "slot_name": "input"
+        }
+      ]
+    },
+    {
+      "modes": [
+        "mode_b"
+      ],
+      "connections": [
+        {
+          "signal_object": "source",
+          "signal_name": "output",
+          "slot_object": "proxy",
+          "slot_name": "input"
+        },
+        {
+          "signal_object": "proxy",
+          "signal_name": "output",
+          "slot_object": "target",
+          "slot_name": "input"
+        }
+      ]
+    }
+  ]
+}

tests/data/transform_tree_config.json

@@ -0,0 +1,62 @@
+{
+  "node_list": [
+    {
+      "name": "Root"
+    },
+    {
+      "name": "B",
+      "parent": "Root"
+    },
+    {
+      "name": "C",
+      "parent": "B",
+      "transform": [
+        1,
+        2,
+        3,
+        1,
+        0,
+        0,
+        0
+      ]
+    },
+    {
+      "name": "D",
+      "parent": "Root",
+      "transform": [
+        -2,
+        -3,
+        -4,
+        1,
+        0,
+        0,
+        0
+      ]
+    },
+    {
+      "name": "E",
+      "parent": "Root",
+      "valid": false,
+      "transform": [
+        -3,
+        -4,
+        -5,
+        1,
+        0,
+        0,
+        0
+      ]
+    }
+  ],
+  "watch_list": [
+    {
+      "target": "E",
+      "observer": "Root"
+    },
+    {
+      "target": "D",
+      "observer": "Root",
+      "name": "track_D"
+    }
+  ]
+}