暂时搁置 NDI 接口的开发

src/core/small_obj.hpp

@@ -48,6 +48,13 @@ namespace sophiar {
     template<typename DeriveT>
     typename small_obj<DeriveT>::allocator_type small_obj<DeriveT>::allocator{};
 
+#define FORWARD_CONSTRUCT(DerivedT, BaseT) \
+template<typename... Args> \
+    explicit DerivedT(Args &&...args) \
+    :BaseT(std::forward<Args>(args)...) { \
+    static_assert(std::is_constructible_v<BaseT, Args...>); \
+}
+
 }
 
 #endif //SOPHIAR2_SMALL_OBJ_H

src/robot/ur/ur_interface.cpp

@@ -2,11 +2,13 @@
 
 #include "utility/debug_utility.hpp"
 #include "utility/coro_signal.hpp"
+#include "utility/tiny_signal.hpp"
 #include "utility/versatile_buffer.hpp"
 
 #include <boost/algorithm/string/join.hpp>
 #include <boost/asio/experimental/awaitable_operators.hpp>
 #include <boost/asio/experimental/channel.hpp>
+#include <boost/asio/ip/address.hpp>
 #include <boost/asio/ip/tcp.hpp>
 #include <boost/asio/co_spawn.hpp>
 #include <boost/asio/detached.hpp>
