测试并改进了 ndi_interface

src/core/global_defs.cpp

@@ -18,7 +18,19 @@
 
 #endif // BOOST_OS_WINDOWS_AVAILABLE
 
+#if !SOPHIAR_TEST || SOPHIAR_TEST_ALGORITHM
+
 DEFAULT_TRISTATE_OBJ_DEF(transform_tree)
+DEFAULT_TRISTATE_OBJ_DEF(transform_stabilizer)
+DEFAULT_TRISTATE_OBJ_DEF(landmark_registration)
+
+#endif
+
+#if !SOPHIAR_TEST || SOPHIAR_TEST_NDI
+
+DEFAULT_TRISTATE_OBJ_DEF(ndi_interface)
+
+#endif
 
 namespace sophiar {
 
@@ -51,13 +63,18 @@ namespace sophiar {
     }
 
     void register_algorithms() {
-        // transform tree
+#if !SOPHIAR_TEST || SOPHIAR_TEST_ALGORITHM
+
         REGISTER_TYPE(transform_tree);
+        REGISTER_TYPE(transform_stabilizer);
+        REGISTER_TYPE(landmark_registration);
         // transform utility
         using transform_inverter = simple_tristate_obj_wrapper<make_transform_inverter_func>;
         using scalarxyz_transformer = simple_tristate_obj_wrapper<make_scalarxyz_transformer>;
         REGISTER_TYPE(transform_inverter);
         REGISTER_TYPE(scalarxyz_transformer);
+
+#endif
     }
 
     struct empty_object : public tristate_obj {
@@ -67,16 +84,17 @@ namespace sophiar {
     void register_object_types() {
 
         REGISTER_TYPE(empty_object);
+        register_algorithms();
 
-#if !SOPHIAR_TEST || SOPHIAR_TEST_ALGORITHM
+#if !SOPHIAR_TEST || SOPHIAR_TEST_ROBOT
 
-        register_algorithms();
+        REGISTER_TYPE(ur_interface);
 
 #endif
 
-#if !SOPHIAR_TEST || SOPHIAR_TEST_ROBOT
+#if !SOPHIAR_TEST || SOPHIAR_TEST_NDI
 
-        REGISTER_TYPE(ur_interface);
+        REGISTER_TYPE(ndi_interface);
 
 #endif
 

src/main.cpp

@@ -1,58 +1,22 @@
-#include <coroutine>
-#include <iostream>
-#include <thread>
+#include "core/global_defs.h"
 
-#include "algorithm/transform_utility.hpp"
-#include "algorithm/measure_window.hpp"
-#include "robot/ur/ur_interface.h"
-#include "utility/variable_utility.hpp"
-#include "utility/dynamic_pool.hpp"
+#include <boost/asio/io_context.hpp>
 
-#include <boost/asio/co_spawn.hpp>
-#include <boost/asio/detached.hpp>
-#include <boost/property_tree/ptree.hpp>
-#include <boost/uuid/uuid_generators.hpp>
-#include <boost/algorithm/hex.hpp>
+#include <fstream>
 
-#include <boost/asio/experimental/channel.hpp>
-#include <boost/intrusive/list.hpp>
+#include <nlohmann/json.hpp>
 
-using boost::asio::co_spawn;
-using boost::asio::detached;
+#include "utility/assert_utility.h"
 
 using namespace sophiar;
 
-void test() {
-    std::vector<uint16_t, global_dynamic_allocator<uint16_t>> v;
-    for (int i = 0; i < 1000; ++i) {
-        v.push_back(i);
-    }
-    std::cout << std::accumulate(v.begin(), v.end(), 1) << std::endl;
-}
-
-int main() {
-//    ur_interface ur;
-//    fake_state_listener listener;
-//    ur.set_ur_ip(boost::asio::ip::make_address_v4("192.168.38.141"));
-//    ur.add_status_listener(listener);
-//    ur.set_report_frequency(125);
-//    auto func = [&]() -> boost::asio::awaitable<void> {
-//        auto &cmd_listener = ur.get_command_listener(0);
-//        co_await ur.init();
-//        co_await ur.start();
-//        for (int i = 0; i < 125; ++i) {
-//            auto move = ur_command::new_tcp_speed({0, 0, 0, 0.01, 0, 0});
-//            cmd_listener.on_signal_received(std::move(move));
-//            co_await coro_sleep(std::chrono::milliseconds(8));
-//        }
-//        co_await ur.reset();
-//        co_return;
-//    };
-//    co_spawn(ur.get_context(), func(), detached);
-//    ur.get_context().run();
-//    std::cout << listener.cnt << std::endl;
-
-    test();
-
+int main(int argc, char *argv[]) {
+    assert(argc == 2);
+    std::ifstream config_file(argv[1]);
+    assert(config_file.is_open());
+    auto config = nlohmann::json::parse(config_file);
+    auto ok = initialize(config);
+    assert(ok);
+    global_context->run();
     return 0;
 }

src/tracker/ndi/ndi_interface.cpp

@@ -1,5 +1,3 @@
-#include "ndi_interface.h"
-
 #include "core/basic_obj_types.hpp"
 #include "core/small_obj.hpp"
 #include "utility/config_utility.hpp"
@@ -30,6 +28,12 @@
 #include <boost/smart_ptr/scoped_ptr.hpp>
 #include <boost/system/error_code.hpp>
 
+#ifdef BOOST_OS_WINDOWS_AVAILABLE
+
+#include <winbase.h> // for sending serial break
+
+#endif // BOOST_OS_WINDOWS_AVAILABLE
+
 #include <fmt/format.h>
 #include <spdlog/spdlog.h>
 
@@ -40,6 +44,8 @@
 
 #include "utility/assert_utility.h"
 
+DEFAULT_TRISTATE_OBJ_DEF(ndi_interface)
+
 namespace sophiar {
 
     using boost::asio::async_read;
@@ -100,7 +106,6 @@ namespace sophiar {
 
         static constexpr auto default_tracking_interval = std::chrono::milliseconds(17); // 60Hz
         static constexpr uint16_t default_ndi_port = 8765;
-        static constexpr auto stream_queue_size = 4;
         static constexpr auto ndi_endian = boost::endian::order::little;
 
         static constexpr uint16_t binary_reply_header_magic = 0xA5C4;
@@ -118,9 +123,6 @@ namespace sophiar {
         using reply_callback_type = std::function<void(dynamic_memory::pointer &&)>;
         string_map<reply_callback_type> stream_reply_callback_pool;
 
-        using streambuf_type = basic_streambuf<DYNAMIC_ALLOCATOR(char) >;
-        streambuf_type ascii_buf;
-
         ndi_address_type address_type;
 
         // If address type is ethernet
@@ -189,6 +191,7 @@ namespace sophiar {
                 return async_read_value<ndi_endian, T>(*ndi_tcp_socket);
             }
             assert(false);
+            __builtin_unreachable();
         }
 
         template<WriteableMemory MemoryType>
@@ -199,6 +202,7 @@ namespace sophiar {
                 return async_fill_memory_from(*ndi_tcp_socket, mem);
             }
             assert(false);
+            __builtin_unreachable();
         }
 
         awaitable<dynamic_memory::pointer> read_binary_reply(uint16_t header_magic) {
@@ -280,16 +284,20 @@ namespace sophiar {
                 }
             }
 
-            auto stream_id = stream_id_buf->as_string_view();
+            auto stream_id = std::string_view{stream_id_buf->data(), stream_id_length};
             assert(stream_reply_callback_pool.contains(stream_id));
             auto &callback = stream_reply_callback_pool.query(stream_id);
             callback(std::move(reply_buf));
+            co_return;
         }
 
         // header_magic is also part of the reply
         awaitable<dynamic_memory::pointer> read_ascii_reply(uint16_t header_magic) {
             static constexpr char cr_ascii = '\r';
 
+            using streambuf_type = basic_streambuf<DYNAMIC_ALLOCATOR(char) >;
+            streambuf_type ascii_buf;
+
             // read the rest ascii reply
             if (address_type == ndi_address_type::serial) {
                 co_await async_read_until(*ndi_com_socket, ascii_buf, cr_ascii, use_awaitable);
@@ -392,6 +400,7 @@ namespace sophiar {
                 return async_write(*ndi_tcp_socket, buffer(cmd.data(), cmd.size()), use_awaitable);
             }
             assert(false);
+            __builtin_unreachable();
         }
 
         template<typename T, typename OutputIterator>
@@ -463,8 +472,20 @@ namespace sophiar {
             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));
 
+#ifdef BOOST_OS_WINDOWS_AVAILABLE // boost asio cannot send serial break under windows
+
+            using namespace std::chrono_literals;
+            auto handle = ndi_com_socket->native_handle();
+            ENSURE_CO(SetCommBreak(handle));
+            co_await coro_sleep(250ms); // hold serial break for 250ms
+            ENSURE_CO(ClearCommBreak(handle));
+
+#else // BOOST_OS_WINDOWS_AVAILABLE
+
             ndi_com_socket->send_break(); // cause the system to reset
 
+#endif // BOOST_OS_WINDOWS_AVAILABLE
+
             // wait the RESET to be sent from ndi
             auto reset_reply = co_await command_reply_queue->async_receive(use_awaitable);
             auto reset_reply_str = reset_reply->as_string_view();
@@ -493,6 +514,7 @@ namespace sophiar {
                 assert(false);
                 co_return 0;
             }
+            SPDLOG_INFO("NDI api version is {}.", reply->as_string_view());
             auto major_version = lexical_cast<int>(reply->data() + 2, 3); // Like G.001.004A0C0
             co_return major_version;
         }
@@ -737,6 +759,8 @@ namespace sophiar {
 
             ndi_api_major_version = co_await get_api_major_version();
 
+            // TODO increase Param.Tracking.IlluminatorRate to 60 for Polaris Spectra device
+
             CO_ENSURE(initialize_ndi())
             CO_ENSURE(config_tools())
 
@@ -751,7 +775,8 @@ namespace sophiar {
                 reply_callback_type callback = [this](dynamic_memory::pointer &&mem) {
                     handle_bx_reply(std::move(mem));
                 };
-                stream_reply_callback_pool.insert(stream_cmd, std::move(callback));
+                auto stream_cmd_id = stream_cmd.substr(0, stream_cmd.size() - 1); // strip the '\r'
+                stream_reply_callback_pool.insert(stream_cmd_id, std::move(callback));
                 CO_ENSURE(start_stream(stream_cmd))
             } else { // use interval streaming
                 assert(tracking_tools_worker == nullptr);
@@ -815,7 +840,7 @@ namespace sophiar {
         try {
             CO_ENSURE(pimpl->on_init_impl())
         } catch (std::exception &e) {
-            // TODO show log
+            SPDLOG_ERROR("Failed to initialize ndi: {}", e.what());
             co_return false;
         }
         CO_ENSURE(tristate_obj::on_init(config))

src/tracker/ndi/ndi_interface.h

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

src/utility/debug_utility.hpp

@@ -51,6 +51,12 @@
         if (!ok) return false; \
     }
 
+#define ENSURE_CO(func) \
+    { \
+        bool ok = (func); \
+        if (!ok) co_return false; \
+    }
+
 #define CO_ENSURE(func) \
     { \
         bool ok = co_await (func); \

src/utility/dynamic_pool.hpp

@@ -62,6 +62,11 @@ namespace sophiar {
 
     template<typename T>
     struct global_dynamic_allocator : public std::allocator<T> {
+        constexpr global_dynamic_allocator() noexcept = default;
+
+        template<typename U>
+        constexpr global_dynamic_allocator(const global_dynamic_allocator<U> &other) noexcept {}
+
         auto allocate(size_t n) {
             return (T *) ALLOCATE_DYNAMIC_MEMORY_N(sizeof(T), n);
         }

tests/CMakeLists.txt

@@ -41,6 +41,6 @@ add_executable(test_ndi
         ./interface/ndi.cpp
         ../src/tracker/ndi/ndi_interface.cpp
         ${CORE_IMPL_FILES})
-target_compile_definitions(test_ndi PUBLIC SOPHIAR_TEST)
+target_compile_definitions(test_ndi PUBLIC SOPHIAR_TEST SOPHIAR_TEST_NDI)
 #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

@@ -1,47 +1,54 @@
 {
-  "mode_list": [
+  "controller_port": 5277,
+  "variable_list": [
     {
-      "name": "test_mode"
+      "name": "probe_in_tracker",
+      "type": "transform_obj"
+    },
+    {
+      "name": "probe_uncertainty",
+      "type": "double_obj"
     }
   ],
   "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": [
-            {
-              "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"
-              }
+      "init_config": {
+        "address_type": "serial",
+        "ip": "169.254.132.51",
+        "tcp_port": 8765,
+        "com_port": "COM3",
+        "tool_list": [
+          {
+            "rom_path": "D:\\Program\\Robot\\Tools\\roms\\Probe_Small_4Ball.rom",
+            "outputs": {
+              "transform": "probe_in_tracker",
+              "marker_uncertainty": "probe_uncertainty"
             }
-          ]
-        }
-      ],
-      "start_configs": [
-        {
-          "allow_unreliable": true,
-          "prefer_stream_tracking": true
-        }
+          }
+        ]
+      },
+      "start_config": {
+        "allow_unreliable": true,
+        "prefer_stream_tracking": false
+      }
+    },
+    {
+      "type": "transform_obj_recorder",
+      "name": "debug_recorder",
+      "start_config": {
+        "variable_name": "probe_in_tracker",
+        "save_file": "probe.txt"
+      }
+    },
+    {
+      "type": "empty_object",
+      "name": "all",
+      "dependencies": [
+        "ndi",
+        "debug_recorder"
       ]
     }
-  ],
-  "connection_list": [
   ]
 }

tests/interface/ndi.cpp

@@ -1,7 +1,6 @@
 #define BOOST_TEST_DYN_LINK
 #define BOOST_TEST_MAIN  // in only one cpp file
 
-#include "tracker/ndi/ndi_interface.h"
 #include "utility/variable_helper.hpp"
 #include "core/basic_obj_types.hpp"
 
@@ -11,7 +10,10 @@
 #include <boost/asio/use_awaitable.hpp>
 #include <boost/test/unit_test.hpp>
 
+#include <spdlog/spdlog.h>
+
 #include <chrono>
+#include <fstream>
 #include <iostream>
 #include <vector>
 
@@ -23,49 +25,54 @@ 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;
-}
+//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();
+    spdlog::set_level(spdlog::level::trace);
+    std::ifstream config_file("data/ndi_interface_config.json");
+    assert(config_file.is_open());
+    auto config = nlohmann::json::parse(config_file);
+    auto ok = initialize(config);
+    assert(ok);
+    global_context->run();
 }