1. 实现了 NDI 接口

2. 测试了 NDI 接口在以太网下的使用

src/core/basic_obj_types.hpp

@@ -0,0 +1,29 @@
+#ifndef SOPHIAR2_BASIC_OBJ_TYPES_HPP
+#define SOPHIAR2_BASIC_OBJ_TYPES_HPP
+
+#include "core/small_obj.hpp"
+
+#include <Eigen/Geometry>
+
+namespace sophiar {
+
+    template<typename T>
+    struct small_obj_wrapper : public small_obj<small_obj_wrapper<T>> {
+        T value;
+
+        small_obj_wrapper() = default;
+
+        explicit small_obj_wrapper(const T &other)
+                : value(other) {}
+
+        explicit small_obj_wrapper(T &&other)
+                : value(std::move(other)) {}
+    };
+
+    using bool_obj = small_obj_wrapper<bool>;
+    using double_obj = small_obj_wrapper<double>;
+    using transform_obj = small_obj_wrapper<Eigen::Isometry3d>;
+
+}
+
+#endif //SOPHIAR2_BASIC_OBJ_TYPES_HPP

src/core/sophiar_manager.h

@@ -25,6 +25,7 @@
 namespace sophiar {
 
     using global_obj_index_type = uint16_t;
+    static constexpr auto invalid_global_obj_index = ~(global_obj_index_type) 0;
 
     class sophiar_obj;
 
@@ -49,11 +50,10 @@ namespace sophiar {
 
         template<typename SmallObjType>
         global_obj_index_type register_global_obj(const std::string &obj_name) {
-            static_assert(std::is_default_constructible_v<SmallObjType>);
             auto ret = register_global_obj_impl(obj_name, typeid(SmallObjType), nullptr);
             if (static_cast<decltype(ret)>(~ret) == 0) { // first time of register
                 using ptr_type = typename SmallObjType::pointer;
-                auto placeholder = new ptr_type(SmallObjType::new_instance()); // requires default constructable
+                auto placeholder = new ptr_type;
                 ret = register_global_obj_impl(obj_name, typeid(SmallObjType), placeholder);
             }
             return ret;

src/main.cpp

@@ -10,6 +10,7 @@
 #include <boost/asio/detached.hpp>
 #include <boost/property_tree/ptree.hpp>
 #include <boost/uuid/uuid_generators.hpp>
+#include <boost/algorithm/hex.hpp>
 
 #include <boost/asio/experimental/channel.hpp>
 #include <boost/intrusive/list.hpp>
@@ -20,30 +21,11 @@ using boost::asio::detached;
 using namespace sophiar;
 
 void test() {
-    using token_hook_type = boost::intrusive::list_base_hook<
-            boost::intrusive::link_mode<boost::intrusive::auto_unlink>>;
-    struct test_type : token_hook_type {
-        int val;
-
-        test_type(int v) : val(v) {}
-    };
-
-    using list_type = boost::intrusive::list<
-            test_type, boost::intrusive::constant_time_size<false>>;
-
-    list_type my_list;
-    test_type a(1), b(2), c(3);
-
-    my_list.push_back(a);
-    my_list.push_back(b);
-    my_list.push_back(c);
-
-    while (!my_list.empty()) {
-        auto &node = my_list.front();
-        node.unlink();
-        std::cout << node.val << std::endl;
-    }
-
+    uint16_t data = 0x0123;
+    char out[16] = {0x00};
+    memset(out, 0x00, 16);
+    boost::algorithm::hex((uint8_t *) &data, (uint8_t *) &data + sizeof(uint16_t), out);
+    std::cout << out << std::endl;
 }
 
 int main() {

src/tracker/ndi/ndi_interface.h

@@ -13,17 +13,15 @@ namespace sophiar {
 
         ~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_init(const nlohmann::json &config) override;
 
-//        boost::asio::awaitable<bool> on_start(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;
+        boost::asio::awaitable<void> on_reset() override;
 
     private:
         struct impl;

src/utility/bit_operations.hpp

@@ -13,6 +13,12 @@ namespace sophiar {
 
     using bit_pos_type = std::uint8_t;
 
+    template<typename T>
+    std::enable_if_t<std::is_integral_v<T>, bool>
+    inline is_valid_id(T val) { // every bit of an invalid id is set to 1
+        return static_cast<T>(~val) != 0;
+    }
+
     template<typename T>
     std::enable_if_t<std::is_integral_v<T>>
     inline set_bit(T &val, bit_pos_type pos) {
@@ -27,6 +33,14 @@ namespace sophiar {
         return val & (T(1) << pos);
     }
 
+    template<typename T, bit_pos_type Pos>
+    std::enable_if_t<std::is_integral_v<T>, bool>
+    inline test_bit(const T &val) {
+        static_assert(sizeof(T) * CHAR_BIT - 1 <= std::numeric_limits<bit_pos_type>::max());
+        static constexpr auto test_mask = T(1) << Pos;
+        return val & test_mask;
+    }
+
     template<typename T>
     std::enable_if_t<std::is_integral_v<T>, bit_pos_type>
     inline popcount(const T &val) {

src/utility/coro_worker.hpp

@@ -16,6 +16,7 @@
 
 namespace sophiar {
 
+    // Can not be shared in any way
     class coro_worker {
     public:
 
@@ -32,16 +33,7 @@ namespace sophiar {
             request_stop_signal.try_notify_all();
         }
 
-        boost::asio::awaitable<void> coro_wait_stop() {
-            if (is_running) {
-                co_await stop_finished_watcher.coro_wait(false);
-            } else {
-                auto is_stopped = stop_finished_watcher.try_wait();
-                assert(is_stopped);
-            }
-            assert(!is_running);
-            co_return;
-        }
+        virtual boost::asio::awaitable<void> coro_wait_stop() = 0;
 
     protected:
 
@@ -49,15 +41,12 @@ namespace sophiar {
         explicit coro_worker(Executor &executor)
                 :request_stop_signal(executor),
                  stop_finished_signal(executor),
-                 request_stop_watcher(request_stop_signal.new_watcher(executor)),
-                 stop_finished_watcher(stop_finished_signal.new_watcher(executor)) {
+                 request_stop_watcher(request_stop_signal.new_watcher(executor)) {
         }
 
-        signal_watcher request_stop_watcher;
-        signal_watcher stop_finished_watcher;
-
         coro_signal2 request_stop_signal;
         coro_signal2 stop_finished_signal;
+        signal_watcher request_stop_watcher;
 
         bool is_running = false;
 
@@ -81,10 +70,17 @@ namespace sophiar {
         void run() override {
             assert(!is_running);
             request_stop_watcher.sync();
-            stop_finished_watcher.sync();
             boost::asio::co_spawn(executor, run_impl(), boost::asio::detached);
         }
 
+        boost::asio::awaitable<void> coro_wait_stop() override {
+            if (!is_running) co_return;
+            auto stop_finished_watcher = stop_finished_signal.new_watcher(executor);
+            co_await stop_finished_watcher.coro_wait();
+            assert(!is_running);
+            co_return;
+        }
+
     private:
 
         using store_type = std::remove_cvref_t<FuncType>;

src/utility/global_obj_helper.hpp

@@ -17,11 +17,12 @@ namespace sophiar {
                             global_obj_index_type _obj_index)
                 : manager(_manager),
                   watcher(_manager.request_global_obj_update_watcher(_obj_index)),
-                  obj_index(_obj_index) {
-            watcher.sync();
-            obj_ptr = manager.get_global_obj<SmallObjType>(obj_index);
-            assert(obj_ptr.get() != nullptr);
-        }
+                  obj_index(_obj_index) {}
+
+        global_obj_delegate(sophiar_manager &_manager,
+                            const std::string &obj_name)
+                : global_obj_delegate(_manager,
+                                      _manager.register_global_obj<SmallObjType>(obj_name)) {}
 
         global_obj_delegate(global_obj_delegate &other) noexcept
                 : manager(other.manager),
@@ -41,10 +42,13 @@ namespace sophiar {
                     obj_ptr = manager.get_global_obj<SmallObjType>(obj_index);
                 }
             }
-            assert(obj_ptr.get() != nullptr);
             return obj_ptr;
         }
 
+        bool empty() {
+            return get_value() == nullptr;
+        }
+
         timestamp_type get_last_update_ts() const {
             return watcher.get_last_update_ts();
         }
@@ -55,10 +59,12 @@ namespace sophiar {
         }
 
         const SmallObjType &operator*() {
+            assert(!empty());
             return *get_value();
         }
 
         const SmallObjType *operator->() {
+            assert(!empty());
             return get_value().get();
         }
 

src/utility/versatile_buffer2.hpp

@@ -37,6 +37,12 @@ namespace sophiar {
             buf = new char[length];
         }
 
+        dynamic_memory(dynamic_memory &&other) noexcept {
+            buf = other.buf;
+            length = other.length;
+            other.buf = nullptr;
+        }
+
         ~dynamic_memory() { delete[]buf; }
 
         char *data() { return buf; }
@@ -100,10 +106,10 @@ namespace sophiar {
             cur_length = 0;
         }
 
-        explicit versatile_readable_buffer(const buffer_type &other_buffer,
+        explicit versatile_readable_buffer(buffer_type &&other_buffer,
                                            size_t initial_length = 0,
                                            size_t initial_pos = 0)
-                : buffer(other_buffer) {
+                : buffer(std::move(other_buffer)) {
             cur_pos = initial_pos;
             cur_length = initial_length;
         }
@@ -180,7 +186,7 @@ namespace sophiar {
         template<typename AsyncReadStream>
         boost::asio::awaitable<void> async_read_from(AsyncReadStream &s, size_t read_length) {
             assert(empty());
-            assert(read_length < buffer.max_length());
+            assert(read_length <= buffer.max_length());
 
             co_await boost::asio::async_read(s,
                                              boost::asio::buffer(buffer.data(), read_length),
@@ -218,7 +224,7 @@ namespace sophiar {
 
         explicit versatile_writable_buffer(buffer_type &&other_buffer,
                                            size_t initial_pos = 0)
-                : buffer(other_buffer) {
+                : buffer(std::move(other_buffer)) {
             cur_pos = initial_pos;
         }
 

tests/CMakeLists.txt

@@ -24,4 +24,13 @@ add_executable(test_utility
         ${CORE_IMPL_FILES})
 target_compile_definitions(test_utility PUBLIC SOPHIAR_TEST)
 target_compile_definitions(test_utility PUBLIC CORO_SIGNAL2_USE_TIMER)
-target_link_libraries(test_utility ${Boost_LIBRARIES} ${EXTRA_LIBS})
+target_link_libraries(test_utility ${Boost_LIBRARIES} ${EXTRA_LIBS})
+
+add_executable(test_ndi
+        interface/ndi.cpp
+        ../src/tracker/ndi/ndi_interface.cpp
+        ${EXTERN_DEF_FILES}
+        ${CORE_IMPL_FILES})
+target_compile_definitions(test_ndi PUBLIC SOPHIAR_TEST)
+target_compile_definitions(test_ndi PUBLIC CORO_SIGNAL2_USE_TIMER)
+target_link_libraries(test_ndi ${Boost_LIBRARIES} ${EXTRA_LIBS})

tests/data/ndi_interface_config.json

@@ -18,14 +18,26 @@
           "com_port": "COM3",
           "tool_list": [
             {
-              "name": "Probe_Small_4Ball",
-              "rom_path": "D:\\Program\\Robot\\Tools\\roms\\Probe_Small_4Ball.rom"
+              "rom_path": "D:\\Program\\Robot\\Tools\\roms\\Probe_Small_4Ball.rom",
+              "outputs": {
+                "transform": "tracker_probe_transform",
+                "marker_uncertainty": "tracker_probe_transform_uncertainty"
+              }
+            },
+            {
+              "rom_path": "D:\\Program\\Robot\\Tools\\roms\\Head_3Ball_2.rom",
+              "outputs": {
+                "transform": "tracker_model_ref_transform",
+                "marker_uncertainty": "tracker_model_ref_transform_uncertainty"
+              }
             }
           ]
         }
       ],
       "start_configs": [
         {
+          "allow_unreliable": true,
+          "prefer_stream_tracking": true
         }
       ]
     }

tests/interface/ndi.cpp

@@ -0,0 +1,72 @@
+#define BOOST_TEST_DYN_LINK
+#define BOOST_TEST_MAIN  // in only one cpp file
+
+#include "tracker/ndi/ndi_interface.h"
+#include "utility/debug_utility.hpp"
+#include "utility/global_obj_helper.hpp"
+#include "core/basic_obj_types.hpp"
+
+#include <boost/asio/co_spawn.hpp>
+#include <boost/asio/detached.hpp>
+#include <boost/asio/this_coro.hpp>
+#include <boost/asio/use_awaitable.hpp>
+#include <boost/test/unit_test.hpp>
+
+#include <chrono>
+#include <iostream>
+#include <vector>
+
+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;
+
+awaitable<void> coro_ndi_test() {
+    auto interface = std::make_unique<ndi_interface>();
+    auto init_config = nlohmann::json::parse(
+            R"({"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":"tracker_probe_transform","marker_uncertainty":"tracker_probe_transform_uncertainty"}},{"rom_path":"D:\\Program\\Robot\\Tools\\roms\\Head_3Ball_2.rom","outputs":{"transform":"tracker_model_ref_transform","marker_uncertainty":"tracker_model_ref_transform_uncertainty"}}]})");
+    auto start_config = nlohmann::json::parse(
+            R"({"allow_unreliable":true,"prefer_stream_tracking":true})");
+
+    BOOST_TEST(co_await interface->init(init_config));
+    BOOST_TEST(co_await interface->start(start_config));
+    auto trans_obj = global_obj_auto_sync_delegate<transform_obj>(
+            global_sophiar_manager,
+            "tracker_probe_transform");
+    int cnt = 0;
+    FILE_LINE_TRACE
+    for (;;) {
+        co_await trans_obj.coro_wait_update();
+//        if (!trans_obj.empty()) {
+//            std::cout << trans_obj->value.matrix() << std::endl;
+//        } else {
+//            std::cout << "empty" << std::endl;
+//        }
+        if (++cnt == 250) break;
+    }
+    FILE_LINE_TRACE
+    co_await interface->reset();
+
+    start_config = nlohmann::json::parse(
+            R"({"allow_unreliable":true,"prefer_stream_tracking":false})");
+    BOOST_TEST(co_await interface->init(init_config));
+    BOOST_TEST(co_await interface->start(start_config));
+    cnt = 0;
+    FILE_LINE_TRACE
+    for (;;) {
+        co_await trans_obj.coro_wait_update();
+        if (++cnt == 60) break;
+    }
+    FILE_LINE_TRACE
+    co_await interface->reset();
+
+    co_return;
+}
+
+BOOST_AUTO_TEST_CASE(test_ndi) {
+    co_spawn(global_context, coro_ndi_test(), detached);
+    global_context.run();
+}