实现了二进制模式的变量IO

src/algorithm/transform_tree.cpp

@@ -13,13 +13,14 @@
 
 #include <Eigen/Geometry>
 
-#include <cassert>
 #include <list>
 #include <optional>
 #include <string>
 #include <vector>
 #include <utility>
 
+#include "utility/assert_utility.h"
+
 namespace sophiar {
 
     using namespace Eigen;

src/core/basic_obj_types.hpp

@@ -44,13 +44,21 @@ namespace sophiar {
         template<typename WriterType>
         static void raw_pointer_write_to(WriterType &writer, void *raw_ptr) {
             auto &real_ptr = *static_cast<typename this_type::pointer *>(raw_ptr);
-            real_ptr->write_to(writer); // TODO handle empty
+            if (real_ptr == nullptr) [[unlikely]] return;
+            real_ptr->write_to(writer);
         }
 
         template<typename ReaderType>
         static void raw_pointer_fill_from(ReaderType &reader, void *raw_ptr) {
             auto &real_ptr = *static_cast<typename this_type::pointer *>(raw_ptr);
-            real_ptr->fill_from(reader); // TODO handle empty
+            if (reader.empty()) [[unlikely]] {
+                real_ptr = nullptr;
+                return;
+            }
+            if (real_ptr == nullptr) [[unlikely]] {
+                real_ptr = this_type::new_instance();
+            }
+            real_ptr->fill_from(reader);
         }
 
         static constexpr size_t binary_length() {
@@ -140,6 +148,7 @@ namespace sophiar {
         using value_type = typename SmallObjType::value_type;
         using element_type = value_type;
         static constexpr size_t element_size = 1;
+        static_assert(std::is_arithmetic_v<value_type>);
     };
 
     template<>

src/core/external_variable_io.cpp

@@ -0,0 +1,350 @@
+#include "external_variable_io.h"
+#include "core/global_defs.h"
+#include "core/sophiar_pool.h"
+#include "utility/config_utility.hpp"
+#include "utility/coro_signal_group.hpp"
+#include "utility/coro_worker.hpp"
+#include "utility/dynamic_pool.hpp"
+#include "utility/versatile_buffer2.hpp"
+
+#include <boost/asio/awaitable.hpp>
+#include <boost/asio/defer.hpp>
+#include <boost/asio/detached.hpp>
+#include <boost/asio/co_spawn.hpp>
+#include <boost/asio/ip/tcp.hpp>
+#include <boost/asio/redirect_error.hpp>
+#include <boost/asio/use_awaitable.hpp>
+#include <boost/system/error_code.hpp>
+
+#include <spdlog/spdlog.h>
+
+namespace sophiar {
+
+    using namespace boost::asio::ip;
+    using boost::asio::awaitable;
+    using boost::asio::co_spawn;
+    using boost::asio::detached;
+    using boost::asio::redirect_error;
+    using boost::asio::use_awaitable;
+    using boost::system::error_code;
+
+    struct external_variable_io::impl {
+
+        static constexpr uint16_t DEFAULT_LISTEN_PORT = 5278;
+
+        using header_type = uint16_t;
+        using variable_size_type = uint8_t;
+        static constexpr size_t header_offset = sizeof(header_type);
+        static constexpr size_t variable_size_type_offset = sizeof(variable_size_type);
+
+        using variable_info_list = dynamic_vector<sophiar_pool::variable_info>;
+
+        struct binary_in_client;
+        struct binary_out_client;
+
+        uint16_t listen_port = DEFAULT_LISTEN_PORT;
+        coro_worker::pointer listen_worker;
+
+        static awaitable<void> create_client_worker(tcp::socket &&s);
+
+        void load_config(const nlohmann::json &config) {
+            if (config.contains("variable_io_port")) {
+                listen_port = LOAD_UINT_ITEM("variable_io_port");
+            } else {
+                listen_port = DEFAULT_LISTEN_PORT;
+                // TODO show log
+            }
+        }
+
+        bool start() {
+            assert(listen_worker == nullptr);
+            auto listen_endpoint = tcp::endpoint(tcp::v4(), listen_port);
+            try {
+                auto listen_func = [=,
+                        acceptor = tcp::acceptor(*global_context, listen_endpoint)
+                ]() mutable -> awaitable<bool> {
+                    error_code ec;
+                    auto client = co_await acceptor.async_accept(redirect_error(use_awaitable, ec));
+                    if (ec) {
+                        SPDLOG_ERROR("Error while waiting for client: {}", ec.what());
+                        co_return false;
+                    } else {
+                        co_spawn(*global_context, create_client_worker(std::move(client)), detached);
+                    }
+                    co_return true;
+                };
+                listen_worker = make_infinite_coro_worker(std::move(listen_func));
+                listen_worker->run();
+                SPDLOG_INFO("External variable I/O is listen on {}:{}",
+                            listen_endpoint.address().to_string(),
+                            listen_endpoint.port());
+            } catch (std::exception &e) {
+                // TODO show log, maybe port is occupied.
+                return false;
+            }
+
+            return true;
+        }
+
+        static sophiar_pool::variable_info query_variable_information(std::string_view var_name) {
+            return global_sophiar_pool->query_variable_information(var_name);
+        }
+
+        static void update_variable_timestamp(variable_index_type var_index, timestamp_type ts) {
+            return global_sophiar_pool->update_variable_timestamp_impl(var_index, ts);
+        }
+
+        template<typename WriterType>
+        static auto require_raw_pointer_writer(std::type_index var_type) {
+            return global_sophiar_pool->require_raw_pointer_writer<WriterType>(var_type);
+        }
+
+        template<typename ReaderType>
+        static auto require_raw_pointer_reader(std::type_index var_type) {
+            return global_sophiar_pool->require_raw_pointer_reader<ReaderType>(var_type);
+        }
+
+    };
+
+    external_variable_io::external_variable_io()
+            : pimpl(std::make_unique<impl>()) {
+    }
+
+    external_variable_io::~external_variable_io() = default;
+
+    bool external_variable_io::load_config_and_start(const nlohmann::json &config) {
+        pimpl->load_config(config);
+        return pimpl->start();
+    }
+
+    struct external_variable_io::impl::binary_in_client {
+
+        using reader_type = versatile_reader<sophiar_endian>;
+        using reader_func_type = void (*)(reader_type &, void *);
+
+        struct variable_io_info {
+            void *placeholder;
+            reader_func_type reader_func;
+        };
+
+        using info_pool_type = std::unordered_map<variable_index_type, variable_io_info>;
+
+        tcp::socket s;
+        info_pool_type info_pool;
+        dynamic_memory::pointer buf;
+
+        binary_in_client(tcp::socket &&_s, const variable_info_list &infos)
+                : s(std::move(_s)),
+                  buf(dynamic_memory::new_instance()) {
+            for (auto &info: infos) {
+                assert(!info_pool.contains(info.var_index));
+                info_pool.emplace(info.var_index,
+                                  variable_io_info{
+                                          .placeholder = info.placeholder,
+                                          .reader_func = require_raw_pointer_reader<reader_type>(info.type)
+                                  });
+            }
+        }
+
+        awaitable<bool> work_once() {
+            // receive message
+            auto length = co_await async_read_value<sophiar_endian, header_type>(s);
+            buf->resize(length);
+            co_await async_fill_memory_from(s, *buf);
+
+            // parse and update variables
+            auto ts = current_timestamp();
+            auto reader = reader_type{*buf};
+            while (!reader.empty()) {
+                auto var_index = reader.read_value<variable_index_type>();
+                auto val_size = reader.read_value<variable_size_type>();
+                auto cur_data = buf->data() + reader.current_offset();
+                reader.manual_offset(val_size);
+                auto val_buf = const_extern_memory{cur_data, val_size};
+                auto val_reader = reader_type{val_buf};
+
+                assert(info_pool.contains(var_index));
+                auto &info = info_pool[var_index];
+                info.reader_func(val_reader, info.placeholder);
+                update_variable_timestamp(var_index, ts);
+            }
+
+            co_return true;
+        }
+
+    };
+
+    struct external_variable_io::impl::binary_out_client {
+
+        using writer_type = dynamic_memory_writer<sophiar_endian>;
+        using writer_func_type = void (*)(writer_type &, void *);
+
+        struct variable_io_info {
+            variable_index_type var_index;
+            void *placeholder;
+            writer_func_type writer_func;
+            timestamp_type last_update_ts;
+        };
+
+        using info_pool_type = dynamic_vector<variable_io_info>;
+
+        tcp::socket s;
+        info_pool_type info_pool;
+        dynamic_memory::pointer buf;
+
+        coro_signal_any_group::pointer signal_group;
+        signal_watcher watcher;
+
+        bool is_first_run = true;
+
+        binary_out_client(tcp::socket &&_s, const variable_info_list &infos)
+                : s(std::move(_s)),
+                  buf(dynamic_memory::new_instance()),
+                  signal_group(coro_signal_any_group::new_instance()),
+                  watcher(signal_group->new_watcher()) {
+            for (auto &info: infos) {
+                signal_group->add_watcher(REQUIRE_VARIABLE_WATCHER(info.var_index));
+                info_pool.push_back(
+                        {
+                                .var_index = info.var_index,
+                                .placeholder = info.placeholder,
+                                .writer_func = require_raw_pointer_writer<writer_type>(info.type),
+                                .last_update_ts = 0
+                        });
+            }
+            signal_group->start();
+        }
+
+        awaitable<bool> work_once() {
+            if (!is_first_run) {
+                co_await watcher.coro_wait(false);
+            }
+
+            auto writer = writer_type{buf.get(), header_offset};
+            for (auto &info: info_pool) {
+                auto cur_update_ts = QUERY_VARIABLE_TS(info.var_index);
+                if (!is_first_run
+                    && cur_update_ts <= info.last_update_ts) {
+                    continue;
+                }
+                info.last_update_ts = cur_update_ts;
+
+                // write variable value
+                writer << info.var_index;
+                auto size_offset = buf->size();
+                buf->increase_size(variable_size_type_offset);
+                info.writer_func(writer, info.placeholder);
+                auto length = buf->size() - size_offset - variable_size_type_offset;
+                assert(length <= std::numeric_limits<variable_size_type>::max());
+                write_binary_value<sophiar_endian>(buf->data() + size_offset,
+                                                   (variable_size_type) length);
+            }
+
+            // fill message size
+            auto length = buf->size() - header_offset;
+            assert(length <= std::numeric_limits<header_type>::max());
+            write_binary_value<sophiar_endian>(buf->data(), (header_type) length);
+
+            is_first_run = false;
+            watcher.sync(); // manual sync to prevent duplicated notification
+
+            co_await async_write_memory_to(s, *buf);
+            co_return true;
+        }
+
+    };
+
+    awaitable<void> external_variable_io::impl::create_client_worker(tcp::socket &&_s) {
+        // receive command
+        tcp::socket s = std::move(_s); // 避免这玩意被莫名其妙的析构掉
+        auto length = co_await async_read_value<sophiar_endian, header_type>(s);
+        auto buf_in = dynamic_memory::new_instance(length);
+        co_await async_fill_memory_from(s, *buf_in);
+
+        // load command and params
+        auto reader = versatile_reader<sophiar_endian>(*buf_in);
+        auto params = dynamic_vector<std::string_view>{};
+        auto cmd = reader.read_string_until(' ');
+        params.clear();
+        while (!reader.empty()) {
+            params.push_back(reader.read_string_until(','));
+        }
+
+        // send initial reply
+        auto writer = string_writer{","};
+        variable_info_list infos;
+        for (auto var_name: params) {
+            auto info = query_variable_information(var_name);
+            writer << info.var_index;
+            infos.push_back(info);
+        }
+        auto buf_str = writer.get_string_and_reset();
+        auto buf_out = dynamic_memory::new_instance(buf_str.length() + header_offset);
+        auto buf_writer = versatile_writer<sophiar_endian>(*buf_out);
+        assert(buf_str.length() <= std::numeric_limits<header_type>::max());
+        buf_writer << (header_type) buf_str.length() << buf_str;
+        assert(buf_writer.remaining_bytes() == 0);
+        co_await async_write_memory_to(s, *buf_out);
+
+        // create worker
+        auto re = s.remote_endpoint();
+        auto error_handler = [=](std::exception &e) {
+            SPDLOG_WARN("Client {}:{} left: {}",
+                        re.address().to_string(), re.port(), e.what());
+        };
+
+        auto worker_ptr_ptr = new coro_worker *;
+        coro_worker::pointer worker;
+        auto exit_func = [=]() {
+            assert(worker_ptr_ptr != nullptr);
+            boost::asio::defer(*global_context, [=]() {
+                delete *worker_ptr_ptr;
+                delete worker_ptr_ptr;
+            });
+        };
+
+        if (cmd == "VARINB") {
+            auto worker_func = [
+                    client = binary_in_client(std::move(s), infos)]() mutable
+                    -> awaitable<bool> {
+                return client.work_once();
+            };
+            auto noexcept_worker_func = make_noexcept_func(
+                    std::move(worker_func), std::move(error_handler));
+            worker = make_infinite_coro_worker(std::move(noexcept_worker_func),
+                                               std::move(exit_func));
+        } else if (cmd == "VAROUTB") {
+            auto client = binary_out_client(std::move(s), infos);
+            auto exit_func_ext = [
+                    old_exit_func = std::move(exit_func),
+                    signal_group = std::move(client.signal_group)]() mutable {
+                co_spawn(*global_context, [ // binary_out_client 的非平凡的析构函数会导致编译错误
+                        signal_group = std::move(signal_group)]() -> awaitable<void> {
+                    co_await signal_group->stop();
+                    co_return;
+                }, detached);
+                old_exit_func();
+            };
+            auto worker_func = [
+                    client = std::move(client)]() mutable
+                    -> awaitable<bool> {
+                return client.work_once();
+            };
+            auto noexcept_worker_func = make_noexcept_func(
+                    std::move(worker_func), std::move(error_handler));
+            worker = make_infinite_coro_worker(std::move(noexcept_worker_func),
+                                               std::move(exit_func_ext));
+        } else {
+            assert(false);
+            co_return;
+        }
+
+        worker->run();
+        SPDLOG_INFO("Working with client {}:{}",
+                    re.address().to_string(), re.port());
+        *worker_ptr_ptr = worker.release();
+
+        co_return;
+    }
+
+}

src/core/external_variable_io.h

@@ -0,0 +1,27 @@
+#ifndef SOPHIAR2_EXTERNAL_VARIABLE_IO_H
+#define SOPHIAR2_EXTERNAL_VARIABLE_IO_H
+
+#include <nlohmann/json.hpp>
+
+#include <memory>
+
+namespace sophiar {
+
+    class external_variable_io {
+    public:
+
+        external_variable_io();
+
+        ~external_variable_io();
+
+        bool load_config_and_start(const nlohmann::json &config);
+
+    private:
+        struct impl;
+        std::unique_ptr<impl> pimpl;
+
+    };
+
+}
+
+#endif //SOPHIAR2_EXTERNAL_VARIABLE_IO_H

src/core/global_defs.cpp

@@ -1,6 +1,7 @@
 #include "global_defs.h"
 #include "algorithm/transform_tree.h"
 #include "core/external_controller.h"
+#include "core/external_variable_io.h"
 #include "core/sophiar_manager.h"
 #include "core/sophiar_pool.h"
 #include "core/timestamp_helper.hpp"
@@ -23,6 +24,7 @@ namespace sophiar {
     sophiar_manager *global_sophiar_manager = nullptr;
     sophiar_pool *global_sophiar_pool = nullptr;
     external_controller *global_external_controller = nullptr;
+    external_variable_io *global_external_variable_io = nullptr;
 
     local_time_type program_start_time;
 
@@ -85,6 +87,7 @@ namespace sophiar {
         global_sophiar_pool = new sophiar_pool{};
         global_sophiar_manager = new sophiar_manager{};
         global_external_controller = new external_controller{};
+        global_external_variable_io = new external_variable_io{};
 
         register_object_types();
 
@@ -101,6 +104,7 @@ namespace sophiar {
         global_sophiar_pool->load_config(config);
         global_sophiar_manager->load_config_and_start(config);
         ENSURE(global_external_controller->load_config_and_start(config))
+        ENSURE(global_external_variable_io->load_config_and_start(config))
 
         return true;
     }

src/core/global_defs.h

@@ -39,6 +39,9 @@ namespace sophiar {
 #define QUERY_VARIABLE_WITH_TS(var_type, var_index, ts_ptr) \
     global_sophiar_pool->query_variable<var_type>(var_index, ts_ptr)
 
+#define QUERY_VARIABLE_TS(var_index) \
+    global_sophiar_pool->query_variable_update_ts(var_index)
+
 #define REQUIRE_VARIABLE_WATCHER(var_index) \
     global_sophiar_pool->require_variable_watcher(var_index)
 

src/core/sophiar_manager.h

@@ -11,7 +11,6 @@
 
 #include <nlohmann/json.hpp>
 
-#include <cassert>
 #include <coroutine>
 #include <exception>
 #include <memory>
@@ -20,6 +19,8 @@
 #include <typeindex>
 #include <typeinfo>
 
+#include "utility/assert_utility.h"
+
 namespace sophiar {
 
     class sophiar_obj;

src/core/sophiar_pool.cpp

@@ -5,10 +5,11 @@
 #include "utility/string_map.hpp"
 #include "utility/versatile_buffer2.hpp"
 
-#include <cassert>
 #include <vector>
 #include <unordered_map>
 
+#include "utility/assert_utility.h"
+
 namespace sophiar {
 
     struct sophiar_pool::impl {
@@ -105,9 +106,13 @@ namespace sophiar {
 
             using writer_big_extern = versatile_writer<boost::endian::order::big>;
             using writer_little_extern = versatile_writer<boost::endian::order::little>;
+            using writer_big_dynamic = dynamic_memory_writer<boost::endian::order::big>;
+            using writer_little_dynamic = dynamic_memory_writer<boost::endian::order::little>;
 
             REGISTER_WRITER_FUNC(writer_big_extern)
             REGISTER_WRITER_FUNC(writer_little_extern)
+            REGISTER_WRITER_FUNC(writer_big_dynamic)
+            REGISTER_WRITER_FUNC(writer_little_dynamic)
 #undef REGISTER_WRITER_FUNC
 
 #define REGISTER_READER_FUNC(reader_type) { \
@@ -196,14 +201,20 @@ namespace sophiar {
         return pimpl->variable_pool.to_name_by_index(var_index);
     }
 
+    timestamp_type sophiar_pool::query_variable_update_ts(variable_index_type var_index) {
+        assert(pimpl->variable_pool.contains(var_index));
+        return pimpl->variable_pool[var_index].last_update_ts;
+    }
+
     sophiar_pool::variable_info sophiar_pool::query_variable_information(std::string_view var_name) {
         assert(pimpl->variable_pool.contains(var_name));
         const auto &info = pimpl->variable_pool[var_name];
-        variable_info ret;
-        ret.type = info.type_info->type;
-        ret.last_update_ts = info.last_update_ts;
-        ret.binary_length = info.type_info->binary_length;
-        ret.type_name = info.type_info->type_name;
+        variable_info ret{
+                .var_index = pimpl->variable_pool.to_index_by_name(var_name),
+                .type = info.type_info->type,
+                .binary_length = info.type_info->binary_length,
+                .placeholder = info.placeholder
+        };
         return ret;
     }
 
@@ -211,10 +222,7 @@ namespace sophiar {
                                                             std::type_index var_type) {
         assert(pimpl->variable_pool.contains(var_name));
         const auto &info = pimpl->variable_pool[var_name];
-        if (info.type_info->type != var_type) { // ensure type consistency
-            assert(false);
-            return invalid_variable_index;
-        }
+        assert(info.type_info->type == var_type);
         return pimpl->variable_pool.to_index_by_name(var_name);
     }
 

src/core/sophiar_pool.h

@@ -53,17 +53,10 @@ namespace sophiar {
 
         signal_watcher require_variable_watcher(variable_index_type var_index);
 
-        struct variable_info {
-            std::type_index type = typeid(void);
-            timestamp_type last_update_ts;
-            size_t binary_length;
-            std::string type_name;
-        };
-
-        variable_info query_variable_information(std::string_view var_name);
-
         std::string query_variable_name(variable_index_type var_index);
 
+        timestamp_type query_variable_update_ts(variable_index_type var_index);
+
 #ifdef SOPHIAR_TEST
 
     public:
@@ -89,6 +82,15 @@ namespace sophiar {
                                             timestamp_type ts);
 
         // for friend classes
+        struct variable_info {
+            variable_index_type var_index;
+            std::type_index type;
+            size_t binary_length;
+            void *placeholder;
+        };
+
+        variable_info query_variable_information(std::string_view var_name);
+
         template<typename WriterType>
         auto require_raw_pointer_writer(std::type_index var_type) {
             using RealWriterType = std::remove_cvref_t<WriterType>;
@@ -102,7 +104,7 @@ namespace sophiar {
         auto require_raw_pointer_reader(std::type_index var_type) {
             using RealReaderType = std::remove_cvref_t<ReaderType>;
             using ReaderFuncType = void (*)(RealReaderType &, void *);
-            auto func_ptr = require_raw_pointer_writer_impl(typeid(RealReaderType), var_type);
+            auto func_ptr = require_raw_pointer_reader_impl(typeid(RealReaderType), var_type);
             assert(func_ptr != nullptr);
             return (ReaderFuncType) func_ptr;
         }
@@ -113,9 +115,7 @@ namespace sophiar {
         void *require_raw_pointer_reader_impl(std::type_index reader_type,
                                               std::type_index var_type);
 
-        friend class variable_watcher;
-
-        friend class variable_updater;
+        friend class external_variable_io;
 
     };
 

src/core/tristate_obj.cpp

@@ -8,7 +8,7 @@
 
 #include <boost/asio/experimental/awaitable_operators.hpp>
 
-#include <cassert>
+#include "utility/assert_utility.h"
 
 namespace sophiar {
 

src/robot/ur/ur_interface.cpp

@@ -23,12 +23,13 @@
 #include <spdlog/spdlog.h>
 
 #include <algorithm>
-#include <cassert>
 #include <exception>
 #include <string>
 #include <string_view>
 #include <vector>
 
+#include "utility/assert_utility.h"
+
 namespace sophiar {
 
     using boost::asio::async_read;
@@ -385,8 +386,7 @@ namespace sophiar {
 
         template<typename ContentType>
         awaitable<void> send_rtde_packet(const ContentType &content) {
-            PREVENT_TWICE_ENTER
-            static auto buf = dynamic_memory::new_instance();
+            auto buf = dynamic_memory::new_instance(sizeof(content) + 64); // 减少内存复制次数
             auto writer = dynamic_memory_writer<ur_endian>(buf.get(), packet_size_offset);
             writer << (uint8_t) content.packet_type;
             content.write_to(writer);
@@ -398,7 +398,6 @@ namespace sophiar {
         }
 
         awaitable<dynamic_memory::pointer> receive_rtde_packet(rtde_packet_header &header) {
-            PREVENT_TWICE_ENTER
             auto header_buf = static_memory<packet_header_offset>{};
             assert(ur_socket->is_open());
             co_await async_fill_memory_from(*ur_socket, header_buf);
@@ -414,7 +413,6 @@ namespace sophiar {
 
         template<typename ContentType>
         awaitable<void> receive_rtde_packet(ContentType &content) {
-            PREVENT_TWICE_ENTER
             auto header = rtde_packet_header{};
             auto content_buf = co_await receive_rtde_packet(header);
             assert(header.packet_type == content.packet_type);
@@ -490,8 +488,8 @@ namespace sophiar {
 
         awaitable<void> install_program() {
             // create no motion command
-            static auto no_motion_content = ur_command_content{};
-            static auto &no_motion_data = no_motion_content.content;
+            auto no_motion_content = ur_command_content{};
+            auto &no_motion_data = no_motion_content.content;
             no_motion_content.recipe_id = inputs_recipe_id;
             no_motion_data.move_type = (int32_t) control_mode_type::NO_CONTROL;
             no_motion_data.cmd_id = current_cmd_id++;
@@ -527,9 +525,8 @@ namespace sophiar {
         }
 
         void handle_data_package(income_reader_type &reader) {
-            PREVENT_TWICE_ENTER
-            static auto content = ur_status_content{};
-            static const auto &status = content.content;
+            auto content = ur_status_content{};
+            const auto &status = content.content;
             content.fill_from(reader);
             assert(content.recipe_id == outputs_recipe_id);
 

src/third_party/debugbreak.h

@@ -0,0 +1,172 @@
+/* Copyright (c) 2011-2021, Scott Tsai
+ *
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice,
+ *    this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright notice,
+ *    this list of conditions and the following disclaimer in the documentation
+ *    and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+#ifndef DEBUG_BREAK_H
+#define DEBUG_BREAK_H
+
+#ifdef _MSC_VER
+
+#define debug_break __debugbreak
+
+#else
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#define DEBUG_BREAK_USE_TRAP_INSTRUCTION 1
+#define DEBUG_BREAK_USE_BULTIN_TRAP      2
+#define DEBUG_BREAK_USE_SIGTRAP          3
+
+#if defined(__i386__) || defined(__x86_64__)
+#define DEBUG_BREAK_IMPL DEBUG_BREAK_USE_TRAP_INSTRUCTION
+__inline__ static void trap_instruction(void) {
+    __asm__ volatile("int $0x03");
+}
+#elif defined(__thumb__)
+#define DEBUG_BREAK_IMPL DEBUG_BREAK_USE_TRAP_INSTRUCTION
+/* FIXME: handle __THUMB_INTERWORK__ */
+__attribute__((always_inline))
+__inline__ static void trap_instruction(void)
+{
+    /* See 'arm-linux-tdep.c' in GDB source.
+     * Both instruction sequences below work. */
+#if 1
+    /* 'eabi_linux_thumb_le_breakpoint' */
+    __asm__ volatile(".inst 0xde01");
+#else
+    /* 'eabi_linux_thumb2_le_breakpoint' */
+    __asm__ volatile(".inst.w 0xf7f0a000");
+#endif
+
+    /* Known problem:
+     * After a breakpoint hit, can't 'stepi', 'step', or 'continue' in GDB.
+     * 'step' would keep getting stuck on the same instruction.
+     *
+     * Workaround: use the new GDB commands 'debugbreak-step' and
+     * 'debugbreak-continue' that become available
+     * after you source the script from GDB:
+     *
+     * $ gdb -x debugbreak-gdb.py <... USUAL ARGUMENTS ...>
+     *
+     * 'debugbreak-step' would jump over the breakpoint instruction with
+     * roughly equivalent of:
+     * (gdb) set $instruction_len = 2
+     * (gdb) tbreak *($pc + $instruction_len)
+     * (gdb) jump   *($pc + $instruction_len)
+     */
+}
+#elif defined(__arm__) && !defined(__thumb__)
+#define DEBUG_BREAK_IMPL DEBUG_BREAK_USE_TRAP_INSTRUCTION
+__attribute__((always_inline))
+__inline__ static void trap_instruction(void)
+{
+    /* See 'arm-linux-tdep.c' in GDB source,
+     * 'eabi_linux_arm_le_breakpoint' */
+    __asm__ volatile(".inst 0xe7f001f0");
+    /* Known problem:
+     * Same problem and workaround as Thumb mode */
+}
+#elif defined(__aarch64__) && defined(__APPLE__)
+#define DEBUG_BREAK_IMPL DEBUG_BREAK_USE_BULTIN_DEBUGTRAP
+#elif defined(__aarch64__)
+#define DEBUG_BREAK_IMPL DEBUG_BREAK_USE_TRAP_INSTRUCTION
+__attribute__((always_inline))
+__inline__ static void trap_instruction(void)
+{
+    /* See 'aarch64-tdep.c' in GDB source,
+     * 'aarch64_default_breakpoint' */
+    __asm__ volatile(".inst 0xd4200000");
+}
+#elif defined(__powerpc__)
+    /* PPC 32 or 64-bit, big or little endian */
+#define DEBUG_BREAK_IMPL DEBUG_BREAK_USE_TRAP_INSTRUCTION
+__attribute__((always_inline))
+__inline__ static void trap_instruction(void)
+{
+    /* See 'rs6000-tdep.c' in GDB source,
+     * 'rs6000_breakpoint' */
+    __asm__ volatile(".4byte 0x7d821008");
+
+    /* Known problem:
+     * After a breakpoint hit, can't 'stepi', 'step', or 'continue' in GDB.
+     * 'step' stuck on the same instruction ("twge r2,r2").
+     *
+     * The workaround is the same as ARM Thumb mode: use debugbreak-gdb.py
+     * or manually jump over the instruction. */
+}
+#elif defined(__riscv)
+    /* RISC-V 32 or 64-bit, whether the "C" extension
+     * for compressed, 16-bit instructions are supported or not */
+#define DEBUG_BREAK_IMPL DEBUG_BREAK_USE_TRAP_INSTRUCTION
+__attribute__((always_inline))
+__inline__ static void trap_instruction(void)
+{
+    /* See 'riscv-tdep.c' in GDB source,
+     * 'riscv_sw_breakpoint_from_kind' */
+    __asm__ volatile(".4byte 0x00100073");
+}
+#else
+#define DEBUG_BREAK_IMPL DEBUG_BREAK_USE_SIGTRAP
+#endif
+
+
+#ifndef DEBUG_BREAK_IMPL
+#error "debugbreak.h is not supported on this target"
+#elif DEBUG_BREAK_IMPL == DEBUG_BREAK_USE_TRAP_INSTRUCTION
+__attribute__((always_inline))
+__inline__ static void debug_break(void) {
+    trap_instruction();
+}
+#elif DEBUG_BREAK_IMPL == DEBUG_BREAK_USE_BULTIN_DEBUGTRAP
+__attribute__((always_inline))
+__inline__ static void debug_break(void)
+{
+    __builtin_debugtrap();
+}
+#elif DEBUG_BREAK_IMPL == DEBUG_BREAK_USE_BULTIN_TRAP
+__attribute__((always_inline))
+__inline__ static void debug_break(void)
+{
+    __builtin_trap();
+}
+#elif DEBUG_BREAK_IMPL == DEBUG_BREAK_USE_SIGTRAP
+#include <signal.h>
+__attribute__((always_inline))
+__inline__ static void debug_break(void)
+{
+    raise(SIGTRAP);
+}
+#else
+#error "invalid DEBUG_BREAK_IMPL value"
+#endif
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif /* ifdef _MSC_VER */
+
+#endif /* ifndef DEBUG_BREAK_H */

src/utility/assert_utility.h

@@ -0,0 +1,20 @@
+#include <cassert>
+#include "third_party/debugbreak.h"
+
+#undef assert
+
+#ifdef NDEBUG
+#define assert(_Expression) ((void)0)
+#else /* !defined (NDEBUG) */
+#if defined(_UNICODE) || defined(UNICODE)
+#define assert(_Expression) \
+ (void) \
+ ((!!(_Expression)) || \
+  (debug_break(),_wassert(_CRT_WIDE(#_Expression),_CRT_WIDE(__FILE__),__LINE__),0))
+#else /* not unicode */
+#define assert(_Expression) \
+ (void) \
+ ((!!(_Expression)) || \
+  (debug_break(),_assert(#_Expression,__FILE__,__LINE__),0))
+#endif /* _UNICODE||UNICODE */
+#endif /* !defined (NDEBUG) */

src/utility/coro_signal2.hpp

@@ -8,8 +8,6 @@
 #include <boost/asio/use_awaitable.hpp>
 #include <boost/core/noncopyable.hpp>
 
-#include <cassert>
-
 #ifdef CORO_SIGNAL2_USE_TIMER // timer based implementation
 
 #include <boost/asio/deadline_timer.hpp>
@@ -24,6 +22,8 @@
 
 #endif
 
+#include "utility/assert_utility.h"
+
 namespace sophiar {
 
     class coro_signal2;

src/utility/coro_signal_group.hpp

@@ -39,6 +39,7 @@ namespace sophiar {
         }
 
         void start(bool auto_sync = true) { // 思考如果 auto_sync == false 有什么副作用?
+            assert(!is_running);
             item_mask.resize(watcher_list.size());
             is_running = true;
             for (size_t index = 0; index < watcher_list.size(); ++index) {

src/utility/debug_utility.hpp

@@ -8,25 +8,42 @@
 #include <boost/asio/high_resolution_timer.hpp>
 #include <boost/asio/this_coro.hpp>
 #include <boost/asio/use_awaitable.hpp>
+#include <boost/core/demangle.hpp>
 
 #include <spdlog/spdlog.h>
 
-#include <cassert>
 #include <iostream>
 
-#define FILE_LINE_TRACE {\
-    std::cout << fmt::format("F:{} L:{} T:{}", \
-                             __FILE_NAME__, __LINE__, sophiar::current_timestamp()) \
-              << std::endl; }
+#include "utility/assert_utility.h"
 
-#define FILE_LINE_TRACE_WITH_THIS {\
-    std::cout << fmt::format("F:{} L:{} T:{} TH:{}", \
-                             __FILE_NAME__, __LINE__, sophiar::current_timestamp(), (void *) this) \
-              << std::endl; }
+#define DEBUG_PRINT(msg) \
+    std::cout << fmt::format("F:{} L:{} {}", \
+                             __FILE_NAME__, __LINE__, msg) \
+              << std::endl
 
-#define PRINT_PTR(obj) {\
-    std::cout << fmt::format("F:{} L:{} PTR: {}", __FILE_NAME__, __LINE__, \
-                             (void*)std::addressof(obj)) << std::endl; }
+#define FILE_LINE_TRACE \
+    { \
+        auto __msg = fmt::format("T:{}", current_timestamp()); \
+        DEBUG_PRINT(__msg); \
+    }
+
+#define FILE_LINE_TRACE_WITH_THIS \
+    { \
+        auto __msg = fmt::format("T:{} TH:{}", current_timestamp(), (void *) this); \
+        DEBUG_PRINT(__msg); \
+    }
+
+#define DEBUG_PRINT_ADDRESS(obj) \
+    { \
+        auto __msg = fmt::format("PTR:{}", (void *) std::addressof(obj)); \
+        DEBUG_PRINT(__msg); \
+    }
+
+#define DEBUG_PRINT_TYPE(type) \
+    { \
+        auto __msg = fmt::format("TYPE:{}", boost::core::demangle(typeid(type).name())); \
+        DEBUG_PRINT(__msg); \
+    }
 
 #define ENSURE(func) \
     { \
@@ -48,20 +65,6 @@
     if (__is_called) [[likely]] return; \
     __is_called = true;
 
-#ifdef NDEBUG
-
-#define PREVENT_TWICE_ENTER
-
-#else // NDEBUG
-
-#define PREVENT_TWICE_ENTER \
-    static bool __running = false; \
-    assert(!__running); \
-    __running = true; \
-    auto __closer = sg::make_scope_guard([&] { __running = false; });
-
-#endif // NDEBUG
-
 namespace sophiar {
 
     using boost::asio::awaitable;

src/utility/dynamic_pool.hpp

@@ -30,13 +30,13 @@ namespace sophiar {
 
         void *allocate(size_t s, size_t n = 1) {
             assert(n >= 1);
-            auto index = determine_index(n == 1 ? s : s * n);
+            auto index = determine_index(s * n);
             return pools[index]->malloc();
         }
 
         void deallocate(void *p, size_t s, size_t n = 1) {
             assert(n >= 1);
-            auto index = determine_index(n == 1 ? s : s * n);
+            auto index = determine_index(s * n);
             pools[index]->free(p);
         }
 

src/utility/name_translator.hpp

@@ -3,10 +3,11 @@
 
 #include "utility/string_map.hpp"
 
-#include <cassert>
 #include <string>
 #include <unordered_map>
 
+#include "utility/assert_utility.h"
+
 namespace sophiar {
 
     template<typename IndexType>

src/utility/named_vector.hpp

@@ -3,10 +3,11 @@
 
 #include "utility/string_map.hpp"
 
-#include <cassert>
 #include <climits>
 #include <vector>
 
+#include "utility/assert_utility.h"
+
 namespace sophiar {
 
     template<typename IndexType, typename ElemType = bool>

src/utility/string_map.hpp

@@ -1,12 +1,13 @@
 #ifndef SOPHIAR2_STRING_MAP_HPP
 #define SOPHIAR2_STRING_MAP_HPP
 
-#include <cassert>
 #include <string>
 #include <string_view>
 #include <type_traits>
 #include <unordered_map>
 
+#include "utility/assert_utility.h"
+
 namespace sophiar {
 
     template<typename T>

src/utility/variable_utility.hpp

@@ -6,6 +6,7 @@
 #include "utility/config_utility.hpp"
 #include "utility/simple_tristate_obj.hpp"
 #include "utility/variable_helper.hpp"
+#include "utility/versatile_buffer2.hpp"
 
 #include <boost/asio/awaitable.hpp>
 
@@ -20,74 +21,6 @@ namespace sophiar {
 
     using boost::asio::awaitable;
 
-    class string_writer {
-    public:
-
-        explicit string_writer(std::string_view _sep = ", ")
-                : sep(_sep) {}
-
-        template<typename T>
-        string_writer &operator<<(const T &val) {
-            if (!is_empty) {
-                ss << sep;
-            }
-            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;
-        std::string sep;
-        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>
-        T read_value() {
-            T val;
-            *this >> val;
-            return val;
-        }
-
-        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;
-    };
-
     template<typename SmallObjType>
     inline std::string variable_to_string(const SmallObjType &var) {
         auto writer = string_writer{};
@@ -102,7 +35,7 @@ namespace sophiar {
         auto worker = make_infinite_coro_worker(
                 [=,
                         buffer = string_writer(),
-                        var_helper = VARIABLE_AUTO_DELEGATE(SmallObjType, var_index)]() mutable
+                        var_helper = VARIABLE_MANUAL_DELEGATE(SmallObjType, var_index)]() mutable
                         -> awaitable<bool> {
                     co_await var_helper.coro_wait_update();
                     if (var_helper.empty()) {

src/utility/versatile_buffer2.hpp

@@ -13,13 +13,14 @@
 #include <Eigen/Core>
 
 #include <algorithm>
-#include <cassert>
 #include <concepts>
 #include <span>
 #include <string>
 #include <string_view>
 #include <type_traits>
 
+#include "utility/assert_utility.h"
+
 namespace sophiar {
 
     template<size_t Length>
@@ -42,7 +43,7 @@ namespace sophiar {
 
     struct dynamic_memory : private boost::noncopyable {
 
-        static constexpr size_t DYNAMIC_MEMORY_DEFAULT_SIZE = 32;
+        static constexpr size_t DYNAMIC_MEMORY_DEFAULT_SIZE = 128;
 
         using pointer = std::unique_ptr<dynamic_memory>;
 
@@ -67,9 +68,10 @@ namespace sophiar {
             used_length = req_size;
         }
 
-        void increase_size(ptrdiff_t extra_size) {
+        char *increase_size(ptrdiff_t extra_size) { // 返回新增内存的起始地址
             assert(extra_size >= 0 || size() > -extra_size);
             resize(size() + extra_size);
+            return data() + size() - extra_size;
         }
 
         static auto new_instance(size_t req_size = DYNAMIC_MEMORY_DEFAULT_SIZE) {
@@ -314,15 +316,13 @@ namespace sophiar {
         template<typename T>
         std::enable_if_t<std::is_arithmetic_v<T>>
         write_value(T val) {
-            buf->increase_size(sizeof(T));
             swap_net_loc_endian<net_order>(val);
-            auto ptr = start_ptr(sizeof(T));
+            auto ptr = buf->increase_size(sizeof(T));
             memcpy(ptr, &val, sizeof(T));
         }
 
         void write_value(std::string_view str) {
-            buf->increase_size(str.length());
-            auto ptr = start_ptr(str.length());
+            auto ptr = buf->increase_size(str.length());
             std::copy(str.begin(), str.end(), ptr);
         }
 
@@ -348,11 +348,74 @@ namespace sophiar {
 
     private:
         dynamic_memory *buf;
+    };
+
+    class string_writer {
+    public:
+
+        explicit string_writer(std::string_view _sep = ", ")
+                : sep(_sep) {}
+
+        template<typename T>
+        string_writer &operator<<(const T &val) {
+            if (!is_empty) {
+                ss << sep;
+            }
+            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;
+        std::string sep;
+        bool is_empty = true;
+    };
 
-        char *start_ptr(size_t offset) { // 从这个位置开始写入数据
-            assert(buf->size() >= offset);
-            return buf->data() + buf->size() - offset;
+    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>
+        T read_value() {
+            T val;
+            *this >> val;
+            return val;
+        }
+
+        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;
     };
 
     template<boost::endian::order net_order,

tests/core/sophiar_pool.cpp

@@ -6,12 +6,16 @@
 
 #include <boost/test/unit_test.hpp>
 
+#include <spdlog/spdlog.h>
+
 #include <fstream>
 
 using namespace sophiar;
 
 BOOST_AUTO_TEST_CASE(test_sophiar_pool) {
 
+    spdlog::set_level(spdlog::level::trace);
+
     std::ifstream config_file("data/sophiar_pool_config.json");
     BOOST_TEST(config_file.is_open());
     auto config = nlohmann::json::parse(config_file);
@@ -60,4 +64,5 @@ BOOST_AUTO_TEST_CASE(test_sophiar_pool) {
         BOOST_TEST(var_ptr->value[0] == 6);
     }
 
+    global_context->run();
 }

tests/data/sophiar_pool_config.json

@@ -51,5 +51,44 @@
       ]
     }
   ],
-  "object_list": []
+  "object_list": [
+    {
+      "type": "bool_obj_watcher",
+      "name": "var_bool_watcher",
+      "start_config": {
+        "variable_name": "var_bool"
+      }
+    },
+    {
+      "type": "u64int_obj_watcher",
+      "name": "var_num_watcher",
+      "start_config": {
+        "variable_name": "var_num"
+      }
+    },
+    {
+      "type": "double_obj_watcher",
+      "name": "var_float_watcher",
+      "start_config": {
+        "variable_name": "var_float"
+      }
+    },
+    {
+      "type": "scalarxyz_obj_watcher",
+      "name": "var_vec_watcher",
+      "start_config": {
+        "variable_name": "var_vec"
+      }
+    },
+    {
+      "type": "empty_object",
+      "name": "all",
+      "dependencies": [
+        "var_bool_watcher",
+        "var_num_watcher",
+        "var_float_watcher",
+        "var_vec_watcher"
+      ]
+    }
+  ]
 }