搁置配准标定算法的开发

benchmark/CMakeLists.txt

@@ -1,7 +1,4 @@
 find_package(benchmark REQUIRED)
 
 add_executable(benchmark_small_obj core/small_obj.cpp ${EXTERN_DEF_FILES})
-target_link_libraries(benchmark_small_obj benchmark::benchmark ${EXTRA_LIBS})
-
-add_executable(benchmark_datanode_base core/datanode_base.cpp ${EXTERN_DEF_FILES} ${CORE_IMPL_FILES})
-target_link_libraries(benchmark_datanode_base benchmark::benchmark ${EXTRA_LIBS})
+target_link_libraries(benchmark_small_obj benchmark::benchmark ${EXTRA_LIBS})

benchmark/core/datanode_base.cpp

@@ -1,68 +0,0 @@
-#include "core/datanode_base.h"
-
-#include <boost/asio/awaitable.hpp>
-#include <boost/asio/co_spawn.hpp>
-#include <boost/asio/detached.hpp>
-
-#include <cassert>
-
-#include <benchmark/benchmark.h>
-
-using boost::asio::awaitable;
-using boost::asio::co_spawn;
-using boost::asio::detached;
-
-using namespace sophiar;
-
-static void BM_timestamp(benchmark::State &state) {
-    for (auto _: state) {
-        benchmark::DoNotOptimize(sophiar::current_timestamp());
-    }
-}
-
-BENCHMARK(BM_timestamp);
-
-static void BM_datanode_chain(benchmark::State &state) {
-
-    struct test_type : public datanode_base<high_freq_tag> {
-        uint64_t cnt = 0;
-
-        awaitable<bool> exec() {
-            auto input_data = get_input_data(0);
-            auto output_data = data_packet::new_instance();
-            output_data->copy_content(*input_data);
-            (*output_data)[0] += 1.0;
-            output_data->timestamp = current_timestamp();
-            cnt = (*output_data)[0];
-            set_output_data(0, output_data);
-            co_return true;
-        }
-    };
-
-    co_spawn(high_freq_context, [&]() -> awaitable<void> {
-        auto total = state.range(0);
-        auto pool = new test_type[total];
-        for (size_t i = 0; i < total; ++i) {
-            auto &node = pool[i];
-            co_await node.init();
-            node.set_trigger_mode(test_type::trigger_mode_type::INPUT);
-            node.set_trigger_input_mask(0b1);
-            if (i != 0) {
-                test_type::connect(pool[i - 1], 0, node, 0);
-            }
-            co_await node.start();
-        }
-
-        auto test_data = test_type::data_packet::new_instance();
-        test_data->refresh();
-        for (auto _: state) {
-            pool[0].update_input_data(0, test_data);
-            assert(pool[total - 1].cnt == total);
-        }
-    }, detached);
-    high_freq_context.run();
-}
-
-BENCHMARK(BM_datanode_chain)->RangeMultiplier(10)->Range(1, 1e6);
-
-BENCHMARK_MAIN();

src/algorithm/landmark_registration.cpp

@@ -0,0 +1,132 @@
+#include "landmark_registration.h"
+
+#include "algorithm/measure_window.hpp"
+#include "core/basic_obj_types.hpp"
+#include "utility/bit_operations.hpp"
+#include "utility/config_utility.hpp"
+#include "utility/coro_worker.hpp"
+#include "utility/global_obj_helper.hpp"
+
+#include <Eigen/Geometry>
+
+namespace sophiar {
+
+    using boost::asio::awaitable;
+
+    using namespace Eigen;
+
+    struct landmark_registration::impl {
+
+        using point_set_type = Matrix3Xd;
+
+        size_t point_number;
+        int next_index = 0;
+        point_set_type fiducial_points;
+        point_set_type pickled_points;
+
+        global_obj_index_type input_obj_index;
+        global_obj_index_type output_trans_obj_index;
+        global_obj_index_type output_error_obj_index;
+
+        coro_worker::pointer main_worker;
+
+        void load_fiducial_points(const nlohmann::json &config) {
+            assert(config.is_array());
+            point_number = config.size();
+            assert(point_number >= 3);
+            fiducial_points = point_set_type(3, point_number);
+            for (int i = 0; i < point_number; ++i) {
+                auto &point_config = config[i];
+                assert(point_config.is_array());
+                assert(point_config.size() == 3);
+                for (int j = 0; j < 3; ++j) {
+                    assert(point_config[j].is_number());
+                    fiducial_points(j, i) = point_config[j].get<double>();
+                }
+            }
+        }
+
+        void load_config(const nlohmann::json &config) {
+            assert(config.contains("fiducial_points"));
+            load_fiducial_points(config["fiducial_points"]);
+            // I/O
+            input_obj_index = LOAD_GLOBAL_OBJ_INDEX(scalarxyz_obj, input_point_obj_name);
+            output_trans_obj_index = LOAD_GLOBAL_OBJ_INDEX(transform_obj, output_transform_obj_name);
+            output_error_obj_index = LOAD_GLOBAL_OBJ_INDEX(double_obj, output_error_obj_name);
+        }
+
+        int get_closest_pickled_point_index(const Vector3d &point) const {
+            double min_dis = std::numeric_limits<double>::max();
+            int ret_index = -1;
+            for (int i = 0; i < point_number; ++i) {
+                auto dis = (pickled_points.col(i) - point).norm();
+                if (dis < min_dis) {
+                    min_dis = dis;
+                    ret_index = i;
+                }
+            }
+            assert(ret_index != -1);
+            return ret_index;
+        }
+
+        double calc_registration_error(const Isometry3d &trans) const {
+            auto distances = VectorXd(point_number);
+            for (int i = 0; i < point_number; ++i) {
+                auto transformed_point = trans * fiducial_points.col(i);
+                auto closest_index = get_closest_pickled_point_index(transformed_point);
+                distances(i) = (pickled_points.col(closest_index) - transformed_point).norm();
+            }
+            auto dis_norm = distances.norm();
+            return dis_norm / std::sqrt(point_number);
+        }
+
+        void do_registration() const {
+            auto result = umeyama(fiducial_points, pickled_points, false); // no scaling
+            UPDATE_GLOBAL_OBJ_VALUE(transform_obj, output_trans_obj_index, result);
+            auto error_value = calc_registration_error(Isometry3d(result));
+            UPDATE_GLOBAL_OBJ_VALUE(double_obj, output_error_obj_index, error_value);
+        }
+
+        void create_worker() {
+            assert(main_worker == nullptr);
+            auto input_helper = GLOBAL_OBJ_AUTO_DELEGATE(scalarxyz_obj, input_obj_index);
+            input_helper.manual_sync();
+            auto func = [this,
+                    input_helper = std::move(input_helper)]() mutable
+                    -> awaitable<bool> {
+                co_await input_helper.coro_wait_update(false);
+                if (input_helper.empty()) co_return true;
+                pickled_points.col(next_index++) = input_helper->value;
+                if (next_index == point_number) {
+                    do_registration();
+                    co_return false;
+                }
+                co_return true;
+            };
+            main_worker = make_infinite_coro_worker(get_context(), std::move(func));
+            main_worker->run();
+        }
+
+    };
+
+    awaitable<bool> landmark_registration::on_init(const nlohmann::json &config) {
+        co_return true;
+    }
+
+    awaitable<bool> landmark_registration::on_start(const nlohmann::json &config) {
+        pimpl->load_config(config);
+        pimpl->create_worker();
+        pimpl->next_index = 0;
+        co_return true;
+    }
+
+    awaitable<void> landmark_registration::on_stop() {
+        SAFE_CLOSE_CORO_WORKER(pimpl->main_worker)
+        co_return;
+    }
+
+    awaitable<void> landmark_registration::on_reset() {
+        co_return;
+    }
+
+}

