实现了 transform_tree

src/core/basic_obj_types.hpp

@@ -25,7 +25,7 @@ namespace sophiar {
         }
 
         template<typename WriterType>
-        void write_to(WriterType &writer) {
+        void write_to(WriterType &writer) const {
             writer << value;
         }
 
@@ -68,7 +68,7 @@ namespace sophiar {
 
     template<>
     template<typename WriterType>
-    inline void transform_obj::write_to(WriterType &writer) {
+    inline void transform_obj::write_to(WriterType &writer) const {
         auto trans_part = value.translation();
         writer << trans_part.x() << trans_part.y() << trans_part.z();
         auto quat_part = Eigen::Quaterniond(value.rotation());

src/core/sophiar_manager.cpp

@@ -480,7 +480,7 @@ namespace sophiar {
                 if (tri_obj_ptr != nullptr) {
                     // check if stop is needed
                     if (tri_obj_ptr->get_state() > tristate_obj::state_type::PENDING) {
-                        co_await coro_stop_obj(obj_index);
+                        co_await coro_stop_obj(obj_index); // TODO 这里可能会有重复，应该直接把对应的对象停止也可以
                     }
                     auto &obj_info = obj_pool[obj_index];
                     co_await tri_obj_ptr->init(obj_info.init_config);
@@ -535,7 +535,7 @@ namespace sophiar {
                     co_await handle_reset_obj(command_reader, reply_writer);
                     break;
                 }
-                default:
+                default: // TODO 增加退出的功能
                     // TODO show
                     break;
             }
@@ -554,7 +554,7 @@ namespace sophiar {
         void create_client_worker(tcp::socket &&client_socket) {
             auto remote_endpoint = client_socket.remote_endpoint();
             auto worker_func = [
-                    this,
+                    this, // TODO 把这几个 memory 放进一个 struct 里面
                     client_socket = std::move(client_socket),
                     command_memory = dynamic_memory(command_initial_length),
                     reply_memory = dynamic_memory(reply_max_length)]() mutable

src/core/sophiar_manager.h

@@ -3,9 +3,6 @@
 
 #include "core/timestamp_helper.hpp"
 #include "utility/coro_signal2.hpp"
-#include "utility/tiny_signal.hpp"
-#include "utility/signal_muxer.hpp"
-#include "utility/slot_demuxer.hpp"
 
 #include <boost/asio/awaitable.hpp>
 
@@ -115,6 +112,9 @@ namespace sophiar {
 #define REGISTER_TYPE(DerivedT) \
     global_sophiar_manager.register_object_type<DerivedT>(#DerivedT)
 
+#define UPDATE_GLOBAL_OBJ_VALUE(obj_type, obj_index, obj_value) \
+    global_sophiar_manager.update_global_obj<obj_type>(obj_index, obj_type::new_instance(obj_value))
+
 }
 
 #endif //SOPHIAR2_SOPHIAR_MANAGER_H

src/core/transform_tree.cpp

@@ -1,101 +1,159 @@
 #include "transform_tree.h"
 
-#include "core/sophiar_manager.h"
+#include "core/basic_obj_types.hpp"
+#include "utility/bit_operations.hpp"
 #include "utility/coro_worker.hpp"
 #include "utility/coro_signal_group.hpp"
-#include "utility/global_obj_helper.hpp"
 #include "utility/named_vector.hpp"
-#include "utility/signal_muxer.hpp"
+
+#include <boost/asio/co_spawn.hpp>
+#include <boost/asio/detached.hpp>
 
 #include <fmt/format.h>
 
+#include <Eigen/Geometry>
+
 #include <cassert>
+#include <optional>
 #include <string>
 #include <vector>
+#include <utility>
 
 namespace sophiar {
 
-    struct transform_tree::impl {
+    using namespace Eigen;
 
-        using trans_update_slot_type = tiny_signal<trans_obj::pointer>::slot_type;
-        using trans_updated_signal_type = tiny_signal<>;
+    using boost::asio::co_spawn;
+    using boost::asio::detached;
 
-        struct node_type {
-            index_type parent = -1, depth = 0; // 父亲的编号, 节点深度
-            bool is_valid = true; // 变换矩阵是否有效
-            trans_type trans; // 在父亲下观察自己的结果
-            trans_updated_signal_type updated_signal; // 当发生有效变换时发送信号
-        };
+    struct transform_tree::impl {
 
-        struct trans_update_slot_impl_type : public trans_update_slot_type {
+        using node_index_type = uint8_t;
+        using trans_type = Isometry3d;
+        static_assert(std::is_same<trans_type, decltype(std::declval<transform_obj>().value)>());
 
-            node_type *node = nullptr;
+        struct node_type { // 在父亲下观察自己
+            node_index_type parent = -1, depth = 0; // 父亲的编号, 节点深度
+            global_obj_index_type transform_obj_index = -1;
+            trans_type fixed_transform; // 如果不提供 transform_obj_index, 则表示这是一个固定变换
+        };
 
-            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();
-                }
-            }
+        struct query_scheme {
+            std::list<node_index_type> left, right;
         };
 
-        transform_tree *q_this = nullptr;
-        named_vector<index_type, node_type> nodes;
+        named_vector<node_index_type, node_type> nodes;
+        std::vector<coro_worker::pointer> worker_list;
 
-        std::tuple<bool, trans_type> query_transform(index_type observer, index_type target) {
-            if (observer == target) [[unlikely]] {
-                return {true, trans_type::Identity()};
+        static trans_type transform_from_json_array(const nlohmann::json &arr) {
+            assert(arr.is_array());
+            assert(arr.size() == 7);
+            double val[7];
+            for (size_t i = 0; i < 7; ++i) {
+                assert(arr[i].is_number());
+                val[i] = arr[i].get<double>();
             }
+            return Translation3d(val[0], val[1], val[2]) *
+                   Quaterniond(val[3], val[4], val[5], val[6]);
+        }
 
-            if (nodes[target].parent == observer) [[unlikely]] {
-                return {nodes[target].is_valid, nodes[target].trans};
+        std::optional<trans_type> get_transform_from_path(const std::list<node_index_type> &path) const {
+            auto ret = trans_type::Identity();
+            for (auto index: path) {
+                const auto &node_info = nodes[index];
+                if (!is_valid_id(node_info.transform_obj_index)) {
+                    ret = ret * node_info.fixed_transform;
+                } else {
+                    auto trans_obj = get_manager().
+                            get_global_obj<transform_obj>(node_info.transform_obj_index);
+                    if (trans_obj == nullptr) return std::nullopt;
+                    ret = ret * trans_obj->value;
+                }
             }
+            return ret;
+        }
 
-            if (nodes[observer].parent == target) [[unlikely]] {
-                return {nodes[observer].is_valid, nodes[observer].trans.inverse()};
-            }
+        std::optional<trans_type> get_transform_from_scheme(const query_scheme &scheme) const {
+            auto left_trans = get_transform_from_path(scheme.left);
+            auto right_trans = get_transform_from_path(scheme.right);
+            if (!left_trans.has_value() || !right_trans.has_value()) return std::nullopt;
+            return left_trans.value().inverse() * right_trans.value();
+        }
 
-            auto observer_trans = trans_type::Identity();
-            auto target_trans = trans_type::Identity();
-            auto is_valid = true;
+        void create_worker(node_index_type observer,
+                           node_index_type target,
+                           global_obj_index_type out_obj_index) {
+            assert(is_valid_id(observer) && is_valid_id(target));
+            assert(observer != target);
+
+            query_scheme scheme;
+            auto signal_group = coro_signal_any_group::new_instance(get_context());
             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);
+                const auto &obs_info = nodes[observer];
+                const auto &tar_info = nodes[target];
+                if (obs_info.depth > tar_info.depth) {
+                    assert(is_valid_id(obs_info.parent));
+                    scheme.left.push_front(observer);
+                    if (is_valid_id(obs_info.transform_obj_index)) {
+                        signal_group->add_watcher(
+                                get_manager().request_global_obj_update_watcher(
+                                        obs_info.transform_obj_index));
+                    }
+                    observer = obs_info.parent;
+                } else { // target is deeper
+                    assert(is_valid_id(tar_info.parent));
+                    scheme.right.push_front(target);
+                    if (is_valid_id(tar_info.transform_obj_index)) {
+                        signal_group->add_watcher(
+                                get_manager().request_global_obj_update_watcher(
+                                        tar_info.transform_obj_index));
+                    }
+                    target = tar_info.parent;
+                }
             }
 
-            return {is_valid, observer_trans.inverse() * target_trans};
+            signal_group->start(get_context());
+            auto signal_watcher = signal_group->new_watcher(get_context());
+            auto exit_func = [signal_group = std::move(signal_group)]() mutable {
+                co_spawn(global_context, [
+                        signal_group = std::move(signal_group)]() mutable
+                        -> boost::asio::awaitable<void> {
+                    co_await signal_group->stop();
+                    co_return;
+                }, detached);
+            };
+
+            auto worker_func = [
+                    this, out_obj_index,
+                    scheme = std::move(scheme),
+                    signal_watcher = std::move(signal_watcher)]() mutable
+                    -> boost::asio::awaitable<bool> {
+                co_await signal_watcher.coro_wait();
+                auto new_trans = get_transform_from_scheme(scheme);
+                get_manager().update_global_obj<transform_obj>(
+                        out_obj_index,
+                        new_trans.has_value() ? transform_obj::new_instance(std::move(new_trans.value())) : nullptr);
+                co_return true;
+            };
+            auto worker = make_infinite_coro_worker(get_context(),
+                                                    std::move(worker_func),
+                                                    std::move(exit_func)); // signal group will be delete here
+            worker->run();
+            worker_list.push_back(std::move(worker));
         }
 
-        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);
+        boost::asio::awaitable<void> stop_workers() {
+            for (auto &worker: worker_list) {
+                worker->cancel();
+            }
+            for (auto &worker: worker_list) {
+                co_await worker->coro_wait_stop();
             }
-            return signal;
+            worker_list.clear();
+            co_return;
         }
 
-        void load_nodes(const nlohmann::json &config) {
+        void build_tree(const nlohmann::json &config) {
             assert(config.contains("node_list"));
             assert(config["node_list"].is_array());
             for (auto &node: config["node_list"]) {
@@ -105,82 +163,83 @@ namespace sophiar {
                 auto name = node["name"].get<std::string>();
                 auto index = nodes.new_elem(name);
 
-                index_type parent_index = -1;
+                node_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();
+                global_obj_index_type transform_obj_index = -1;
+                if (node.contains("transform_obj_name")) {
+                    assert(node["transform_obj_name"].is_string());
+                    auto transform_obj_name = node["transform_obj_name"].get<std::string>();
+                    transform_obj_index = get_manager().
+                            register_global_obj<transform_obj>(transform_obj_name);
                 }
 
-                bool is_valid = true;
-                if (node.contains("valid")) {
-                    assert(node["valid"].is_boolean());
-                    is_valid = node["valid"].get<bool>();
+                trans_type default_trans = trans_type::Identity();
+                if (node.contains("transform")) {
+                    default_trans = transform_from_json_array(node["transform"]);
+                } else if (is_valid_id(parent_index)) {
+                    assert(is_valid_id(transform_obj_index));
                 }
 
-                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 &node_info = nodes[index];
+                node_info.parent = parent_index;
+                node_info.transform_obj_index = transform_obj_index;
+                node_info.fixed_transform = default_trans;
+                if (is_valid_id(parent_index)) {
+                    node_info.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;
+        void config_watcher(const nlohmann::json &config) {
+            assert(config.contains("watch_list"));
             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());
+                assert(watch_config.contains("transform_name"));
+                assert(watch_config["transform_name"].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);
+                auto transform_obj_name = watch_config["transform_name"].get<std::string>();
+                auto transform_obj_index = get_manager().
+                        register_global_obj<transform_obj>(transform_obj_name);
+                create_worker(observer_index, target_index, transform_obj_index);
             }
         }
 
     };
 
     transform_tree::transform_tree()
-            : pimpl(std::make_unique<impl>()) {
-        pimpl->q_this = this;
-    }
+            : pimpl(std::make_unique<impl>()) {}
 
-    std::tuple<bool, transform_tree::trans_type>
-    transform_tree::query_transform(index_type observer, index_type target) {
-        return pimpl->query_transform(observer, target);
+    transform_tree::~transform_tree() = default;
+
+    boost::asio::awaitable<bool> transform_tree::on_init(const nlohmann::json &config) {
+        pimpl->build_tree(config);
+        co_return true;
     }
 
-    void transform_tree::load_construct_config(const nlohmann::json &config) {
-        pimpl->load_nodes(config);
-        pimpl->load_watches(config);
+    boost::asio::awaitable<bool> transform_tree::on_start(const nlohmann::json &config) {
+        pimpl->config_watcher(config);
+        co_return true;
     }
 
-    transform_tree::index_type transform_tree::get_index_by_name(const std::string &name) {
-        return pimpl->nodes.to_index_by_name(name);
+    boost::asio::awaitable<void> transform_tree::on_stop() {
+        co_await pimpl->stop_workers();
+        co_return;
     }
 
-    transform_tree::~transform_tree() = default;
+    boost::asio::awaitable<void> transform_tree::on_reset() {
+        pimpl->nodes.clear();
+        co_return;
+    }
 
 }

src/core/transform_tree.h

@@ -1,68 +1,8 @@
 #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 "core/tristate_obj.h"
 
-#include <Eigen/Geometry>
-
-#include <nlohmann/json.hpp>
-
-#include <limits>
-#include <memory>
-#include <tuple>
-
-namespace sophiar {
-
-    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;
-        using trans_obj = versatile_obj;
-
-        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:
-
-        struct impl;
-        std::unique_ptr<impl> pimpl;
-
-    };
-
-}
+DEFAULT_TRISTATE_OBJ_DEF(transform_tree)
 
 #endif //SOPHIAR2_TRANSFORM_TREE_H

src/core/tristate_obj.h

@@ -69,6 +69,24 @@ namespace sophiar {
 
     };
 
+#define DEFAULT_TRISTATE_OBJ_DEF(type_name) \
+    namespace sophiar { \
+        class type_name : public tristate_obj { \
+        public: \
+            DEFAULT_NEW_INSTANCE(type_name); \
+            type_name(); \
+            ~type_name() override; \
+        protected: \
+            boost::asio::awaitable<bool> on_init(const nlohmann::json &config) override; \
+            boost::asio::awaitable<bool> on_start(const nlohmann::json &config) override; \
+            boost::asio::awaitable<void> on_stop() override; \
+            boost::asio::awaitable<void> on_reset() override; \
+        private: \
+            struct impl; \
+            std::unique_ptr<impl> pimpl; \
+        }; \
+    }
+
 }
 
 #endif //SOPHIAR2_TRISTATE_OBJ_H

src/tracker/ndi/ndi_interface.cpp

@@ -1,6 +1,7 @@
 #include "ndi_interface.h"
 
 #include "core/basic_obj_types.hpp"
+#include "core/small_obj.hpp"
 #include "utility/debug_utility.hpp"
 #include "utility/coro_worker.hpp"
 #include "utility/name_translator.hpp"

src/tracker/ndi/ndi_interface.h

@@ -1,34 +1,8 @@
 #ifndef SOPHIAR2_NDI_INTERFACE_H
 #define SOPHIAR2_NDI_INTERFACE_H
 
-#include "core/small_obj.hpp"
 #include "core/tristate_obj.h"
 
-namespace sophiar {
-
-    class ndi_interface : public tristate_obj {
-    public:
-
-        ndi_interface();
-
-        ~ndi_interface() override;
-
-    protected:
-
-        boost::asio::awaitable<bool> on_init(const nlohmann::json &config) override;
-
-        boost::asio::awaitable<bool> on_start(const nlohmann::json &config) override;
-
-        boost::asio::awaitable<void> on_stop() override;
-
-        boost::asio::awaitable<void> on_reset() override;
-
-    private:
-        struct impl;
-        std::unique_ptr<impl> pimpl;
-
-    };
-
-}
+DEFAULT_TRISTATE_OBJ_DEF(ndi_interface)
 
 #endif //SOPHIAR2_NDI_INTERFACE_H

src/utility/coro_signal_group.hpp

@@ -27,6 +27,11 @@ namespace sophiar {
             assert(!is_running);
         }
 
+        template<typename Executor>
+        static pointer new_instance(Executor &executor) {
+            return std::make_unique<coro_signal_group<CondFunc>>(executor);
+        }
+
         void add_watcher(signal_watcher &&watcher) {
             assert(!is_running);
             watcher_list.push_back(std::move(watcher));

src/utility/debug_utility.hpp

@@ -1,7 +1,10 @@
 #ifndef SOPHIAR2_DEBUG_UTILITY_HPP
 #define SOPHIAR2_DEBUG_UTILITY_HPP
 
+#include "core/basic_obj_types.hpp"
 #include "core/timestamp_helper.hpp"
+#include "utility/simple_tristate_obj.hpp"
+#include "utility/global_obj_helper.hpp"
 
 #include <boost/asio/awaitable.hpp>
 #include <boost/asio/high_resolution_timer.hpp>
@@ -9,6 +12,7 @@
 #include <boost/asio/use_awaitable.hpp>
 
 #include <fmt/format.h>
+#include <spdlog/spdlog.h>
 
 #include <chrono>
 #include <coroutine>
@@ -36,4 +40,57 @@ inline awaitable<void> coro_sleep(std::chrono::milliseconds t) {
     co_return;
 }
 
+class string_writer {
+public:
+
+    template<typename T>
+    string_writer &operator<<(const T &val) {
+        if (!is_empty) {
+            ss << ", ";
+        }
+        ss << val;
+        is_empty = false;
+        return *this;
+    }
+
+    std::string get_string_and_reset() {
+        auto ret = ss.str();
+        ss.str("");
+        ss.clear();
+        is_empty = true;
+        return ret;
+    }
+
+private:
+    std::stringstream ss;
+    bool is_empty = true;
+};
+
+namespace sophiar {
+
+    template<typename SmallObjType>
+    inline coro_worker::pointer global_obj_watcher_func(const nlohmann::json &config) {
+        assert(config.contains("obj_name"));
+        assert(config["obj_name"].is_string());
+        auto obj_name = config["obj_name"].get<std::string>();
+        auto obj_index = global_sophiar_manager.register_global_obj<SmallObjType>(obj_name);
+        auto worker = make_infinite_coro_worker(global_context, [
+                obj_name, buffer = string_writer{},
+                obj_helper = GLOBAL_OBJ_AUTO_DELEGATE(SmallObjType, obj_index)]() mutable
+                -> awaitable<bool> {
+            co_await obj_helper.coro_wait_update();
+            obj_helper->write_to(buffer);
+            SPDLOG_DEBUG("{} = {}", obj_name, buffer.get_string_and_reset());
+            co_return true;
+        });
+        return std::move(worker);
+    }
+
+    template<typename SmallObjType>
+    using global_obj_watcher = simple_tristate_obj_wrapper<global_obj_watcher_func<SmallObjType>>;
+
+    using transform_obj_watcher = global_obj_watcher<transform_obj>;
+
+}
+
 #endif //SOPHIAR2_DEBUG_UTILITY_HPP

src/utility/global_obj_helper.hpp

@@ -26,6 +26,10 @@ namespace sophiar {
 
         global_obj_delegate(global_obj_delegate &&other) noexcept = default;
 
+        signal_watcher new_watcher() const {
+            return manager.request_global_obj_update_watcher(obj_index);
+        }
+
         void set_value(const pointer_type &ptr,
                        timestamp_type ts = current_timestamp()) {
             manager.update_global_obj<SmallObjType>(obj_index, ptr, ts);
@@ -83,6 +87,9 @@ namespace sophiar {
     template<typename SmallObjType>
     using global_obj_manual_sync_delegate = global_obj_delegate<SmallObjType, false>;
 
+#define GLOBAL_OBJ_AUTO_DELEGATE(obj_type, obj_index) \
+    global_obj_auto_sync_delegate<obj_type>(global_sophiar_manager, obj_index)
+
 }
 
 #endif //SOPHIAR2_GLOBAL_OBJ_HELPER_HPP

src/utility/named_vector.hpp

@@ -62,6 +62,12 @@ namespace sophiar {
             return index >= 0 && index < size();
         }
 
+        void clear() {
+            name_map.clear();
+            name_list.clear();
+            elem_list.clear();
+        }
+
     private:
 
         std::unordered_map<std::string, IndexType> name_map;

src/utility/simple_tristate_obj.hpp

@@ -0,0 +1,72 @@
+#ifndef SOPHIAR2_SIMPLE_TRISTATE_OBJ_HPP
+#define SOPHIAR2_SIMPLE_TRISTATE_OBJ_HPP
+
+#include "core/tristate_obj.h"
+#include "utility/coro_worker.hpp"
+
+#include <utility>
+
+namespace sophiar {
+
+    template<auto CreateFunc, bool Flag = true> // true, create on start; false, create on init
+    class simple_tristate_obj_wrapper : public tristate_obj {
+    public:
+        static_assert(std::is_same<
+                decltype(CreateFunc(std::declval<nlohmann::json>())),
+                coro_worker::pointer>());
+
+        using this_type = simple_tristate_obj_wrapper<CreateFunc, Flag>;
+
+        DEFAULT_NEW_INSTANCE(this_type);
+
+        ~simple_tristate_obj_wrapper() override = default;
+
+    protected:
+
+        boost::asio::awaitable<bool> on_init(const nlohmann::json &config) override {
+            if constexpr (!Flag) {
+                create_worker(config);
+            }
+            co_return true;
+        }
+
+        boost::asio::awaitable<bool> on_start(const nlohmann::json &config) override {
+            if constexpr (Flag) {
+                create_worker(config);
+            }
+            co_return true;
+        }
+
+        boost::asio::awaitable<void> on_stop() override {
+            if constexpr (Flag) {
+                co_await stop_worker();
+            }
+            co_return;
+        }
+
+        boost::asio::awaitable<void> on_reset() override {
+            if constexpr (!Flag) {
+                co_await stop_worker();
+            }
+            co_return;
+        }
+
+    private:
+        coro_worker::pointer worker;
+
+        void create_worker(const nlohmann::json &config) {
+            worker = CreateFunc(config);
+            worker->run();
+        }
+
+        boost::asio::awaitable<void> stop_worker() {
+            worker->cancel();
+            co_await worker->coro_wait_stop();
+            co_return;
+        }
+
+    };
+
+}
+
+#endif //SOPHIAR2_SIMPLE_TRISTATE_OBJ_HPP

tests/CMakeLists.txt

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

tests/core/sophiar_manager.cpp

@@ -5,7 +5,6 @@
 #include "core/basic_obj_types.hpp"
 #include "utility/coro_signal_group.hpp"
 #include "utility/coro_worker.hpp"
-#include "utility/debug_utility.hpp"
 #include "utility/global_obj_helper.hpp"
 
 #include <boost/asio/co_spawn.hpp>
@@ -15,6 +14,8 @@
 #include <boost/asio/use_awaitable.hpp>
 #include <boost/test/unit_test.hpp>
 
+#include <spdlog/spdlog.h>
+
 #include <chrono>
 #include <fstream>
 #include <iostream>

tests/core/transform_tree.cpp

@@ -1,8 +1,13 @@
 #define BOOST_TEST_DYN_LINK
 
 #include "core/transform_tree.h"
+#include "core/basic_obj_types.hpp"
+#include "utility/debug_utility.hpp"
 
 #include <boost/test/unit_test.hpp>
+#include <boost/asio/awaitable.hpp>
+#include <boost/asio/co_spawn.hpp>
+#include <boost/asio/detached.hpp>
 
 #include <nlohmann/json.hpp>
 
@@ -11,25 +16,38 @@
 using namespace nlohmann;
 using namespace sophiar;
 
-BOOST_AUTO_TEST_CASE(test_transform_tree) {
+using boost::asio::awaitable;
+using boost::asio::co_spawn;
+using boost::asio::detached;
 
-    std::ifstream config_file("data/transform_tree_config.json");
-    BOOST_TEST(config_file.is_open());
+BOOST_AUTO_TEST_CASE(test_transform_tree) {
 
-    transform_tree tree;
-    tree.load_construct_config(json::parse(config_file));
+    spdlog::set_level(spdlog::level::trace);
 
-    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)));
+    REGISTER_TYPE(transform_obj_watcher);
+    REGISTER_TYPE(transform_tree);
 
-    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)));
+    std::ifstream config_file("data/transform_tree_config.json");
+    BOOST_TEST(config_file.is_open());
 
+    using namespace std::chrono_literals;
+    auto c_in_b_index = global_sophiar_manager.register_global_obj<transform_obj>("C_in_B");
+    auto worker_a = make_interval_coro_worker(global_context, 1s, [=, cur_y = int(0)]() mutable -> awaitable<bool> {
+        auto new_trans = Eigen::Isometry3d(Eigen::Translation3d(0, ++cur_y, 0));
+        UPDATE_GLOBAL_OBJ_VALUE(transform_obj, c_in_b_index, std::move(new_trans));
+        co_return true;
+    });
+    worker_a->run();
+
+    auto d_in_root_index = global_sophiar_manager.register_global_obj<transform_obj>("D_in_Root");
+    auto worker_b = make_interval_coro_worker(global_context, 2s, [=, cur_z = int(0)]() mutable -> awaitable<bool> {
+        auto new_trans = Eigen::Isometry3d(Eigen::Translation3d(0, 0, --cur_z));
+        UPDATE_GLOBAL_OBJ_VALUE(transform_obj, d_in_root_index, std::move(new_trans));
+        co_return true;
+    });
+    worker_b->run();
+
+    global_sophiar_manager.load_config_and_start(nlohmann::json::parse(config_file));
+
+    global_context.run();
 }

tests/data/transform_tree_config.json

@@ -1,62 +1,55 @@
 {
-  "node_list": [
+  "listen_port": 5277,
+  "object_list": [
     {
-      "name": "Root"
+      "type": "transform_tree",
+      "name": "transform_tree",
+      "init_config": {
+        "node_list": [
+          {
+            "name": "Root"
+          },
+          {
+            "name": "B",
+            "parent": "Root",
+            "transform": [
+              1.123,
+              0,
+              0,
+              1,
+              0,
+              0,
+              0
+            ]
+          },
+          {
+            "name": "C",
+            "parent": "B",
+            "transform_obj_name": "C_in_B"
+          },
+          {
+            "name": "D",
+            "parent": "Root",
+            "transform_obj_name": "D_in_Root"
+          }
+        ]
+      },
+      "start_config": {
+        "watch_list": [
+          {
+            "target": "C",
+            "observer": "D",
+            "transform_name": "C_in_D"
+          }
+        ]
+      }
     },
     {
-      "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"
+      "type": "transform_obj_watcher",
+      "name": "transform_watcher",
+      "start_config": {
+        "obj_name": "C_in_D"
+      }
     }
   ]
 }