@@ -288,6 +290,7 @@ namespace sophiar {
         double report_frequency = 125;
         uint8_t inputs_recipe_id = 0, outputs_recipe_id = 0;
 
+        using ip_address_type = boost::asio::ip::address;
         ip_address_type ur_ip;
         scoped_ptr<tcp::socket> ur_socket; // https://stackoverflow.com/questions/3062803/
 
@@ -534,6 +537,7 @@ namespace sophiar {
             co_return;
         }
 
+        // TODO 这个写法不对, 开一个专门的协程开来控制这玩意
         awaitable<void> handle_command_request() {
             versatile_buffer buffer;
             auto stop_token = stop_requested_signal.new_token();
@@ -662,7 +666,7 @@ namespace sophiar {
         pimpl->q_this = this;
     }
 
-    bool ur_interface::load_construct_config(const nlohmann::json &config) {
+    void ur_interface::load_construct_config(const nlohmann::json &config) {
         assert(config.empty());
         pimpl->register_signal_slot();
     }

src/robot/ur/ur_interface.h

@@ -4,12 +4,6 @@
 #include "core/small_obj.hpp"
 #include "core/tristate_obj.h"
 #include "core/types/versatile_data.hpp"
-#include "utility/signal_muxer.hpp"
-#include "utility/tiny_signal.hpp"
-
-#include <Eigen/Core>
-
-#include <boost/asio/ip/address.hpp>
 
 #include <cassert>
 #include <memory>
@@ -174,13 +168,11 @@ namespace sophiar {
     class ur_interface : public tristate_obj {
     public:
 
-        using ip_address_type = boost::asio::ip::address;
-
         ur_interface();
 
         ~ur_interface() override;
 
-        bool load_construct_config(const nlohmann::json &config) override;
+        void load_construct_config(const nlohmann::json &config) override;
 
     protected:
 

src/tracker/ndi/ndi_interface.cpp

@@ -0,0 +1,290 @@
+#include "ndi_interface.h"
+
+#include "utility/debug_utility.hpp"
+#include "utility/coro_signal.hpp"
+#include "utility/name_translator.hpp"
+#include "utility/tiny_signal.hpp"
+#include "utility/versatile_buffer2.hpp"
+#include "third_party/static_block.hpp"
+
+#include <boost/asio/experimental/awaitable_operators.hpp>
+#include <boost/asio/experimental/channel.hpp>
+#include <boost/asio/ip/address.hpp>
+#include <boost/asio/ip/tcp.hpp>
+#include <boost/asio/co_spawn.hpp>
+#include <boost/asio/detached.hpp>
+#include <boost/asio/serial_port.hpp>
+#include <boost/asio/use_awaitable.hpp>
+#include <boost/crc.hpp>
+#include <boost/iostreams/device/mapped_file.hpp>
+#include <boost/smart_ptr/scoped_ptr.hpp>
+#include <boost/system/error_code.hpp>
+
+#include <spdlog/spdlog.h>
+
+#include <cassert>
+
+namespace sophiar {
+
+    using boost::asio::async_read;
+    using boost::asio::async_write;
+    using boost::asio::awaitable;
+    using boost::asio::buffer;
+    using boost::asio::co_spawn;
+    using boost::asio::detached;
+    using boost::asio::experimental::channel;
+    using boost::asio::serial_port;
+    using boost::asio::use_awaitable;
+    using boost::iostreams::mapped_file;
+    using boost::scoped_ptr;
+    using boost::system::error_code;
+
+    using namespace boost::asio::experimental::awaitable_operators;
+    using namespace boost::asio::ip;
+
+    enum class ndi_address_type {
+        ethernet,
+        serial,
+    };
+
+    name_translator<ndi_address_type> ndi_address_type_translator;
+
+    static_block {
+        ndi_address_type_translator.register_item("ethernet", ndi_address_type::ethernet);
+        ndi_address_type_translator.register_item("serial", ndi_address_type::serial);
+    };
+
+    struct ndi_interface::impl {
+
+        using ascii_reply_content = static_memory<1024>;
+        struct ascii_reply_obj : public ascii_reply_content,
+                                 public small_obj<ascii_reply_obj> {
+        };
+
+        using binary_reply_content = dynamic_memory;
+
+        struct binary_reply_obj : public binary_reply_content,
+                                  public small_obj<binary_reply_obj> {
+            FORWARD_CONSTRUCT(binary_reply_obj, binary_reply_content)
+        };
+
+        static constexpr auto reply_queue_size = 4;
+        static constexpr auto ndi_endian = boost::endian::order::little;
+
+        using crc_checker_type = boost::crc_16_type;
+
+        using ascci_reply_queue_type = channel<void(error_code, ascii_reply_obj::pointer)>;
+        ascci_reply_queue_type ascii_reply_queue;
+
+        using binary_reply_queue_type = channel<void(error_code, binary_reply_obj::pointer)>;
+        binary_reply_queue_type binary_reply_queue;
+        binary_reply_queue_type stream_reply_queue; // TODO 如果同时需要多个 stream, 则考虑建一个哈希池
+
+        coro_signal stop_requested_signal;
+
+        ndi_address_type address_type;
+
+        // If address type is ethernet
+        using ip_address_type = boost::asio::ip::address;
+        ip_address_type ndi_ip;
+        uint16_t ndi_port;
+        scoped_ptr<tcp::socket> ndi_tcp_socket;
+
+        // If address type is serial
+        std::string com_port_name;
+        scoped_ptr<serial_port> ndi_com_socket;
+
+        void load_init_config(const nlohmann::json &config) {
+            assert(config.contains("address_type"));
+            assert(config["address_type"].is_string());
+            address_type = ndi_address_type_translator.translate(config["address_type"].get<std::string>());
+            if (address_type == ndi_address_type::ethernet) {
+                assert(config.contains("ip"));
+                assert(config["ip"].is_string());
+                ndi_ip = make_address(config["ip"].get<std::string>());
+                assert(config.contains("tcp_port"));
+                assert(config["tcp_port"].is_number_unsigned());
+                ndi_port = config["tcp_port"].get<uint64_t>();
+            } else if (address_type == ndi_address_type::serial) {
+                assert(config.contains("com_port"));
+                assert(config["com_port"].is_string());
+                com_port_name = config["com_port"].get<std::string>();
+            }
+            assert(false);
+        }
+
+        awaitable<binary_reply_obj::pointer> read_binary_reply(uint16_t header_magic) {
+            // read length and crc
+            using header_reader_type = versatile_readable_buffer<ndi_endian, static_memory<4>>;
+            header_reader_type header_reader;
+            if (address_type == ndi_address_type::serial) {
+                co_await header_reader.async_read_from(*ndi_com_socket, 4);
+            } else if (address_type == ndi_address_type::ethernet) {
+                co_await header_reader.async_read_from(*ndi_tcp_socket, 4);
+            }
+            uint16_t reply_length, header_crc16;
+            header_reader >> reply_length >> header_crc16;
+
+            // check header crc
+            crc_checker_type crc_checker;
+            uint16_t header_magic_ndi = header_magic;
+            swap_net_loc_endian<ndi_endian>(header_magic_ndi);
+            crc_checker.process_bytes(&header_magic_ndi, 2); // header magic
+            crc_checker.process_bytes(header_reader.get_buffer().data(), 2); // reply size
+            if (crc_checker.checksum() != header_crc16) {
+                // TODO show log
+                co_return nullptr;
+            }
+
+            // read reply content
+            auto binary_reply = binary_reply_obj::new_instance(reply_length + 2); // 2 for crc
+            if (address_type == ndi_address_type::serial) {
+                co_await async_read(*ndi_com_socket,
+                                    buffer(binary_reply->data(), reply_length + 2),
+                                    use_awaitable);
+            } else if (address_type == ndi_address_type::ethernet) {
+                co_await async_read(*ndi_tcp_socket,
+                                    buffer(binary_reply->data(), reply_length + 2),
+                                    use_awaitable);
+            }
+
+            // check reply crc
+            crc_checker.reset();
+            crc_checker.process_bytes(binary_reply->data(), reply_length);
+            uint16_t reply_crc;
+            memcpy(&reply_crc, binary_reply->data() + reply_length, 2);
+            swap_net_loc_endian<ndi_endian>(reply_crc);
+            if (crc_checker.checksum() != reply_crc) {
+                // TODO show log
+                co_return nullptr;
+            }
+
+            co_return binary_reply;
+        }
+
+        // 读取并分发 "一份" 来自 NDI 的回复消息
+        awaitable<void> read_and_dispatch_reply() {
+            static constexpr uint16_t binary_reply_header_magic = 0xA5C4;
+            static constexpr uint16_t extended_binary_reply_header_magic = 0xA5C8;
+            static constexpr uint16_t streaming_header_magic = 0xB5D4;
+
+            // 读取前两个字节, 判断报文类型
+            uint16_t header_magic;
+            if (address_type == ndi_address_type::serial) {
+                co_await async_read_value<ndi_endian>(*ndi_com_socket, header_magic);
+            } else if (address_type == ndi_address_type::ethernet) {
+                co_await async_read_value<ndi_endian>(*ndi_tcp_socket, header_magic);
+            } else {
+                assert(false);
+                co_return;
+            }
+
+            switch (header_magic) {
+                case binary_reply_header_magic: {
+                    auto binary_reply = co_await read_binary_reply(header_magic);
+                    if (binary_reply) {
+                        auto ok = binary_reply_queue.try_send(error_code{}, std::move(binary_reply));
+                        if (!ok) {
+                            // TODO show log
+                        }
+                    }
+                    break;
+                }
+                case extended_binary_reply_header_magic: {
+                    break;
+                }
+                case streaming_header_magic: {
+                    break;
+                }
+                default: { // ascii reply
+
+                }
+            }
+
+            co_return;
+        }
+
+        awaitable<void> send_command(std::string &&cmd) {
+            cmd.push_back('\r'); // Add CR to end the command
+            if (address_type == ndi_address_type::serial) {
+                assert(ndi_com_socket->is_open());
+                co_await async_write(*ndi_com_socket, buffer(cmd), use_awaitable);
+            } else if (address_type == ndi_address_type::ethernet) {
+                assert(ndi_tcp_socket->is_open());
+                co_await async_write(*ndi_tcp_socket, buffer(cmd), use_awaitable);
+            } else {
+                assert(false);
+            }
+            co_return;
+        }
+
+        // 对串口进行配置
+        awaitable<void> setup_com_socket() {
+            assert(address_type == ndi_address_type::serial);
+
+            ndi_com_socket->open(com_port_name);
+            assert(ndi_com_socket->is_open());
+
+            // load default config
+            ndi_com_socket->set_option(serial_port::baud_rate(9600));
+            ndi_com_socket->set_option(serial_port::character_size(8));
+            ndi_com_socket->set_option(serial_port::parity(serial_port::parity::none));
+            ndi_com_socket->set_option(serial_port::stop_bits(serial_port::stop_bits::one));
+            ndi_com_socket->set_option(serial_port::flow_control(serial_port::flow_control::none));
+
+            ndi_com_socket->send_break(); // cause the system to reset
+
+
+            co_return;
+        }
+
+        awaitable<bool> on_init_impl() {
+            // establish connection
+            if (address_type == ndi_address_type::ethernet) {
+                ndi_tcp_socket.reset(new tcp::socket(get_context()));
+                co_await ndi_tcp_socket->async_connect({ndi_ip, ndi_port}, use_awaitable);
+                // decrease delay
+                tcp::no_delay no_delay_option(true);
+                ndi_tcp_socket->set_option(no_delay_option);
+            } else if (address_type == ndi_address_type::serial) {
+                ndi_com_socket.reset(new serial_port(get_context()));
+                co_await setup_com_socket();
+            } else {
+                co_return false;
+            }
+
+            // setup steps
+//            versatile_buffer buffer;
+//            CO_ENSURE(negotiate_protocol(buffer))
+//            co_await print_ur_control_version(buffer);
+//            CO_ENSURE(setup_inputs(buffer))
+//            CO_ENSURE(setup_outputs(buffer))
+//            co_await install_program();
+//            co_return true;
+        }
+
+        awaitable<void> on_stop_impl() {
+            // TODO
+            co_return;
+        }
+
+        impl()
+                : ascii_reply_queue(get_context(), reply_queue_size),
+                  binary_reply_queue(get_context(), reply_queue_size),
+                  stream_reply_queue(get_context(), reply_queue_size),
+                  stop_requested_signal(get_context()) {}
+
+    };
+
+    ndi_interface::ndi_interface()
+            : pimpl(std::make_unique<impl>()) {}
+
+    boost::asio::awaitable<void> ndi_interface::on_stop() {
+        co_await pimpl->on_stop_impl();
+        co_await tristate_obj::on_stop();
+        co_return;
+    }
+
+    ndi_interface::~ndi_interface() = default;
+
+}

src/tracker/ndi/ndi_interface.h

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

src/utility/bit_operations.hpp

@@ -35,6 +35,7 @@ namespace sophiar {
     }
 
     template<typename T>
+    [[deprecated("This version indicate that the net order is big endian.")]]
     std::enable_if_t<std::is_arithmetic_v<T>>
     inline swap_net_loc_endian(T &val) {
         if constexpr (boost::endian::order::native == boost::endian::order::big) {
@@ -44,6 +45,26 @@ namespace sophiar {
         }
     }
 
+    template<boost::endian::order net_order, typename T>
+    std::enable_if_t<std::is_arithmetic_v<T>>
+    inline swap_net_loc_endian(T &val) {
+        if constexpr (boost::endian::order::native == net_order) {
+            return;
+        } else {
+            boost::endian::endian_reverse_inplace(val);
+        }
+    }
+
+    template<typename T>
+    inline void swap_big_loc_endian(T &val) {
+        swap_net_loc_endian<boost::endian::order::big>(val);
+    }
+
+    template<typename T>
+    inline void swap_little_loc_endian(T &val) {
+        swap_net_loc_endian<boost::endian::order::little>(val);
+    }
+
 }
 
 #endif //SOPHIAR2_BIT_OPERATIONS_HPP

src/utility/versatile_buffer.hpp

@@ -19,7 +19,8 @@ namespace sophiar {
     static constexpr size_t VERSATILE_BUFFER_MAX_SIZE = 1024;
 
     // handle endian properly
-    class versatile_buffer {
+    class [[deprecated("This version is poorly designed.")]]
+    versatile_buffer {
     public:
 
         template<typename T>
@@ -142,6 +143,19 @@ namespace sophiar {
             co_return;
         }
 
+        // will cause reset
+//        template<typename AsyncReadStream>
+//        boost::asio::awaitable<void> async_read_until(AsyncReadStream &s, char delimiter = '\r') {
+//            assert(empty());
+//            reset();
+//            assert(length <= VERSATILE_BUFFER_MAX_SIZE);
+//            co_await boost::asio::async_read(s,
+//                                             boost::asio::buffer(buffer, length),
+//                                             boost::asio::use_awaitable);
+//            end_pos = length;
+//            co_return;
+//        }
+
         // will cause reset
         template<typename AsyncWriteStream>
         boost::asio::awaitable<void> async_write_to(AsyncWriteStream &s) {
@@ -176,6 +190,7 @@ namespace sophiar {
     };
 
     template<typename AsyncWriteStream, typename T>
+    [[deprecated]]
     std::enable_if_t<std::is_arithmetic_v<T>, boost::asio::awaitable<void>>
     inline async_write_value(AsyncWriteStream &s, T val) {
         swap_net_loc_endian(val);
@@ -186,6 +201,7 @@ namespace sophiar {
     }
 
     template<typename T, typename AsyncReadStream>
+    [[deprecated]]
     std::enable_if_t<std::is_arithmetic_v<T>, boost::asio::awaitable<T>>
     inline async_read_value(AsyncReadStream &s) {
         T tmp_val;
@@ -197,6 +213,7 @@ namespace sophiar {
     }
 
     template<typename T, typename AsyncReadStream>
+    [[deprecated]]
     std::enable_if_t<std::is_arithmetic_v<T>, boost::asio::awaitable<void>>
     inline async_read_value(AsyncReadStream &s, T &val) {
         val = co_await async_read_value<T>(s);

src/utility/versatile_buffer2.hpp

@@ -0,0 +1,320 @@
+#ifndef SOPHIAR2_VERSATILE_BUFFER2_HPP
+#define SOPHIAR2_VERSATILE_BUFFER2_HPP
+
+#include "utility/bit_operations.hpp"
+
+#include <boost/asio/awaitable.hpp>
+#include <boost/asio/read.hpp>
+#include <boost/asio/use_awaitable.hpp>
+#include <boost/asio/write.hpp>
+
+#include <Eigen/Core>
+
+#include <cassert>
+#include <concepts>
+#include <string>
+#include <type_traits>
+
+namespace sophiar {
+
+    template<size_t length>
+    struct static_memory {
+
+        char *data() { return buf; }
+
+        const char *data() const { return buf; }
+
+        constexpr size_t max_length() { return length; }
+
+    private:
+        char buf[length];
+    };
+
+    struct dynamic_memory {
+
+        explicit dynamic_memory(size_t buf_length) {
+            length = buf_length;
+            buf = new char[length];
+        }
+
+        ~dynamic_memory() { delete[]buf; }
+
+        char *data() { return buf; }
+
+        const char *data() const { return buf; }
+
+        size_t max_length() const { return length; }
+
+    private:
+        char *buf;
+        size_t length;
+    };
+
+    struct extern_memory {
+
+        extern_memory(char *extern_buf, size_t buf_length)
+                : buf(extern_buf), length(buf_length) {}
+
+        char *data() { return buf; }
+
+        const char *data() const { return buf; }
+
+        size_t max_length() const { return length; }
+
+    private:
+        char *buf;
+        size_t length;
+    };
+
+    struct const_extern_memory {
+        const_extern_memory(const char *extern_buf, size_t buf_length)
+                : buf(extern_buf), length(buf_length) {}
+
+        const char *data() const { return buf; }
+
+        size_t max_length() const { return length; }
+
+    private:
+        const char *buf;
+        size_t length;
+    };
+
+    template<typename T>
+    concept FixedLengthMemory = requires(T a) {
+        { a.data() } -> std::convertible_to<const char *>; // 如果需要 char *, 就让它报错
+        { a.max_length() }->std::convertible_to<size_t>;
+    };
+
+    static constexpr size_t VERSATILE_BUFFER_DEFAULT_SIZE = 1024;
+
+    // 一次性写入数据，分多次读取
+    template<boost::endian::order net_order = boost::endian::order::big,
+            FixedLengthMemory buffer_type = static_memory<VERSATILE_BUFFER_DEFAULT_SIZE>>
+    class versatile_readable_buffer {
+    public:
+
+        template<typename other_buffer_type = buffer_type,
+                typename  = std::enable_if_t<std::is_default_constructible_v<other_buffer_type>>>
+        versatile_readable_buffer() {
+            cur_pos = 0;
+            cur_length = 0;
+        }
+
+        explicit versatile_readable_buffer(const buffer_type &other_buffer,
+                                           size_t initial_length = 0,
+                                           size_t initial_pos = 0)
+                : buffer(other_buffer) {
+            cur_pos = initial_pos;
+            cur_length = initial_length;
+        }
+
+        template<typename T>
+        std::enable_if_t<std::is_arithmetic_v<T>, T>
+        read_value() {
+            assert(cur_pos + sizeof(T) <= cur_length);
+            T tmp_val;
+            std::memcpy(&tmp_val, cur_data(), sizeof(T));
+            swap_net_loc_endian<net_order>(tmp_val);
+            cur_pos += sizeof(T);
+            return tmp_val;
+        }
+
+        template<typename T>
+        std::enable_if_t<std::is_arithmetic_v<T>>
+        read_value(T &val) {
+            val = read_value<T>();
+        }
+
+        template<typename Derived>
+        void read_value(Eigen::MatrixBase<Derived> &matrix) {
+            for (Eigen::Index i = 0; i < matrix.size(); ++i) {
+                read_value(matrix(i));
+            }
+        }
+
+        std::string read_string(size_t read_length) {
+            assert(cur_pos + read_length <= cur_length);
+            auto ret = std::string{cur_data(), cur_data() + read_length};
+            cur_pos += read_length;
+            return ret;
+        }
+
+        void read_string(std::string &str, size_t length) {
+            str = read_string(length);
+        }
+
+        std::string read_string_until(char sep) {
+            auto start_pos = cur_pos;
+            while (cur_pos != cur_length && *cur_data() != sep) {
+                ++cur_pos;
+            }
+            return std::string{buffer.data() + start_pos, cur_data()};
+        }
+
+        void read_string_until(std::string &str, char sep) {
+            str = read_string_until(sep);
+        }
+
+        template<typename T>
+        auto &operator>>(T &val) {
+            read_value(val);
+            return *this;
+        }
+
+        // 从当前位置开始调整 offset
+        void manual_offset(ptrdiff_t offset) {
+            assert(offset < 0 && cur_pos >= -offset);
+            assert(offset > 0 && cur_pos + offset <= cur_length);
+            cur_pos += offset;
+        }
+
+        const buffer_type &get_buffer() const {
+            return buffer;
+        }
+
+        const char *cur_data() const {
+            return buffer.data() + cur_pos;
+        }
+
+        // 一次性写入数据，原先的数据会直接被覆盖
+        template<typename AsyncReadStream>
+        boost::asio::awaitable<void> async_read_from(AsyncReadStream &s, size_t read_length) {
+            assert(empty());
+            assert(read_length < buffer.max_length());
+
+            co_await boost::asio::async_read(s,
+                                             boost::asio::buffer(buffer.data(), read_length),
+                                             boost::asio::use_awaitable);
+            cur_length = read_length;
+            cur_pos = 0;
+            co_return;
+        }
+
+        bool empty() const {
+            return cur_pos == cur_length;
+        }
+
+        explicit operator bool() const {
+            return !empty();
+        }
+
+    private:
+        buffer_type buffer;
+        size_t cur_pos, cur_length;
+
+    };
+
+    // 分多次写入数据，一次性读取
+    template<boost::endian::order net_order = boost::endian::order::big,
+            FixedLengthMemory buffer_type = static_memory<VERSATILE_BUFFER_DEFAULT_SIZE>>
+    class versatile_writable_buffer {
+    public:
+
+        template<typename other_buffer_type = buffer_type,
+                typename  = std::enable_if_t<std::is_default_constructible_v<other_buffer_type>>>
+        versatile_writable_buffer() {
+            cur_pos = 0;
+        }
+
+        explicit versatile_writable_buffer(buffer_type &&other_buffer,
+                                           size_t initial_pos = 0)
+                : buffer(other_buffer) {
+            cur_pos = initial_pos;
+        }
+
+        template<typename T>
+        std::enable_if_t<std::is_arithmetic_v<T>>
+        write_value(T val) {
+            assert(cur_pos + sizeof(T) <= buffer.max_length());
+            swap_net_loc_endian<net_order>(val);
+            std::memcpy(cur_data(), &val, sizeof(T));
+            cur_pos += sizeof(T);
+        }
+
+        void write_value(const std::string &str) {
+            assert(cur_pos + str.length() <= buffer.max_length());
+            std::memcpy(cur_data(), str.data(), str.length());
+            cur_pos += str.length();
+        }
+
+        template<typename Derived>
+        void write_value(const Eigen::MatrixBase<Derived> &matrix) {
+            for (Eigen::Index i = 0; i < matrix.size(); ++i) {
+                write_value(matrix(i));
+            }
+        }
+
+        template<typename T>
+        auto &operator<<(const T &val) {
+            write_value(val);
+            return *this;
+        }
+
+        // 从当前位置开始调整 offset
+        void manual_offset(ptrdiff_t offset) {
+            assert(offset < 0 && cur_pos >= -offset);
+            assert(offset > 0 && cur_pos + offset <= buffer.max_length());
+            cur_pos += offset;
+        }
+
+        char *cur_data() {
+            return buffer.data() + cur_pos;
+        }
+
+        template<typename AsyncWriteStream>
+        boost::asio::awaitable<void> async_write_to(AsyncWriteStream &s) {
+            co_await boost::asio::async_write(s,
+                                              boost::asio::buffer(buffer, cur_pos),
+                                              boost::asio::use_awaitable);
+            cur_pos = 0;
+            co_return;
+        }
+
+        bool empty() const {
+            return cur_pos == 0;
+        }
+
+        explicit operator bool() const {
+            return !empty();
+        }
+
+    private:
+        buffer_type buffer;
+        size_t cur_pos;
+
+    };
+
+    template<boost::endian::order net_order = boost::endian::order::big,
+            typename AsyncWriteStream, typename T>
+    std::enable_if_t<std::is_arithmetic_v<T>, boost::asio::awaitable<void>>
+    inline async_write_value(AsyncWriteStream &s, T val) {
+        swap_net_loc_endian<net_order>(val);
+        co_await boost::asio::async_write(s,
+                                          boost::asio::buffer(&val, sizeof(T)),
+                                          boost::asio::use_awaitable);
+        co_return;
+    }
+
+    template<boost::endian::order net_order = boost::endian::order::big,
+            typename T, typename AsyncReadStream>
+    std::enable_if_t<std::is_arithmetic_v<T>, boost::asio::awaitable<T>>
+    inline async_read_value(AsyncReadStream &s) {
+        T tmp_val;
+        co_await boost::asio::async_read(s,
+                                         boost::asio::buffer(&tmp_val, sizeof(T)),
+                                         boost::asio::use_awaitable);
+        swap_net_loc_endian<net_order>(tmp_val);
+        co_return tmp_val;
+    }
+
+    template<boost::endian::order net_order = boost::endian::order::big,
+            typename T, typename AsyncReadStream>
+    std::enable_if_t<std::is_arithmetic_v<T>, boost::asio::awaitable<void>>
+    inline async_read_value(AsyncReadStream &s, T &val) {
+        val = co_await async_read_value<net_order, T>(s);
+        co_return;
+    }
+
+}
+
+#endif //SOPHIAR2_VERSATILE_BUFFER2_HPP

tests/data/ndi_interface_config.json

@@ -0,0 +1,35 @@
+{
+  "mode_list": [
+    {
+      "name": "test_mode"
+    }
+  ],
+  "object_list": [
+    {
+      "type": "ndi_interface",
+      "name": "ndi",
+      "enabled_modes": "all",
+      "construct_config": {},
+      "init_configs": [
+        {
+          "address_type": "ethernet",
+          "ip": "169.254.132.51",
+          "tcp_port": 8765,
+          "com_port": "COM3",
+          "tool_list": [
+            {
+              "name": "Probe_Small_4Ball",
+              "rom_path": "D:\\Program\\Robot\\Tools\\roms\\Probe_Small_4Ball.rom"
+            }
+          ]
+        }
+      ],
+      "start_configs": [
+        {
+        }
+      ]
+    }
+  ],
+  "connection_list": [
+  ]
+}