src/algorithm/landmark_registration.h

@@ -0,0 +1,8 @@
+#ifndef SOPHIAR2_LANDMARK_REGISTRATION_H
+#define SOPHIAR2_LANDMARK_REGISTRATION_H
+
+#include "core/tristate_obj.h"
+
+DEFAULT_TRISTATE_OBJ_DEF(landmark_registration)
+
+#endif //SOPHIAR2_LANDMARK_REGISTRATION_H

src/algorithm/measure_window.hpp

@@ -0,0 +1,79 @@
+#ifndef SOPHIAR2_MEASURE_WINDOW_HPP
+#define SOPHIAR2_MEASURE_WINDOW_HPP
+
+#include <boost/core/noncopyable.hpp>
+
+#include <cmath>
+#include <queue>
+
+namespace sophiar {
+
+    class measure_window : private boost::noncopyable {
+    public:
+
+        static constexpr auto max_length = std::numeric_limits<size_t>::max();
+
+        explicit measure_window(size_t _length = max_length)
+                : win_length(_length) {}
+
+        void reset() {
+            while (!win.empty()) {
+                win.pop();
+            }
+            cur_n = 0;
+            sum_x = 0.0;
+            sum_x2 = 0.0;
+        }
+
+        void pop() {
+            assert(!win.empty());
+            auto val = win.front();
+            win.pop();
+            sum_x -= val;
+            sum_x2 -= val * val;
+            --cur_n;
+        }
+
+        void push(double val) {
+            if (cur_n == win_length) {
+                pop();
+            }
+            win.push(val);
+            sum_x += val;
+            sum_x2 += val * val;
+            ++cur_n;
+        }
+
+        double calc_mean() const {
+            assert(cur_n >= 1);
+            return sum_x / static_cast<double>(cur_n);
+        }
+
+        double calc_std() const {
+            assert(cur_n >= 2);
+            auto mean_val = calc_mean();
+            return std::sqrt(sum_x2
+                             - 2 * mean_val * sum_x
+                             + static_cast<double>(cur_n) * mean_val * mean_val)
+                   / static_cast<double>(cur_n - 1);
+        }
+
+        bool is_full() const {
+            return win.size() == win_length;
+        }
+
+        size_t length() const {
+            return win.size();
+        }
+
+    private:
+        size_t win_length;
+        size_t cur_n = 0;
+        double sum_x = 0, sum_x2 = 0;
+        std::queue<double> win;
+
+    };
+
+}
+
+#endif //SOPHIAR2_MEASURE_WINDOW_HPP

src/algorithm/transform_stabilizer.cpp

