实现了记录与重放

src/core/basic_obj_types.hpp

@@ -9,6 +9,9 @@ namespace sophiar {
 
     template<typename T>
     struct small_obj_wrapper : public small_obj<small_obj_wrapper<T>> {
+
+        using value_type = T;
+
         T value;
 
         small_obj_wrapper() = default;
@@ -33,7 +36,7 @@ namespace sophiar {
         static void raw_pointer_write_to(WriterType &writer, void *raw_ptr) {
             using this_type = small_obj_wrapper<T>;
             auto &real_ptr = *static_cast<typename this_type::pointer *>(raw_ptr);
-            real_ptr->write_to(writer);
+            real_ptr->write_to(writer); // TODO handle empty
         }
 
         static constexpr size_t binary_length() {
@@ -66,6 +69,12 @@ namespace sophiar {
         value = Eigen::Quaterniond(qw, qx, qy, qz) * Eigen::Translation3d(tx, ty, tz);
     }
 
+    template<>
+    template<typename ReaderType>
+    inline void scalarxyz_obj::fill_from(ReaderType &reader) {
+        reader >> value.x() >> value.y() >> value.z();
+    }
+
     template<>
     template<typename WriterType>
     inline void transform_obj::write_to(WriterType &writer) const {
@@ -75,6 +84,12 @@ namespace sophiar {
         writer << quat_part.w() << quat_part.x() << quat_part.y() << quat_part.z();
     }
 
+    template<>
+    template<typename WriterType>
+    inline void scalarxyz_obj::write_to(WriterType &writer) const {
+        writer << value.x() << value.y() << value.z();
+    }
+
     template<>
     constexpr size_t transform_obj::binary_length() {
         return 7 * sizeof(double);

src/core/sophiar_manager.cpp

@@ -687,6 +687,12 @@ namespace sophiar {
         return obj_info.placeholder;
     }
 
+    timestamp_type sophiar_manager::get_global_obj_update_timestamp(
+            sophiar::global_obj_index_type obj_index) {
+        auto &obj_info = pimpl->global_obj_pool[obj_index];
+        return obj_info.last_update_ts;
+    }
+
     void sophiar_manager::update_global_obj_timestamp(global_obj_index_type obj_index,
                                                       timestamp_type ts) {
         auto &obj_info = pimpl->global_obj_pool[obj_index];

src/core/sophiar_manager.h

@@ -72,6 +72,8 @@ namespace sophiar {
             return *static_cast<ptr_type *>(placeholder);
         }
 
+        timestamp_type get_global_obj_update_timestamp(global_obj_index_type obj_index);
+
         signal_watcher request_global_obj_update_watcher(global_obj_index_type obj_index);
 
         sophiar_manager();
@@ -112,6 +114,15 @@ namespace sophiar {
 #define REGISTER_TYPE(DerivedT) \
     global_sophiar_manager.register_object_type<DerivedT>(#DerivedT)
 
+#define REGISTER_GLOBAL_OBJ(obj_type, obj_name) \
+    global_sophiar_manager.register_global_obj<obj_type>(obj_name)
+
+#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_VALUE(obj_type, obj_index, obj_value) \
     global_sophiar_manager.update_global_obj<obj_type>(obj_index, obj_type::new_instance(obj_value))
 

src/utility/coro_worker.hpp

@@ -169,28 +169,6 @@ namespace sophiar {
         return std::move(noexcept_func);
     }
 
-    inline auto stop_on_exit_func(tristate_obj *obj) {
-        auto func = [obj]() {
-            assert(obj != nullptr);
-            boost::asio::co_spawn(
-                    global_context,
-                    std::bind(&tristate_obj::stop, obj),
-                    boost::asio::detached);
-        };
-        return std::move(func);
-    }
-
-    inline auto reset_on_exit_func(tristate_obj *obj) {
-        auto func = [obj]() {
-            assert(obj != nullptr);
-            boost::asio::co_spawn(
-                    global_context,
-                    std::bind(&tristate_obj::reset, obj),
-                    boost::asio::detached);
-        };
-        return std::move(func);
-    }
-
 }
 
 #endif //SOPHIAR2_CORO_WORKER_HPP

src/utility/coro_worker_helper_func.hpp

@@ -0,0 +1,37 @@
+#ifndef SOPHIAR2_CORO_WORKER_HELPER_FUNC_HPP
+#define SOPHIAR2_CORO_WORKER_HELPER_FUNC_HPP
+
+#include "core/tristate_obj.h"
+
+#include <boost/asio/co_spawn.hpp>
+#include <boost/asio/detached.hpp>
+
+#include <functional>
+
+namespace sophiar {
+
+    inline auto stop_on_exit_func(tristate_obj *obj) {
+        auto func = [obj]() {
+            assert(obj != nullptr);
+            boost::asio::co_spawn(
+                    global_context,
+                    std::bind(&tristate_obj::stop, obj),
+                    boost::asio::detached);
+        };
+        return std::move(func);
+    }
+
+    inline auto reset_on_exit_func(tristate_obj *obj) {
+        auto func = [obj]() {
+            assert(obj != nullptr);
+            boost::asio::co_spawn(
+                    global_context,
+                    std::bind(&tristate_obj::reset, obj),
+                    boost::asio::detached);
+        };
+        return std::move(func);
+    }
+
+}
+
+#endif //SOPHIAR2_CORO_WORKER_HELPER_FUNC_HPP

src/utility/debug_utility.hpp

@@ -17,6 +17,7 @@
 #include <chrono>
 #include <coroutine>
 #include <cstring>
+#include <fstream>
 #include <iostream>
 
 using boost::asio::awaitable;
@@ -33,7 +34,8 @@ using boost::asio::use_awaitable;
         if (!ok) co_return false; \
     }
 
-inline awaitable<void> coro_sleep(std::chrono::milliseconds t) {
+template<typename DurationType>
+inline awaitable<void> coro_sleep(DurationType t) {
     boost::asio::high_resolution_timer timer(co_await boost::asio::this_coro::executor);
     timer.expires_from_now(t);
     co_await timer.async_wait(use_awaitable);
@@ -70,6 +72,37 @@ private:
     bool is_empty = true;
 };
 
+class string_reader {
+public:
+
+    explicit string_reader(int _sep_length = 1)
+            : sep_length(_sep_length) {}
+
+    void set_string(std::string str) {
+        ss.clear();
+        ss.str(std::move(str));
+        ss.seekg(0, std::ios::beg);
+    }
+
+    template<typename T>
+    string_reader &operator>>(T &val) {
+        assert(!empty());
+        ss >> val;
+        if (!empty()) {
+            ss.ignore(sep_length);
+        }
+        return *this;
+    }
+
+    bool empty() const {
+        return ss.rdbuf()->in_avail() == 0;
+    }
+
+private:
+    std::stringstream ss;
+    int sep_length;
+};
+
 namespace sophiar {
 
     template<typename SmallObjType>
@@ -77,12 +110,16 @@ namespace sophiar {
         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 obj_index = 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();
+            if (obj_helper.empty()) {
+                SPDLOG_DEBUG("{} is empty.", obj_name);
+                co_return true;
+            }
             obj_helper->write_to(buffer);
             SPDLOG_DEBUG("{} = {}", obj_name, buffer.get_string_and_reset());
             co_return true;
@@ -90,11 +127,99 @@ namespace sophiar {
         return std::move(worker);
     }
 
+    template<typename SmallObjType>
+    inline coro_worker::pointer global_obj_recorder_func(const nlohmann::json &config) {
+        // global obj_config
+        assert(config.contains("obj_name"));
+        assert(config["obj_name"].is_string());
+        auto obj_name = config["obj_name"].get<std::string>();
+        auto obj_index = REGISTER_GLOBAL_OBJ(SmallObjType, obj_name);
+        // output file config
+        assert(config.contains("save_file"));
+        assert(config["save_file"].is_string());
+        auto save_file_path = config["save_file"].get<std::string>();
+        auto ofs = std::ofstream(save_file_path, std::ofstream::out);
+        assert(ofs.is_open());
+        // create worker
+        auto worker = make_infinite_coro_worker(global_context, [
+                obj_index, buffer = string_writer(","), ofs = std::move(ofs),
+                obj_helper = GLOBAL_OBJ_AUTO_DELEGATE(SmallObjType, obj_index)]() mutable
+                -> awaitable<bool> {
+            co_await obj_helper.coro_wait_update();
+            auto ts = GLOBAL_OBJ_UPDATE_TS(obj_index);
+            buffer << (static_cast<double>(ts) / 1000.0); // us -> ms
+            if (!obj_helper.empty()) {
+                obj_helper->write_to(buffer);
+            }
+            ofs << buffer.get_string_and_reset() << std::endl;
+            co_return true;
+        });
+        return std::move(worker);
+    }
+
+    template<typename SmallObjType>
+    inline coro_worker::pointer global_obj_replayer_func(const nlohmann::json &config) {
+        // global obj_config
+        assert(config.contains("obj_name"));
+        assert(config["obj_name"].is_string());
+        auto obj_name = config["obj_name"].get<std::string>();
+        auto obj_index = REGISTER_GLOBAL_OBJ(SmallObjType, obj_name);
+        // output file config
+        assert(config.contains("record_file"));
+        assert(config["record_file"].is_string());
+        auto record_file_path = config["record_file"].get<std::string>();
+        auto ifs = std::ifstream(record_file_path, std::ifstream::in);
+        assert(ifs.is_open());
+        // create worker
+        auto worker = make_infinite_coro_worker(global_context, [
+                obj_index, buffer = string_reader(), ifs = std::move(ifs)]() mutable
+                -> awaitable<bool> {
+            std::string str_buf;
+            if (!std::getline(ifs, str_buf)) co_return false; // EOF
+            buffer.set_string(std::move(str_buf));
+            double next_ts_ms;
+            buffer >> next_ts_ms;
+            auto next_ts = static_cast<timestamp_type>(next_ts_ms * 1000); // ms -> us
+            timestamp_type cur_ts = current_timestamp();
+            if (next_ts > cur_ts) {
+                co_await coro_sleep(std::chrono::microseconds(next_ts - cur_ts));
+            } else if (next_ts != cur_ts) { // if time is missed, simply ignore
+                co_return true;
+            }
+            if (buffer.empty()) { // empty obj
+                UPDATE_GLOBAL_OBJ(SmallObjType, obj_index, nullptr);
+            } else {
+                auto new_obj = SmallObjType::new_instance();
+                new_obj->fill_from(buffer);
+                assert(buffer.empty());
+                UPDATE_GLOBAL_OBJ(SmallObjType, obj_index, std::move(new_obj));
+            }
+            co_return true;
+        });
+        return std::move(worker);
+    }
+
     template<typename SmallObjType>
     using global_obj_watcher = simple_tristate_obj_wrapper<global_obj_watcher_func<SmallObjType>>;
 
+    template<typename SmallObjType>
+    using global_obj_recorder = simple_tristate_obj_wrapper<global_obj_recorder_func<SmallObjType>>;
+
+    template<typename SmallObjType>
+    using global_obj_replayer = simple_tristate_obj_wrapper<global_obj_replayer_func<SmallObjType>>;
+
+    using double_obj_watcher = global_obj_watcher<double_obj>;
+    using scalarxyz_obj_watcher = global_obj_watcher<scalarxyz_obj>;
     using transform_obj_watcher = global_obj_watcher<transform_obj>;
 
+    using double_obj_recorder = global_obj_recorder<double_obj>;
+    using scalarxyz_obj_recorder = global_obj_recorder<scalarxyz_obj>;
+    using transform_obj_recorder = global_obj_recorder<transform_obj>;
+
+    using double_obj_replayer = global_obj_replayer<double_obj>;
+    using scalarxyz_obj_replayer = global_obj_replayer<scalarxyz_obj>;
+    using transform_obj_replayer = global_obj_replayer<transform_obj>;
+
 }
 
 #endif //SOPHIAR2_DEBUG_UTILITY_HPP

src/utility/simple_tristate_obj.hpp

@@ -3,6 +3,7 @@
 
 #include "core/tristate_obj.h"
 #include "utility/coro_worker.hpp"
+#include "utility/coro_worker_helper_func.hpp"
 
 #include <utility>
 
@@ -69,8 +70,8 @@ namespace sophiar {
 
         void create_worker(const nlohmann::json &config) {
             worker = CreateFunc(config);
-            create_watchdog();
             worker->run();
+            create_watchdog();
             assert(worker.use_count() == 2);
         }
 

tests/CMakeLists.txt

@@ -18,6 +18,7 @@ add_executable(test_utility
         utility/coro_signal_group.cpp
         utility/coro_worker.cpp
         utility/global_obj_helper.cpp
+        utility/global_obj_record_playback.cpp
         ${EXTERN_DEF_FILES}
         ${CORE_IMPL_FILES})
 target_compile_definitions(test_utility PUBLIC SOPHIAR_TEST)

tests/data/global_obj_record_config.json

@@ -0,0 +1,29 @@
+{
+  "listen_port": 5277,
+  "object_list": [
+    {
+      "type": "transform_obj_recorder",
+      "name": "transform_recorder",
+      "start_config": {
+        "obj_name": "transform_sample",
+        "save_file": "transform_sample.txt"
+      }
+    },
+    {
+      "type": "scalarxyz_obj_recorder",
+      "name": "xyz_recorder",
+      "start_config": {
+        "obj_name": "xyz_sample",
+        "save_file": "xyz_sample.txt"
+      }
+    },
+    {
+      "type": "double_obj_recorder",
+      "name": "double_recorder",
+      "start_config": {
+        "obj_name": "double_sample",
+        "save_file": "double_sample.txt"
+      }
+    }
+  ]
+}

tests/data/global_obj_replay_config.json

@@ -0,0 +1,50 @@
+{
+  "listen_port": 5277,
+  "object_list": [
+    {
+      "type": "transform_obj_watcher",
+      "name": "transform_watcher",
+      "start_config": {
+        "obj_name": "transform_sample"
+      }
+    },
+    {
+      "type": "scalarxyz_obj_watcher",
+      "name": "scalarxyz_watcher",
+      "start_config": {
+        "obj_name": "xyz_sample"
+      }
+    },
+    {
+      "type": "double_obj_watcher",
+      "name": "double_watcher",
+      "start_config": {
+        "obj_name": "double_sample"
+      }
+    },
+    {
+      "type": "transform_obj_replayer",
+      "name": "transform_replayer",
+      "start_config": {
+        "obj_name": "transform_sample",
+        "record_file": "transform_sample.txt"
+      }
+    },
+    {
+      "type": "scalarxyz_obj_replayer",
+      "name": "xyz_replayer",
+      "start_config": {
+        "obj_name": "xyz_sample",
+        "record_file": "xyz_sample.txt"
+      }
+    },
+    {
+      "type": "double_obj_replayer",
+      "name": "double_replayer",
+      "start_config": {
+        "obj_name": "double_sample",
+        "record_file": "double_sample.txt"
+      }
+    }
+  ]
+}

tests/utility/global_obj_record_playback.cpp

@@ -0,0 +1,74 @@
+#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>
+
+#include <fstream>
+
+using namespace nlohmann;
+using namespace sophiar;
+
+using boost::asio::awaitable;
+using boost::asio::co_spawn;
+using boost::asio::detached;
+
+BOOST_AUTO_TEST_CASE(test_global_obj_recoreder) {
+
+    spdlog::set_level(spdlog::level::trace);
+
+    REGISTER_TYPE(double_obj_recorder);
+    REGISTER_TYPE(scalarxyz_obj_recorder);
+    REGISTER_TYPE(transform_obj_recorder);
+
+    std::ifstream config_file("data/global_obj_record_config.json");
+    BOOST_TEST(config_file.is_open());
+
+    using namespace std::chrono_literals;
+    auto double_sample_index = REGISTER_GLOBAL_OBJ(double_obj, "double_sample");
+    auto xyz_sample_index = REGISTER_GLOBAL_OBJ(scalarxyz_obj, "xyz_sample");
+    auto transform_sample_index = REGISTER_GLOBAL_OBJ(transform_obj, "transform_sample");
+    auto worker_a = make_interval_coro_worker(global_context, 1s, [=]() mutable -> awaitable<bool> {
+        static auto sample_val = 1.1;
+        auto new_double = double_obj::value_type(sample_val);
+        auto new_xyz = scalarxyz_obj::value_type(0.1, 0.2, sample_val);
+        auto new_trans = transform_obj::value_type(Eigen::Translation3d(0.3, 0.4, sample_val));
+        UPDATE_GLOBAL_OBJ_VALUE(double_obj, double_sample_index, new_double);
+        UPDATE_GLOBAL_OBJ_VALUE(scalarxyz_obj, xyz_sample_index, std::move(new_xyz));
+        UPDATE_GLOBAL_OBJ_VALUE(transform_obj, transform_sample_index, std::move(new_trans));
+        sample_val += 0.6;
+        co_return true;
+    });
+    worker_a->run();
+
+    global_sophiar_manager.load_config_and_start(nlohmann::json::parse(config_file));
+
+    global_context.run();
+}
+
+BOOST_AUTO_TEST_CASE(test_global_obj_replayer) {
+
+    spdlog::set_level(spdlog::level::trace);
+
+    REGISTER_TYPE(double_obj_watcher);
+    REGISTER_TYPE(scalarxyz_obj_watcher);
+    REGISTER_TYPE(transform_obj_watcher);
+
+    REGISTER_TYPE(double_obj_replayer);
+    REGISTER_TYPE(scalarxyz_obj_replayer);
+    REGISTER_TYPE(transform_obj_replayer);
+
+    std::ifstream config_file("data/global_obj_replay_config.json");
+    BOOST_TEST(config_file.is_open());
+
+    global_sophiar_manager.load_config_and_start(nlohmann::json::parse(config_file));
+
+    global_context.run();
+}