@@ -0,0 +1,328 @@
+#include "transform_stabilizer.h"
+
+#include "algorithm/measure_window.hpp"
+#include "core/basic_obj_types.hpp"
+#include "utility/bit_operations.hpp"
+#include "utility/coro_worker.hpp"
+
+#include <Eigen/Geometry>
+
+namespace sophiar {
+
+    using boost::asio::awaitable;
+
+    using namespace Eigen;
+
+    struct transform_stabilizer::impl {
+
+        static constexpr double default_linear_tolerance_mm = 0.2;
+        static constexpr double default_angular_tolerance_deg = 0.05;
+
+        bool stable_point_only = true; // 是否仅稳定一个点
+        double linear_tolerance = 0.0002; // 0.2 mm
+        double angular_tolerance = 0.01; // 0.01 rad
+
+        bool use_temporal_interval = true; // 使用基于时间长度的窗长控制
+        timestamp_type temporal_interval = 1500000; // 1.5s
+        timestamp_type counting_start_ts = 0, last_sample_ts = 0;
+        size_t counting_interval = 150;
+
+        measure_window pos_win[3];
+        measure_window ang_win[3];
+        measure_window qua_win[4];
+
+        global_obj_index_type input_obj_index = invalid_global_obj_index;
+        global_obj_index_type output_obj_index = invalid_global_obj_index;
+
+        coro_worker::pointer stable_worker;
+
+#define ALL_WIN_OP(cmd) \
+        for (auto &win: pos_win) cmd \
+        for (auto &win: ang_win) cmd \
+        for (auto &win: qua_win) cmd
+
+        void reset_windows() {
+            ALL_WIN_OP({ win.reset(); })
+        }
+
+        size_t get_cur_length() const {
+            return pos_win[0].length();
+        }
+
+        auto get_rotation() const {
+            assert(get_cur_length() >= 1);
+            auto x = qua_win[0].calc_mean();
+            auto y = qua_win[1].calc_mean();
+            auto z = qua_win[2].calc_mean();
+            auto w = qua_win[3].calc_mean();
+            auto rot_quat = Quaterniond{w, x, y, z};
+            rot_quat.normalize();
+            return rot_quat;
+        }
+
+        auto get_translation() const {
+            assert(get_cur_length() >= 1);
+            auto x = pos_win[0].calc_mean();
+            auto y = pos_win[1].calc_mean();
+            auto z = pos_win[2].calc_mean();
+            return Translation3d{x, y, z};
+        }
+
+        auto get_transform() const {
+            return get_translation() * get_rotation();
+        }
+
+        double calc_rotation_std() const {
+            assert(get_cur_length() >= 2);
+            double std2 = 0.0;
+            for (auto &win: ang_win) {
+                auto std = win.calc_std();
+                std2 += std * std;
+            }
+            return std::sqrt(std2);
+        }
+
+        double calc_translation_std() const {
+            assert(get_cur_length() >= 2);
+            double std2 = 0.0;
+            for (auto &win: pos_win) {
+                auto std = win.calc_std();
+                std2 += std * std;
+            }
+            return std::sqrt(std2);
+        }
+
+        template<bool UseTemporalInterval>
+        bool check_interval_condition() const {
+            if constexpr (UseTemporalInterval) {
+                return last_sample_ts - counting_start_ts >= temporal_interval;
+            } else {
+                return get_cur_length() >= counting_interval;
+            }
+            assert(false);
+            return false;
+        }
+
+        bool check_rotation_condition() const {
+            return calc_rotation_std() <= angular_tolerance;
+        }
+
+        bool check_translation_condition() const {
+            return calc_translation_std() <= linear_tolerance;
+        }
+
+        template<bool StablePointOnly, bool UseTemporalInterval>
+        bool check_stable() const {
+            if (!check_interval_condition<UseTemporalInterval>()) return false;
+            if (!check_translation_condition()) return false;
+            if constexpr (!StablePointOnly) {
+                if (!check_rotation_condition()) return false;
+            }
+            return true;
+        }
+
+        void update_sample_ts(timestamp_type ts = current_timestamp()) {
+            if (get_cur_length() == 0) {
+                counting_start_ts = ts;
+            }
+            last_sample_ts = ts;
+        }
+
+        template<typename RotationType>
+        void on_new_rotation_sample(const RotationType &rot) {
+            static const Vector3d unit_vector[] = {
+                    {1, 0, 0},
+                    {0, 1, 0},
+                    {0, 0, 1}
+            };
+            for (int i = 0; i < 3; ++i) { // 以三轴角度记录
+                auto rot_vector = rot * unit_vector[i];
+                auto angle = acos(unit_vector[i].dot(rot_vector));
+                ang_win[i].push(angle);
+            }
+            auto qua = Quaterniond(rot);
+            auto &qua_coeff = qua.coeffs(); // 以四元数形式进行记录
+            for (int j = 0; j < 4; ++j) {
+                qua_win[j].push(qua_coeff(j));
+            }
+        }
+
+        template<typename Vector3Type>
+        void on_new_translation_sample(const Vector3Type &trans) {
+            for (int i = 0; i < 3; ++i) {
+                pos_win[i].push(trans(i));
+            }
+        }
+
+        void on_new_transform_sample(const Isometry3d &trans) {
+            on_new_translation_sample(trans.translation());
+            on_new_rotation_sample(trans.rotation());
+        }
+
+        void on_point_stabled() const {
+            auto trans_result = get_translation();
+            auto ret = scalarxyz_obj::new_instance();
+            ret->value.x() = trans_result.x();
+            ret->value.y() = trans_result.y();
+            ret->value.z() = trans_result.z();
+            UPDATE_GLOBAL_OBJ(scalarxyz_obj, output_obj_index, ret);
+        }
+
+        void on_transform_stabled() const {
+            auto trans_result = get_transform();
+            auto ret = transform_obj::new_instance();
+            ret->value = trans_result;
+            UPDATE_GLOBAL_OBJ(transform_obj, output_obj_index, ret);
+        }
+
+        template<bool StablePointOnly>
+        void on_stabled() const {
+            assert(is_valid_id(output_obj_index));
+            if constexpr (StablePointOnly) {
+                on_point_stabled();
+            } else {
+                on_transform_stabled();
+            }
+        }
+
+        template<bool StablePointOnly, bool UseTemporalInterval>
+        coro_worker::pointer create_worker_impl() {
+            auto func = [this,
+                    watcher = NEW_GLOBAL_OBJ_WATCHER(input_obj_index)]() mutable
+                    -> awaitable<bool> {
+                co_await watcher.coro_wait(false);
+                auto update_ts = GLOBAL_OBJ_UPDATE_TS(input_obj_index);
+                if constexpr (StablePointOnly) {
+                    auto point = GET_GLOBAL_OBJ(scalarxyz_obj, input_obj_index);
+                    if (point == nullptr) {
+                        reset_windows();
+                    } else {
+                        if constexpr (UseTemporalInterval) {
+                            update_sample_ts(update_ts);
+                        }
+                        on_new_translation_sample(point->value);
+                    }
+                } else {
+                    auto trans = GET_GLOBAL_OBJ(transform_obj, input_obj_index);
+                    if (trans == nullptr) {
+                        reset_windows();
+                    } else {
+                        if constexpr (UseTemporalInterval) {
+                            update_sample_ts(update_ts);
+                        }
+                        on_new_transform_sample(trans->value);
+                    }
+                }
+                if (check_stable<StablePointOnly, UseTemporalInterval>()) [[unlikely]] {
+                    on_stabled<StablePointOnly>();
+                }
+                co_return true;
+            };
+            return make_infinite_coro_worker(get_context(), std::move(func));
+        }
+
+        void create_worker() {
+            assert(stable_worker == nullptr);
+            if (stable_point_only) {
+                if (use_temporal_interval) {
+                    stable_worker = create_worker_impl<true, true>();
+                } else {
+                    stable_worker = create_worker_impl<true, false>();
+                }
+            } else {
+                if (use_temporal_interval) {
+                    stable_worker = create_worker_impl<false, true>();
+                } else {
+                    stable_worker = create_worker_impl<false, false>();
+                }
+            }
+            stable_worker->run();
+        }
+
+        void load_config(const nlohmann::json &config) {
+            // stable type
+            assert(config.contains("stable_type"));
+            assert(config["stable_type"].is_string());
+            if (config["stable_type"] == "transform") {
+                stable_point_only = false;
+            } else {
+                assert(config["stable_type"] == "point");
+                stable_point_only = true;
+            }
+
+            // I/O
+            assert(config.contains("input_obj_name"));
+            assert(config.contains("output_obj_name"));
+            assert(config["input_obj_name"].is_string());
+            assert(config["output_obj_name"].is_string());
+            auto input_obj_name = config["input_obj_name"].get<std::string>();
+            auto output_obj_name = config["output_obj_name"].get<std::string>();
+            if (stable_point_only) {
+                input_obj_index = REGISTER_GLOBAL_OBJ(scalarxyz_obj, input_obj_name);
+                output_obj_index = REGISTER_GLOBAL_OBJ(scalarxyz_obj, output_obj_name);
+            } else {
+                input_obj_index = REGISTER_GLOBAL_OBJ(transform_obj, input_obj_name);
+                output_obj_index = REGISTER_GLOBAL_OBJ(transform_obj, output_obj_name);
+            }
+
+            // linear tolerance
+            double linear_tolerance_mm = default_linear_tolerance_mm;
+            if (config.contains("linear_tolerance_mm")) {
+                assert(config["linear_tolerance_mm"].is_number());
+                linear_tolerance_mm = config["linear_tolerance_mm"].get<double>();
+            } else {
+                // TODO show log, use default value
+            }
+            linear_tolerance = linear_tolerance_mm * 1000.0; // mm -> m
+
+            // angular tolerance
+            if (!stable_point_only) {
+                double angular_tolerance_deg = default_angular_tolerance_deg;
+                if (config.contains("angular_tolerance_deg")) {
+                    assert(config["angular_tolerance_deg"].is_number());
+                    angular_tolerance_deg = config["angular_tolerance_deg"].get<double>();
+                } else {
+                    // TODO show log, use default value
+                }
+                angular_tolerance = angular_tolerance_deg * std::numbers::pi / 180.0; // deg -> rad
+            }
+
+            // interval
+            if (config.contains("temporal_interval_s")) {
+                assert(config["temporal_interval_s"].is_number());
+                use_temporal_interval = true;
+                auto temporal_interval_s = config["temporal_interval_s"].get<double>();
+                temporal_interval = temporal_interval_s * 1000000; // s -> us
+            } else {
+                assert(config.contains("counting_interval"));
+                assert(config["counting_interval"].is_number_unsigned());
+                use_temporal_interval = false;
+                counting_interval = config["counting_interval"].get<uint64_t>();
+            }
+        }
+
+    };
+
+    awaitable<bool> transform_stabilizer::on_init(const nlohmann::json &config) {
+        co_return true;
+    }
+
+    awaitable<bool> transform_stabilizer::on_start(const nlohmann::json &config) {
+        pimpl->load_config(config);
+        pimpl->reset_windows();
+        pimpl->create_worker();
+        co_return true;
+    }
+
+    awaitable<void> transform_stabilizer::on_stop() {
+        pimpl->stable_worker->cancel();
+        co_await pimpl->stable_worker->coro_wait_stop();
+        pimpl->stable_worker = nullptr;
+        co_return;
+    }
+
+    awaitable<void> transform_stabilizer::on_reset() {
+        co_return;
+    }
+
+}

src/algorithm/transform_stabilizer.h

@@ -0,0 +1,8 @@
+#ifndef SOPHIAR2_TRANSFORM_STABILIZER_H
+#define SOPHIAR2_TRANSFORM_STABILIZER_H
+
+#include "core/tristate_obj.h"
+
+DEFAULT_TRISTATE_OBJ_DEF(transform_stabilizer)
+
+#endif //SOPHIAR2_TRANSFORM_STABILIZER_H

src/algorithm/transform_utility.hpp

@@ -0,0 +1,111 @@
+#ifndef SOPHIAR2_TRANSFORM_UTILITY_HPP
+#define SOPHIAR2_TRANSFORM_UTILITY_HPP
+
+#include "core/basic_obj_types.hpp"
+#include "utility/coro_signal_group.hpp"
+#include "utility/coro_worker.hpp"
+#include "utility/simple_tristate_obj.hpp"
+#include "utility/global_obj_helper.hpp"
+
+#include <cassert>
+
+namespace sophiar {
+
+    inline coro_worker::pointer make_transform_inverter_func(const nlohmann::json &config) {
+        assert(config.contains("input_obj_name"));
+        assert(config.contains("output_obj_name"));
+        assert(config["input_obj_name"].is_string());
+        assert(config["output_obj_name"].is_string());
+        auto input_obj_name = config["input_obj_name"].get<std::string>();
+        auto output_obj_name = config["output_obj_name"].get<std::string>();
+        auto input_obj_index = REGISTER_GLOBAL_OBJ(transform_obj, input_obj_name);
+        auto output_obj_index = REGISTER_GLOBAL_OBJ(transform_obj, output_obj_name);
+        auto worker = make_infinite_coro_worker(global_context, [=,
+                input_helper = GLOBAL_OBJ_AUTO_DELEGATE(transform_obj, input_obj_index)]() mutable
+                -> boost::asio::awaitable<bool> {
+            co_await input_helper.coro_wait_update(false);
+            auto ts = GLOBAL_OBJ_UPDATE_TS(input_obj_index);
+            if (input_helper.empty()) [[unlikely]] {
+                UPDATE_GLOBAL_OBJ_TS(transform_obj, output_obj_index, nullptr, ts);
+            } else [[likely]] {
+                auto new_trans = transform_obj::new_instance();
+                new_trans->value = input_helper->value.inverse();
+                UPDATE_GLOBAL_OBJ_TS(transform_obj, output_obj_index, new_trans, ts);
+            }
+            co_return true;
+        });
+        return std::move(worker);
+    }
+
+    scalarxyz_obj::value_type get_scalarxyz_from_json_list(const nlohmann::json &config) {
+        assert(config.is_array());
+        assert(config.size() == 3);
+        double val[3];
+        for (int i = 0; i < 3; ++i) {
+            assert(config[i].is_number());
+            val[i] = config[i].get<double>();
+        }
+        return {val[0], val[1], val[2]};
+    }
+
+    template<bool IsVector>
+    inline coro_worker::pointer make_fixed_scalarxyz_transformer(const nlohmann::json &config) {
+        assert(config["transform_type"] == "point");
+        auto fixed_value = get_scalarxyz_from_json_list(config["fixed_value"]);
+        auto trans_obj_name = config["transform_obj_name"].get<std::string>();
+        auto output_obj_name = config["output_obj_name"].get<std::string>();
+        auto trans_obj_index = REGISTER_GLOBAL_OBJ(transform_obj, trans_obj_name);
+        auto output_obj_index = REGISTER_GLOBAL_OBJ(scalarxyz_obj, output_obj_name);
+        auto worker = make_infinite_coro_worker(global_context, [=,
+                trans_helper = GLOBAL_OBJ_AUTO_DELEGATE(transform_obj, trans_obj_index)]() mutable
+                -> boost::asio::awaitable<bool> {
+            co_await trans_helper.coro_wait_update(false);
+            auto ts = GLOBAL_OBJ_UPDATE_TS(trans_obj_index);
+            if (trans_helper.empty()) [[unlikely]] {
+                UPDATE_GLOBAL_OBJ_TS(scalarxyz_obj, output_obj_index, nullptr, ts);
+            } else [[likely]] {
+                auto new_value = scalarxyz_obj::new_instance();
+                if constexpr (IsVector) {
+                    new_value->value = trans_helper->value.linear() * fixed_value;
+                } else {
+                    new_value->value = trans_helper->value * fixed_value;
+                }
+                UPDATE_GLOBAL_OBJ_TS(scalarxyz_obj, output_obj_index, new_value, ts);
+            }
+            co_return true;
+        });
+        return std::move(worker);
+    }
+
+    inline coro_worker::pointer make_scalarxyz_transformer(const nlohmann::json &config) {
+        assert(config.contains("transform_obj_name"));
+        assert(config.contains("output_obj_name"));
+        assert(config["transform_obj_name"].is_string());
+        assert(config["output_obj_name"].is_string());
+        assert(config.contains("transform_type"));
+        assert(config["transform_type"].is_string());
+        auto trans_type = config["transform_type"].get<std::string>();
+        if (config.contains("fixed_value")) {
+            if (trans_type == "vector") {
+                return make_fixed_scalarxyz_transformer<true>(config);
+            } else {
+                assert(trans_type == "point");
+                return make_fixed_scalarxyz_transformer<false>(config);
+            }
+        } else {
+            assert(config.contains("input_obj_name"));
+            assert(config["input_obj_name"].is_string());
+            // TODO 实现 dynamic_scalaxyz_transformer
+            assert(false);
+            return nullptr;
+        }
+        assert(false);
+        return nullptr;
+    }
+
+    using transform_inverter = simple_tristate_obj_wrapper<make_transform_inverter_func>;
+    using scalarxyz_transformer = simple_tristate_obj_wrapper<make_scalarxyz_transformer>;
+
+}
+
+#endif //SOPHIAR2_TRANSFORM_UTILITY_HPP

src/core/basic_obj_types.hpp

@@ -24,6 +24,12 @@ namespace sophiar {
         explicit small_obj_wrapper(T &&other)
                 : value(std::move(other)) {}
 
+        template<typename... Args>
+        explicit small_obj_wrapper(Args &&...args)
+                : value(std::forward<Args>(args)...) {
+            static_assert(std::is_constructible_v<T, Args...>);
+        }
+
         template<typename ReaderType>
         void fill_from(ReaderType &reader) {
             reader >> value;

src/core/datanode_base.cpp

@@ -1,371 +0,0 @@
-#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"
-
-#include <boost/asio/co_spawn.hpp>
-#include <boost/asio/detached.hpp>
-#include <boost/asio/experimental/awaitable_operators.hpp>
-#include <boost/asio/high_resolution_timer.hpp>
-#include <boost/asio/this_coro.hpp>
-#include <boost/asio/use_awaitable.hpp>
-
-#include <fmt/format.h>
-
-#include <cassert>
-#include <functional>
-#include <string>
-
-namespace sophiar {
-
-    using boost::asio::awaitable;
-    using boost::asio::co_spawn;
-    using boost::asio::detached;
-    using boost::asio::use_awaitable;
-
-    using namespace boost::asio::experimental::awaitable_operators;
-
-    name_translator<datanode_base::trigger_mode_type> trigger_mode_translator;
-
-    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 update_slot_impl_type : public update_slot_type {
-            impl *p_this = nullptr;
-            uint8_t channel_index;
-
-            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;
-
-        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];
-
-        channel_mask_type input_update_mask = 0;
-        channel_mask_type output_updated_mask = 0;
-
-        trigger_mode_type trigger_mode = trigger_mode_type::MANUAL;
-        channel_mask_type trigger_input_mask = 0;
-        std::chrono::microseconds trigger_interval{0};
-        std::chrono::microseconds minimal_exec_interval{0};
-        boost::asio::high_resolution_timer trigger_timer;
-
-        exec_state_type exec_state = exec_state_type::IDLE;
-        bool last_exec_success = false;
-        timestamp_type last_exec_ts = 0;
-
-        bool enable_stimer = false; // 是否启用统计时钟
-
-        using trigger_stimer_type = statistic_timer<statistic_timer_single_shot_tag>;
-        using exec_stimer_type = statistic_timer<statistic_timer_start_stop_tag>;
-        trigger_stimer_type trigger_stimer;
-        exec_stimer_type exec_stimer;
-
-        coro_signal exec_cancel_signal;
-        coro_signal run_finished_signal;
-        bool exec_block = false;
-
-        impl()
-                : 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);
-        }
-
-        uint8_t 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);
-            return slot_index;
-        }
-
-        uint8_t 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]);
-            return 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>());
-            }
-        }
-
-        bool check_is_running() const {
-            return q_this->get_state() == state_type::RUNNING;
-        }
-
-        bool check_input_trigger_condition() const {
-            return (input_update_mask & trigger_input_mask) == trigger_input_mask;
-        }
-
-        void commit_output() {
-            // 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;
-            }
-        }
-
-        awaitable<bool> exec_impl(timestamp_type current_ts = current_timestamp()) {
-            exec_state = exec_state_type::RUNNING;
-            last_exec_ts = current_ts;
-
-            if (enable_stimer) {
-                trigger_stimer.start(current_ts);
-                exec_stimer.start(current_ts);
-            }
-            auto ret_val = co_await q_this->exec();
-            if (enable_stimer) {
-                exec_stimer.stop();
-            }
-            last_exec_success = ret_val;
-
-            input_update_mask = 0;
-            commit_output();
-            exec_state = exec_state_type::IDLE;
-
-            co_return ret_val;
-        }
-
-        awaitable<void> periodic_run() {
-            auto exec_cancel_token = exec_cancel_signal.new_token();
-            for (;;) {
-                co_await exec_impl();
-                assert(exec_state == exec_state_type::IDLE);
-
-                // check if cancelled
-                if (exec_cancel_signal.try_wait(exec_cancel_token)) break;
-
-                // schedule next run
-                auto current_ts = current_timestamp();
-                auto time_left = last_exec_ts + trigger_interval.count() - current_ts;
-                if (time_left <= 0) {
-                    // TODO show warning, requested frequency too high
-                } else {
-                    exec_state = exec_state_type::PENDING;
-                    trigger_timer.expires_at(get_time_from_timestamp(last_exec_ts) + trigger_interval);
-                    auto await_result = co_await (trigger_timer.async_wait(use_awaitable) ||
-                                                  exec_cancel_signal.coro_wait(exec_cancel_token));
-                    if (await_result.index() == 1) { // cancelled
-                        exec_state = exec_state_type::IDLE;
-                        break;
-                    }
-                }
-            }
-
-            run_finished_signal.try_notify_all();
-            co_return;
-        }
-
-        awaitable<bool> on_start(const nlohmann::json &config) {
-            load_start_config(config);
-            trigger_stimer.reset();
-            exec_stimer.reset();
-            exec_block = false;
-            if (trigger_mode == trigger_mode_type::PERIODIC) {
-                if (trigger_interval.count() < 0) {
-                    co_return false;
-                } else if (trigger_interval.count() == 0) {
-                    // TODO show warning, running without interval
-                }
-                co_spawn(get_context(), periodic_run(), detached);
-            }
-            co_return true;
-        }
-
-        awaitable<void> on_stop() {
-            exec_block = true;
-            if (trigger_mode == trigger_mode_type::PERIODIC ||
-                exec_state == exec_state_type::PENDING) {
-                exec_cancel_signal.try_notify_all();
-            }
-            if (exec_state == exec_state_type::RUNNING) {
-                co_await run_finished_signal.coro_wait();
-            }
-            co_return;
-        }
-
-        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(get_context(), trigger(), detached);
-                }
-            }
-        }
-
-        boost::asio::awaitable<bool> trigger() {
-            if (!check_is_running()) co_return false; // only trigger when running
-            if (exec_block || exec_state != exec_state_type::IDLE) co_return false;
-            assert(trigger_mode != trigger_mode_type::PERIODIC);
-            auto current_ts = current_timestamp();
-            auto time_left = last_exec_ts + minimal_exec_interval.count() - current_ts;
-            if (time_left > 0) {
-                exec_state = exec_state_type::PENDING;
-                trigger_timer.expires_at(get_time_from_timestamp(last_exec_ts) + minimal_exec_interval);
-                auto await_result = co_await (trigger_timer.async_wait(use_awaitable) ||
-                                              exec_cancel_signal.coro_wait());
-                if (await_result.index() == 1) { // cancelled
-                    exec_state = exec_state_type::IDLE;
-                    run_finished_signal.try_notify_all();
-                    co_return false;
-                } else {
-                    current_ts = current_timestamp();
-                }
-            }
-
-            auto ret_val = co_await exec_impl(current_ts);
-            run_finished_signal.try_notify_all();
-            co_return ret_val;
-        }
-
-//        bool cancel_pending() {
-//            if (exec_state != exec_state_type::PENDING) return false;
-//            if (trigger_mode == trigger_mode_type::PERIODIC) return false;
-//            exec_cancel_signal.try_notify_all();
-//            return true;
-//        }
-
-    };
-
-    awaitable<bool> datanode_base::on_start(const nlohmann::json &config) {
-        return pimpl->on_start(config);
-    }
-
-    awaitable<void> datanode_base::on_stop() {
-        return pimpl->on_stop();
-    }
-
-    datanode_base::datanode_base()
-            : pimpl(std::make_unique<impl>()) {
-        pimpl->q_this = this;
-    }
-
-    void datanode_base::load_construct_config(const nlohmann::json &json) {
-        pimpl->create_trigger_slot();
-    }
-
-    uint8_t datanode_base::register_input(const std::string &name) {
-        return pimpl->create_input_slot(name);
-    }
-
-    uint8_t datanode_base::register_output(const std::string &name) {
-        return pimpl->create_output_signal(name);
-    }
-
-//    bool datanode_base::cancel_pending() {
-//        return pimpl->cancel_pending();
-//    }
-
-    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];
-    }
-
-    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);
-    }
-
-    datanode_base::exec_state_type datanode_base::get_exec_state() const {
-        return pimpl->exec_state;
-    }
-
-    timestamp_type datanode_base::get_last_exec_ts() const {
-        return pimpl->last_exec_ts;
-    }
-
-    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());
-//    }
-
-    void datanode_base::enable_statistic_timer() {
-        pimpl->enable_stimer = true;
-    }
-
-    void datanode_base::disable_statistic_timer() {
-        pimpl->enable_stimer = false;
-    }
-
-    datanode_base::~datanode_base() = default;
-
-}

src/core/datanode_base.h

@@ -1,84 +0,0 @@
-#ifndef SOPHIAR2_DATANODE_BASE_H
-#define SOPHIAR2_DATANODE_BASE_H
-
-#include "core/small_obj.hpp"
-#include "core/timestamp_helper.hpp"
-#include "core/tristate_obj.h"
-#include "core/types/versatile_data.hpp"
-
-#include <chrono>
-#include <cstring>
-#include <memory>
-
-namespace sophiar {
-
-    class [[deprecated]] datanode_base : public tristate_obj {
-    public:
-
-        enum class exec_state_type {
-            IDLE,
-            PENDING,
-            RUNNING
-        };
-
-        enum class trigger_mode_type {
-            MANUAL,
-            INPUT,
-            PERIODIC
-        };
-
-        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;
-
-        // 创建 trigger slot
-        void load_construct_config(const nlohmann::json &json) override;
-
-        exec_state_type get_exec_state() const;
-
-        timestamp_type get_last_exec_ts() const;
-
-        bool is_last_exec_success() const;
-
-        void enable_statistic_timer();
-
-        void disable_statistic_timer();
-
-//        std::string get_statistic_info_as_string() const;
-
-//        bool cancel_pending();
-
-    protected: // for child class to use
-
-        uint8_t register_input(const std::string &name);
-
-        uint8_t register_output(const std::string &name);
-
-        data_pointer_type get_input_data(uint8_t channel_index);
-
-        void set_output_data(uint8_t channel_index, data_pointer_type data);
-
-        virtual boost::asio::awaitable<bool> exec() = 0;
-
-    protected: // implementation
-
-        boost::asio::awaitable<bool> on_start(const nlohmann::json &config) override;
-
-        boost::asio::awaitable<void> on_stop() override;
-
-    private:
-
-        struct impl;
-        std::unique_ptr<impl> pimpl;
-
-    };
-
-}
-
-#endif //SOPHIAR2_DATANODE_BASE_H

src/core/sophiar_manager.h

@@ -5,6 +5,7 @@
 #include "utility/coro_signal2.hpp"
 
 #include <boost/asio/awaitable.hpp>
+#include <boost/asio/io_context.hpp>
 
 #include <fmt/format.h>
 
@@ -61,6 +62,7 @@ namespace sophiar {
             using ptr_type = typename SmallObjType::pointer;
             auto placeholder = get_global_obj_placeholder(obj_index, typeid(SmallObjType));
             auto &inner_ptr = *static_cast<ptr_type *>(placeholder);
+            if (inner_ptr == nullptr && value == nullptr) return; // nullptr value will not be duplicated
             inner_ptr = value;
             update_global_obj_timestamp(obj_index, ts);
         }
@@ -109,6 +111,7 @@ namespace sophiar {
 
     };
 
+    extern boost::asio::io_context global_context;
     extern sophiar_manager global_sophiar_manager;
 
 #define REGISTER_TYPE(DerivedT) \
@@ -117,12 +120,21 @@ namespace sophiar {
 #define REGISTER_GLOBAL_OBJ(obj_type, obj_name) \
     global_sophiar_manager.register_global_obj<obj_type>(obj_name)
 
+#define GET_GLOBAL_OBJ(obj_type, obj_index) \
+    global_sophiar_manager.get_global_obj<obj_type>(obj_index)
+
+#define NEW_GLOBAL_OBJ_WATCHER(obj_index) \
+    global_sophiar_manager.request_global_obj_update_watcher(obj_index)
+
 #define GLOBAL_OBJ_UPDATE_TS(obj_index) \
     global_sophiar_manager.get_global_obj_update_timestamp(obj_index)
 
 #define UPDATE_GLOBAL_OBJ(obj_type, obj_index, obj) \
     global_sophiar_manager.update_global_obj<obj_type>(obj_index, obj)
 
+#define UPDATE_GLOBAL_OBJ_TS(obj_type, obj_index, obj, ts) \
+    global_sophiar_manager.update_global_obj<obj_type>(obj_index, obj, ts)
+
 #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))
 

src/core/sophiar_obj.hpp

@@ -3,14 +3,10 @@
 
 #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:
 

src/core/transform_tree.cpp

@@ -14,6 +14,7 @@
 #include <Eigen/Geometry>
 
 #include <cassert>
+#include <list>
 #include <optional>
 #include <string>
 #include <vector>

src/extern_defs/core/sophiar_manager.cpp

@@ -1,7 +1,27 @@
 #include "core/sophiar_manager.h"
+#include "third_party/static_block.hpp"
+
+#include <boost/predef.h>
+
+#ifdef BOOST_OS_WINDOWS_AVAILABLE
+
+#include <timeapi.h>
+#include <cstdlib>
+
+#endif // BOOST_OS_WINDOWS_AVAILABLE
 
 namespace sophiar {
 
+    boost::asio::io_context global_context;
     sophiar_manager global_sophiar_manager;
 
+#ifdef BOOST_OS_WINDOWS_AVAILABLE
+
+    static_block { // make windows timer more precise
+        timeBeginPeriod(1);
+        std::atexit([]() { timeEndPeriod(1); });
+    };
+
+#endif // BOOST_OS_WINDOWS_AVAILABLE
+
 }

src/extern_defs/core/sophiar_obj.cpp

@@ -1,26 +0,0 @@
-#include "core/sophiar_obj.hpp"
-#include "third_party/static_block.hpp"
-
-#include <boost/predef.h>
-
-#ifdef BOOST_OS_WINDOWS_AVAILABLE
-
-#include <timeapi.h>
-#include <cstdlib>
-
-#endif // BOOST_OS_WINDOWS_AVAILABLE
-
-namespace sophiar {
-
-    boost::asio::io_context global_context;
-
-#ifdef BOOST_OS_WINDOWS_AVAILABLE
-
-    static_block { // make windows timer more precise
-        timeBeginPeriod(1);
-        std::atexit([]() { timeEndPeriod(1); });
-    };
-
-#endif // BOOST_OS_WINDOWS_AVAILABLE
-
-}

src/main.cpp

@@ -2,6 +2,8 @@
 #include <iostream>
 #include <thread>
 
+#include "algorithm/transform_utility.hpp"
+#include "algorithm/measure_window.hpp"
 #include "robot/ur/ur_interface.h"
 #include "utility/debug_utility.hpp"
 #include "core/types/geometry_types.hpp"

src/utility/config_utility.hpp

@@ -0,0 +1,15 @@
+#ifndef SOPHIAR2_CONFIG_UTILITY_HPP
+#define SOPHIAR2_CONFIG_UTILITY_HPP
+
+#define LOAD_GLOBAL_OBJ_INDEX(obj_type, obj_name) ({ \
+    assert(config.contains(#obj_name)); \
+    assert(config[#obj_name].is_string()); \
+    auto obj_name = config[#obj_name].get<std::string>(); \
+    REGISTER_GLOBAL_OBJ(obj_type, obj_name); })
+
+#define SAFE_CLOSE_CORO_WORKER(worker) \
+    worker->cancel(); \
+    co_await worker->coro_wait_stop(); \
+    worker = nullptr;
+
+#endif //SOPHIAR2_CONFIG_UTILITY_HPP

src/utility/global_obj_helper.hpp

@@ -35,11 +35,15 @@ namespace sophiar {
             manager.update_global_obj<SmallObjType>(obj_index, ptr, ts);
         }
 
+        void manual_sync() {
+            if (watcher.try_wait()) {// new value available
+                obj_ptr = manager.get_global_obj<SmallObjType>(obj_index);
+            }
+        }
+
         pointer_type get_value() {
             if constexpr (AutoSync) {
-                if (watcher.try_wait()) { // new value available
-                    obj_ptr = manager.get_global_obj<SmallObjType>(obj_index);
-                }
+                manual_sync();
             }
             return obj_ptr;
         }
@@ -90,6 +94,9 @@ namespace sophiar {
 #define GLOBAL_OBJ_AUTO_DELEGATE(obj_type, obj_index) \
     global_obj_auto_sync_delegate<obj_type>(global_sophiar_manager, obj_index)
 
+#define GLOBAL_OBJ_MANUAL_DELEGATE(obj_type, obj_index) \
+    global_obj_manual_sync_delegate<obj_type>(global_sophiar_manager, obj_index)
+
 }
 
 #endif //SOPHIAR2_GLOBAL_OBJ_HELPER_HPP

tests/core/datanode_base.cpp

@@ -1,276 +0,0 @@
-#define BOOST_TEST_DYN_LINK
-
-#include "core/datanode_base.h"
-#include "utility/bit_operations.hpp"
-#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 <fmt/format.h>
-
-#include <algorithm>
-#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 cnt_node_type : public datanode_base {
-    DEFAULT_NEW_INSTANCE(cnt_node_type);
-
-    unsigned int cnt = 0;
-
-    void load_construct_config(const nlohmann::json &config) override {
-        auto input_index = register_input("input_1");
-        BOOST_TEST(input_index == 0);
-        datanode_base::load_construct_config(config);
-    }
-
-    awaitable<bool> exec() {
-        ++cnt;
-        co_return true;
-    }
-};
-
-struct trigger_node_type : public sophiar_obj {
-    DEFAULT_NEW_INSTANCE(trigger_node_type);
-
-    tiny_signal<> trigger_signal;
-
-    void load_construct_config(const nlohmann::json &config) override {
-        get_manager().register_signal(this, "triggered", trigger_signal);
-    }
-
-    void trigger() {
-        trigger_signal.emit();
-    }
-};
-
-struct update_node_type : public sophiar_obj {
-    DEFAULT_NEW_INSTANCE(update_node_type);
-
-    tiny_signal<versatile_obj::pointer> trigger_signal;
-
-    void load_construct_config(const nlohmann::json &config) override {
-        get_manager().register_signal(this, "triggered", trigger_signal);
-    }
-
-    void trigger() {
-        trigger_signal.emit(versatile_obj::new_instance());
-    }
-};
-
-struct test_node_type : public datanode_base {
-    DEFAULT_NEW_INSTANCE(test_node_type);
-
-    unsigned int cnt = 0;
-
-    void load_construct_config(const nlohmann::json &config) override {
-        auto input_index = register_input("input_1");
-        BOOST_TEST(input_index == 0);
-        auto output_index = register_output("output_1");
-        BOOST_TEST(output_index == 0);
-        datanode_base::load_construct_config(config);
-    }
-
-    awaitable<bool> exec() {
-        auto input_data = get_input_data(0);
-        cnt = (*input_data)[0];
-        auto output_data = versatile_obj::new_instance();
-        output_data->copy_from(*input_data);
-        (*output_data)[0] += 1.0;
-        set_output_data(0, output_data);
-        co_return true;
-    }
-};
-
-struct speedtest_node_type : public datanode_base {
-    uint64_t cnt = 0;
-
-    void load_construct_config(const nlohmann::json &config) override {
-        auto input_index = register_input("input_1");
-        BOOST_TEST(input_index == 0);
-        auto output_index = register_output("output_1");
-        BOOST_TEST(output_index == 0);
-        datanode_base::load_construct_config(config);
-    }
-
-    awaitable<bool> exec() {
-        auto input_data = get_input_data(0);
-        auto output_data = versatile_obj::new_instance();
-        output_data->copy_from(*input_data);
-        for (size_t i = 0; i < versatile_data::FLOAT_FILED_LENGTH; ++i) {
-            (*output_data)[i] += i;
-        }
-        std::reverse(output_data->floats, output_data->floats + versatile_data::FLOAT_FILED_LENGTH);
-        output_data->timestamp = current_timestamp();
-        cnt = (*output_data)[0];
-        set_output_data(0, output_data);
-        co_return true;
-    }
-};
-
-awaitable<void> test_datanode_base_1() {
-
-    auto trigger_node = dynamic_cast<trigger_node_type *>(global_sophiar_manager.get_object("trigger_node"));
-    auto cnt_node = dynamic_cast<cnt_node_type *>(global_sophiar_manager.get_object("cnt_node"));
-    assert(trigger_node != nullptr);
-    assert(cnt_node != nullptr);
-
-    co_await global_sophiar_manager.switch_mode("mode_a");
-
-    trigger_node->trigger();
-    co_await coro_sleep(10ms);
-    BOOST_TEST(cnt_node->cnt == 1);
-
-    co_await coro_sleep(50ms);
-    for (int i = 0; i < 5; ++i) {
-        trigger_node->trigger();
-    }
-    BOOST_TEST(cnt_node->cnt == 2);
-    co_await coro_sleep(50ms);
-    BOOST_TEST(cnt_node->cnt == 3);
-
-    co_await global_sophiar_manager.switch_mode("mode_b");
-
-    auto update_node = dynamic_cast<update_node_type *>(global_sophiar_manager.get_object("update_node"));
-    assert(update_node != nullptr);
-
-    cnt_node->cnt = 0;
-    update_node->trigger();
-    co_await coro_sleep(10ms);
-    BOOST_TEST(cnt_node->cnt == 1);
-
-    co_await coro_sleep(50ms);
-    for (int i = 0; i < 5; ++i) {
-        update_node->trigger();
-    }
-    BOOST_TEST(cnt_node->cnt == 2);
-    co_await coro_sleep(50ms);
-    BOOST_TEST(cnt_node->cnt == 3);
-
-    cnt_node->cnt = 0;
-    co_spawn(co_await boost::asio::this_coro::executor, [&]() -> awaitable<void> {
-        co_await coro_sleep(275ms);
-        co_await cnt_node->stop();
-    }, detached);
-
-    co_await global_sophiar_manager.switch_mode("mode_c");
-    co_await coro_sleep(350ms);
-    BOOST_TEST(cnt_node->cnt == 6);
-    BOOST_TEST((cnt_node->get_state() == datanode_base::state_type::PENDING));
-
-    auto test_node_a = dynamic_cast<test_node_type *>(global_sophiar_manager.get_object("test_node_a"));
-    auto test_node_b = dynamic_cast<test_node_type *>(global_sophiar_manager.get_object("test_node_b"));
-    assert(test_node_a != nullptr);
-    assert(test_node_b != nullptr);
-
-    auto &test_slot = global_sophiar_manager.get_slot<versatile_obj::pointer>("test_node_a", "input_1");
-
-    co_await global_sophiar_manager.switch_mode("mode_d");
-    auto packet = versatile_obj::new_instance();
-    (*packet)[0] = 100;
-    test_slot.signal_received(std::move(packet));
-    BOOST_TEST(test_node_a->cnt == 100);
-    BOOST_TEST(test_node_b->cnt == 101);
-
-    co_return;
-}
-
-awaitable<void> test_datanode_base_speed(size_t length, size_t repeat) {
-
-    std::destroy_at(&global_sophiar_manager);
-    std::construct_at(&global_sophiar_manager);
-
-    nlohmann::json config_json;
-    config_json["mode_list"].push_back({{"name", "test_mode"}});
-
-    nlohmann::json object_config;
-    object_config["type"] = "speedtest_node_type";
-    object_config["enabled_modes"].push_back("test_mode");
-    object_config["construct_config"] = nlohmann::json({});
-    object_config["init_configs"] = nlohmann::json::array();
-
-    nlohmann::json start_config;
-    start_config["modes"].push_back("test_mode");
-    start_config["config"]["trigger_mode"] = "input";
-    start_config["config"]["input_mask"] = 1;
-    object_config["start_configs"].push_back(start_config);
-
-    for (auto i = 0; i < length; ++i) {
-        object_config["name"] = fmt::format("speedtest_node_{}", i);
-        config_json["object_list"].push_back(object_config);
-    }
-
-    std::cout << config_json.dump() << std::endl;
-
-    // TODO
-//
-//    auto pool = new test_type[length];
-//    for (size_t i = 0; i < length; ++i) {
-//        auto &node = pool[i];
-//        co_await node.init();
-//        node.set_trigger_mode(test_type::trigger_mode_type::INPUT);
-//        node.set_trigger_input_mask(0b1);
-//        if (i != 0) {
-//            test_type::connect(pool[i - 1], 0, node, 0);
-//        }
-//        co_await node.start();
-//    }
-//
-//    auto test_data = test_type::data_packet::new_instance();
-//    auto start_ts = current_timestamp();
-//    for (int i = 0; i < repeat; ++i) {
-//        pool[0].update_input_data(0, test_data);
-////        assert(pool[length - 1].cnt == length);
-//    }
-//
-//    auto time_used = (current_timestamp() - start_ts) / 1000.0;
-//    auto time_left = 1000.0 / repeat - time_used / repeat;
-//    std::cout << fmt::format("Length = {}, Repeat = {}, "
-//                             "Time used = {:.3f}ms ({:.3f}ms, {:.2f}% left)",
-//                             length, repeat, time_used,
-//                             time_left, time_left / (10.0 / repeat))
-//              << std::endl;
-//
-//    BOOST_TEST(true);
-    co_return;
-}
-
-BOOST_AUTO_TEST_CASE(test_datanode_base) {
-
-    spdlog::set_level(spdlog::level::trace);
-
-    REGISTER_TYPE(cnt_node_type);
-    REGISTER_TYPE(trigger_node_type);
-    REGISTER_TYPE(update_node_type);
-    REGISTER_TYPE(test_node_type);
-
-    std::ifstream config_file("data/datanode_base_config.json");
-    BOOST_TEST(config_file.is_open());
-
-    global_sophiar_manager.load_config_and_start(nlohmann::json::parse(config_file));
-
-    co_spawn(global_context, test_datanode_base_1(), detached);
-//    co_spawn(high_freq_context, test_datanode_base_2(), detached);
-//    for (auto length: {1, 4, 16, 64, 128})
-//        for (auto repeat: {1, 50, 125, 500, 1000, 2000, 5000, 10000})
-//            co_spawn(high_freq_context, test_datanode_base_speed(length, repeat), detached);
-    global_context.run();
-}
-
-BOOST_AUTO_TEST_CASE(test_datanode_speed) {
-    co_spawn(global_context, test_datanode_base_speed(3, 1), detached);
-    global_context.run();
-}

tests/data/landmark_registration_config.json

@@ -0,0 +1,158 @@
+{
+  "listen_port": 5277,
+  "object_list": [
+    {
+      "type": "transform_tree",
+      "name": "global_transform_tree",
+      "init_config": {
+        "node_list": [
+          {
+            "name": "tracker"
+          },
+          {
+            "name": "probe",
+            "parent": "tracker",
+            "transform_obj_name": "probe_in_tracker"
+          },
+          {
+            "name": "model_ref",
+            "parent": "tracker",
+            "transform_obj_name": "model_ref_in_tracker"
+          },
+          {
+            "name": "model",
+            "parent": "model_ref",
+            "transform_obj_name": "model_in_model_ref"
+          }
+        ]
+      },
+      "start_config": {
+        "watch_list": [
+          {
+            "target": "probe",
+            "observer": "model",
+            "transform_name": "probe_in_model"
+          },
+          {
+            "target": "probe",
+            "observer": "model_ref",
+            "transform_name": "probe_in_model_ref"
+          }
+        ]
+      }
+    },
+    {
+      "type": "ndi_interface",
+      "name": "main_ndi",
+      "init_config": {
+        "address_type": "ethernet",
+        "ip": "169.254.132.51",
+        "tcp_port": 8765,
+        "com_port": "COM3",
+        "tool_list": [
+          {
+            "rom_path": "D:\\Program\\Robot\\Tools\\roms\\Probe_Small_4Ball.rom",
+            "outputs": {
+              "transform": "probe_in_tracker",
+              "marker_uncertainty": "probe_in_tracker_uncertainty"
+            }
+          },
+          {
+            "rom_path": "D:\\Program\\Robot\\Tools\\roms\\Head_3Ball_2.rom",
+            "outputs": {
+              "transform": "model_ref_in_tracker",
+              "marker_uncertainty": "model_ref_in_tracker_uncertainty"
+            }
+          }
+        ]
+      },
+      "start_config": {
+        "allow_unreliable": true,
+        "prefer_stream_tracking": true
+      }
+    },
+    {
+      "type": "transform_stabilizer",
+      "name": "model_ref_point_pickler",
+      "dependencies": [
+        "global_transform_tree",
+        "main_ndi"
+      ],
+      "start_config": {
+        "stable_type": "point",
+        "input_obj_name": "probe_in_model_ref",
+        "output_obj_name": "pickled_point_in_model_ref",
+        "linear_tolerance_mm": 0.2,
+        "angular_tolerance_deg": 0.05,
+        "temporal_interval_s": 1.5,
+        "counting_interval": 150
+      }
+    },
+    {
+      "type": "landmark_registration",
+      "name": "model_registration",
+      "dependencies": [
+        "model_ref_point_pickler"
+      ],
+      "start_config": {
+        "fiducial_points": [
+          [
+            164.66571453500848,
+            18.71164620295797,
+            82.62518570008311
+          ],
+          [
+            123.52831200584696,
+            14.231978533211485,
+            96.3638417317588
+          ],
+          [
+            93.07753837629471,
+            11.094425704616972,
+            79.15928190277317
+          ],
+          [
+            143.6043085335037,
+            25.819879662834747,
+            108.72907272943397
+          ],
+          [
+            132.738357125514,
+            26.348048379275646,
+            109.0070454170842
+          ],
+          [
+            116.95809799419658,
+            24.962181959549664,
+            109.67909720928142
+          ]
+        ],
+        "input_point_obj_name": "pickled_point_in_model_ref",
+        "output_transform_obj_name": "model_in_model_ref",
+        "output_error_obj_name": "model_registration_error"
+      }
+    },
+    {
+      "type": "transform_obj_recorder",
+      "name": "model_ref_in_tracker_recorder",
+      "dependencies": [
+        "main_ndi"
+      ],
+      "start_config": {
+        "obj_name": "probe_in_tracker",
+        "save_file": "probe_in_tracker.txt"
+      }
+    },
+    {
+      "type": "transform_obj_recorder",
+      "name": "model_ref_in_tracker_recorder",
+      "dependencies": [
+        "main_ndi"
+      ],
+      "start_config": {
+        "obj_name": "model_ref_in_tracker",
+        "save_file": "model_ref_in_tracker.txt"
+      }
+    }
+  ]
+}

tests/data/transform_utility_config.json

@@ -0,0 +1,72 @@
+{
+  "listen_port": 5277,
+  "object_list": [
+    {
+      "type": "transform_inverter",
+      "name": "sample_inverter",
+      "start_config": {
+        "input_obj_name": "C_in_D",
+        "output_obj_name": "D_in_C"
+      }
+    },
+    {
+      "type": "scalarxyz_transformer",
+      "name": "sample_transformer",
+      "start_config": {
+        "transform_type": "point",
+        "input_obj_name": "probe_offset",
+        "fixed_value": [
+          1,
+          2,
+          3
+        ],
+        "transform_obj_name": "tracker_probe_transform",
+        "output_obj_name": "point_in_probe"
+      }
+    },
+    {
+      "type": "transform_stabilizer",
+      "name": "sample_stabilizer",
+      "start_config": {
+        "stable_type": "point",
+        "input_obj_name": "point_in_tracker",
+        "output_obj_name": "pickled_point_in_tracker",
+        "linear_tolerance_mm": 0.2,
+        "angular_tolerance_deg": 0.05,
+        "temporal_interval_s": 1.5,
+        "counting_interval": 150
+      }
+    },
+    {
+      "type": "landmark_registration",
+      "name": "sample_registration",
+      "start_config": {
+        "fiducial_points": [
+          [
+            0,
+            0,
+            0
+          ],
+          [
+            0,
+            1,
+            1
+          ],
+          [
+            1,
+            1,
+            1
+          ],
+          [
+            1,
+            1,
+            2
+          ]
+        ],
+        "input_point_obj_name": "pickled_point_in_tracker",
+        "output_transform_obj_name": "registration_result",
+        "output_error_obj_name": "registration_error"
+      }
+    }
+  ]
+}