Implemented NDI aurora interface.

CMakeLists.txt

@@ -11,5 +11,5 @@ target_include_directories(${PROJECT_NAME} PRIVATE ./src)
 
 target_link_libraries(${PROJECT_NAME} Sophiar2Lib)
 
-add_subdirectory(app)
+#add_subdirectory(app)
 #add_subdirectory(tests)

src/sensor/ndi/ndi_interface.cpp

@@ -23,6 +23,7 @@
 #include <boost/asio/streambuf.hpp>
 #include <boost/asio/use_awaitable.hpp>
 #include <boost/crc.hpp>
+#include <boost/integer.hpp>
 #include <boost/iostreams/device/mapped_file.hpp>
 #include <boost/lexical_cast.hpp>
 #include <boost/smart_ptr/scoped_ptr.hpp>
@@ -92,7 +93,13 @@ namespace sophiar {
             variable_index_type transform_var_index = invalid_variable_index;
             variable_index_type rms_var_index = invalid_variable_index;
             uint32_t last_frame_number = 0;
+
+            // for polaris
             std::string rom_path;
+
+            // for aurora
+            std::string serial_number;
+            uint8_t physical_port;
         };
 
         using tool_info_pool_type = std::vector<tool_info>;
@@ -132,6 +139,12 @@ namespace sophiar {
         std::string com_port_name;
         scoped_ptr<serial_port> ndi_com_socket;
 
+        enum ndi_device_type_enum {
+            NDI_Polaris,
+            NDI_Aurora
+        };
+        ndi_device_type_enum ndi_device_type;
+
         int ndi_api_major_version = 0;
         bool accept_unreliable_transform = true;
         bool prefer_stream_tracking = true;
@@ -158,7 +171,6 @@ namespace sophiar {
             assert(tool_info_pool.empty());
             ENSURE_ARRAY("tool_list")
             for (auto &tool_config: config["tool_list"]) {
-                auto rom_path = LOAD_STRING_ITEM2(tool_config, "rom_path");
                 assert(tool_config.contains("outputs"));
                 auto &output_config = tool_config["outputs"];
                 auto transform_var_index = TRY_LOAD_VARIABLE_INDEX2(output_config, transform_obj, "transform");
@@ -168,7 +180,9 @@ namespace sophiar {
                                 .port_handle = invalid_port_handle,
                                 .transform_var_index = transform_var_index,
                                 .rms_var_index = rms_var_index,
-                                .rom_path = std::move(rom_path)
+                                .rom_path = TRY_LOAD_STRING_ITEM2(tool_config, "rom_path", ""),
+                                .serial_number = TRY_LOAD_STRING_ITEM2(tool_config, "serial_number", ""),
+                                .physical_port = (uint8_t) TRY_LOAD_UINT_ITEM2(tool_config, "physical_port", 0),
                         });
             }
         }
@@ -441,16 +455,27 @@ namespace sophiar {
             return 0; // no error
         }
 
+        static void log_cmd_result(std::string_view cmd, int ret_code) {
+            if (ret_code == 0) [[likely]] return;
+            cmd.remove_suffix(1); // remove tailing '\r'
+            if (ret_code < 0) {
+                SPDLOG_ERROR("Command {} failed with code {:x}", cmd, -ret_code);
+            } else {
+                assert(ret_code > 0);
+                SPDLOG_WARN("Command {} has warning with code {:x}", cmd, ret_code);
+            }
+        }
+
         awaitable<bool> simple_cmd_helper(std::string_view cmd) {
             co_await send_command(cmd);
             auto reply = co_await command_reply_queue->async_receive(use_awaitable);
             auto reply_str = reply->as_string_view();
             auto reply_code = get_error_warning_code(reply_str);
+            log_cmd_result(cmd, reply_code);
             if (reply_code < 0) { // error
-                // TODO show error
                 co_return false;
             } else if (reply_code > 0) { // warning
-                // TODO show warning
+
             } else { // normal
                 assert(reply_str == "OKAY");
             }
@@ -477,11 +502,13 @@ namespace sophiar {
             co_await coro_sleep(250ms); // hold serial break for 250ms
             ENSURE_CO(ClearCommBreak(handle));
 
-#else // BOOST_OS_WINDOWS_AVAILABLE
+#endif // BOOST_OS_WINDOWS_AVAILABLE
+
+#ifdef BOOST_OS_UNIX_AVAILABLE
 
             ndi_com_socket->send_break(); // cause the system to reset
 
-#endif // BOOST_OS_WINDOWS_AVAILABLE
+#endif // BOOST_OS_UNIX_AVAILABLE
 
             // wait the RESET to be sent from ndi
             auto reset_reply = co_await command_reply_queue->async_receive(use_awaitable);
@@ -491,29 +518,51 @@ namespace sophiar {
                 // TODO show log
                 co_return false;
             }
+
+            while (*reset_reply_str.begin() == '\0') { // TODO: figure out why there are so many heading '\0's
+                reset_reply_str.remove_prefix(1);
+            }
+
             assert(reset_reply_str == "RESET");
 
             // change the serial config of NDI
-            CO_ENSURE(simple_cmd_helper("COMM 70001\r"sv))
+            CO_ENSURE(simple_cmd_helper("COMM 60001\r"sv))
 
             // load advanced config
-            ndi_com_socket->set_option(serial_port::baud_rate(19200));
+            ndi_com_socket->set_option(serial_port::baud_rate(921600));
             ndi_com_socket->set_option(serial_port::flow_control(serial_port::flow_control::hardware));
 
             co_return true;
         }
 
-        awaitable<int> get_api_major_version() {
+        // query NDI device type and api major version
+        awaitable<bool> query_api_info() {
             co_await send_command("APIREV \r"sv);
             auto reply = co_await command_reply_queue->async_receive(use_awaitable);
             auto reply_code = get_error_warning_code(reply->as_string_view());
             if (reply_code < 0) {
-                assert(false);
-                co_return 0;
+                co_return false;
             }
             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;
+
+            auto device_type_char = reply->data()[0];
+            switch (device_type_char) {
+                case 'G': {
+                    ndi_device_type = NDI_Polaris;
+                    break;
+                }
+                case 'D': {
+                    ndi_device_type = NDI_Aurora;
+                    break;
+                }
+                default: {
+                    SPDLOG_ERROR("Unknown NDI device type {}.", device_type_char);
+                    co_return false;
+                }
+            }
+
+            ndi_api_major_version = lexical_cast<int>(reply->data() + 2, 3); // Like G.001.004A0C0
+            co_return true;
         }
 
         awaitable<bool> initialize_ndi() {
@@ -594,40 +643,147 @@ namespace sophiar {
             co_return true;
         }
 
-        awaitable<bool> initialize_port_handle(port_handle_type port_handle) {
-            static constexpr auto pinit_cmd_template = "PINIT 00\r"sv;
-            static constexpr auto pinit_cmd_length = pinit_cmd_template.size();
-            static constexpr ptrdiff_t port_handle_offset = 6;
-            auto pinit_cmd_buf = static_memory<pinit_cmd_length>();
-
-            // fill the template
+        template<string_literal Tmp, ptrdiff_t Offset>
+        auto fill_port_handle_cmd_template(port_handle_type port_handle) {
             assert(is_valid_id(port_handle));
-            pinit_cmd_template.copy(pinit_cmd_buf.data(), pinit_cmd_length);
-            write_number_as_hex(port_handle, pinit_cmd_buf.data() + port_handle_offset);
+            auto cmd_buf = static_memory<Tmp.size>();
+            std::copy_n(Tmp.value, Tmp.size, cmd_buf.data());
+            write_number_as_hex(port_handle, cmd_buf.data() + Offset);
+            return cmd_buf;
+        }
+
+        struct phsr_item_type {
+            port_handle_type port_handle;
+            uint16_t status; // 12-bits
+        };
+        using phsr_ret_type = std::vector<phsr_item_type>;
+
+        awaitable<phsr_ret_type> query_port_handles(uint8_t opt) {
+            auto cmd_buf = fill_port_handle_cmd_template<"PHSR 00\r", 5>(opt);
+            co_await send_command(cmd_buf.as_string_view());
 
-            auto ok = co_await simple_cmd_helper(pinit_cmd_buf.as_string_view());
+            auto reply = co_await command_reply_queue->async_receive(use_awaitable);
+            auto reply_code = get_error_warning_code(reply->as_string_view());
+            log_cmd_result(cmd_buf.as_string_view(), reply_code);
+            if (reply_code < 0) {
+                co_return phsr_ret_type();
+            }
+
+            auto ptr = reply->data();
+            auto num_handle = read_number_as_hex<uint8_t>(ptr);
+            ptr += 2;
+
+            phsr_ret_type ret;
+            for (auto k = 0; k < num_handle; ++k) {
+                auto port_handle = read_number_as_hex<uint8_t>(ptr);
+                ptr += 2;
+//                auto status = read_number_as_hex_n<3>(ptr);
+                auto status = 0; // TODO: re-implement read hex number
+                ptr += 3;
+                ret.emplace_back(port_handle, status);
+            }
+            co_return ret;
+        }
+
+        // Tmp: command template
+        // Offset: position of port handle
+        template<string_literal Tmp, ptrdiff_t Offset>
+        awaitable<bool> simple_port_handle_cmd_helper(port_handle_type port_handle) {
+            auto cmd_buf = fill_port_handle_cmd_template<Tmp, Offset>(port_handle);
+            auto ok = co_await simple_cmd_helper(cmd_buf.as_string_view());
             co_return ok;
         }
 
+        awaitable<bool> initialize_port_handle(port_handle_type port_handle) {
+            return simple_port_handle_cmd_helper<"PINIT 00\r", 6>(port_handle);
+        }
+
         awaitable<bool> enable_port_handle(port_handle_type port_handle) {
-            static constexpr auto pena_cmd_template = "PENA 00D\r"sv;
-            static constexpr auto pena_cmd_length = pena_cmd_template.size();
-            static constexpr ptrdiff_t port_handle_offset = 5;
-            auto pena_cmd_buf = static_memory<pena_cmd_length>();
+            return simple_port_handle_cmd_helper<"PENA 00D\r", 5>(port_handle);
+        }
 
-            // fill the template
-            assert(is_valid_id(port_handle));
-            pena_cmd_template.copy(pena_cmd_buf.data(), pena_cmd_length);
-            write_number_as_hex(port_handle, pena_cmd_buf.data() + port_handle_offset);
+        awaitable<bool> release_port_handle(port_handle_type port_handle) {
+            return simple_port_handle_cmd_helper<"PHF 00\r", 4>(port_handle);
+        }
 
-            auto ok = co_await simple_cmd_helper(pena_cmd_buf.as_string_view());
-            co_return ok;
+        awaitable<bool> disable_port_handle(port_handle_type port_handle) {
+            return simple_port_handle_cmd_helper<"PDIS 00\r", 5>(port_handle);
         }
 
-        awaitable<bool> config_tools() {
+        awaitable<bool> release_free_port_handles() {
+            auto list = co_await query_port_handles(1); // 01, port handles needed to be freed
+            for (auto item: list) {
+                CO_ENSURE(release_port_handle(item.port_handle));
+            }
+            co_return true;
+        }
+
+        struct phinf_ret_type {
+            std::string serial_number;
+            uint8_t physical_port;
+        };
+
+        awaitable<phinf_ret_type> query_port_handle_info(port_handle_type port_handle) {
+            // 0001, for serial number
+            // 0020, for physical port
+            auto cmd_buf = fill_port_handle_cmd_template<"PHINF 000021\r", 6>(port_handle);
+            co_await send_command(cmd_buf.as_string_view());
+
+            auto reply = co_await command_reply_queue->async_receive(use_awaitable);
+            auto reply_code = get_error_warning_code(reply->as_string_view());
+            log_cmd_result(cmd_buf.as_string_view(), reply_code);
+            if (reply_code < 0) {
+                co_return phinf_ret_type();
+            }
+
+            auto ptr = reply->data();
+            auto ret = phinf_ret_type();
+
+            static constexpr auto serial_offset = 8 + 12 + 3;
+            static constexpr auto serial_length = 8;
+            ret.serial_number = std::string(ptr + serial_offset, serial_length);
+
+            static constexpr auto physical_offset = serial_offset + serial_length
+                                                    + 2 + 8 + 2;
+            ret.physical_port = read_number_as_hex<uint8_t>(ptr + physical_offset);
+
+            co_return ret;
+        }
+
+        tool_info_index_type get_tool_info_index(const std::string &serial_number,
+                                                 uint8_t physical_port) {
+            auto begin_iter = tool_info_pool.begin();
+            auto end_iter = tool_info_pool.end();
+            auto iter = tool_info_pool_type::iterator();
+
+            // try search with serial number first
+            assert(!serial_number.empty());
+            iter = std::find_if(begin_iter, end_iter,
+                                [=](auto &item) { return item.serial_number == serial_number; });
+            if (iter != end_iter) {
+                return std::distance(begin_iter, iter);
+            }
+
+            // try search with physical port
+            iter = std::find_if(begin_iter, end_iter,
+                                [=](auto &item) { return item.physical_port == physical_port; });
+            if (iter != end_iter) {
+                return std::distance(begin_iter, iter);
+            }
+
+            SPDLOG_WARN("Tool at port {} with serial {} has not been registered, and will not be tracked.",
+                        physical_port, serial_number);
+            return 0xFF;
+        }
+
+        awaitable<bool> config_tools_polaris() {
             assert(port_handle_map.empty());
             for (size_t index = 0; index < tool_info_pool.size(); ++index) {
                 auto &info = tool_info_pool[index];
+
+                // Aurora tools do not have rom files
+                if (info.rom_path.empty()) continue;
+
                 auto port_handle = co_await request_port_handle();
                 if (!is_valid_id(port_handle)) {
                     // TODO show log
@@ -645,6 +801,27 @@ namespace sophiar {
             co_return true;
         }
 
+        awaitable<bool> config_tools_aurora() {
+            auto list = co_await query_port_handles(2); // 02, port handles not initialized
+            for (auto item: list) {
+                auto port_handle = item.port_handle;
+                CO_ENSURE(initialize_port_handle(port_handle))
+                CO_ENSURE(enable_port_handle(port_handle))
+
+                auto port_info = co_await query_port_handle_info(port_handle);
+                assert(!port_info.serial_number.empty());
+
+                auto port_index = get_tool_info_index(port_info.serial_number,
+                                                      port_info.physical_port);
+                if (port_index == 0xFF) {
+                    CO_ENSURE(disable_port_handle(port_handle))
+                } else {
+                    port_handle_map[port_handle] = port_index;
+                }
+            }
+            co_return true;
+        }
+
         awaitable<bool> start_ndi_tracking() {
             return simple_cmd_helper("TSTART 80\r"sv);
         }
@@ -659,14 +836,18 @@ namespace sophiar {
             auto ts = current_timestamp();
             for (uint8_t i = 0; i < num_handle; ++i) {
                 auto port_handle = reader.read_value<port_handle_type>();
+                auto handle_status = reader.read_value<uint8_t>();
+                if (handle_status == 0x04) { // disabled
+                    continue;
+                }
+
                 assert(port_handle_map.contains(port_handle));
                 auto tool_index = port_handle_map[port_handle];
                 assert(tool_index < tool_info_pool.size());
                 auto &info = tool_info_pool[tool_index];
 
-                auto handle_status = reader.read_value<uint8_t>();
                 uint32_t port_status, frame_number;
-                if (handle_status != 0x01) { // not invalid
+                if (handle_status == 0x02) { // is missing
                     reader >> port_status >> frame_number;
                     try_update_variable<transform_obj>(info.transform_var_index, nullptr, ts);
                     try_update_variable<double_obj>(info.rms_var_index, nullptr, ts);
@@ -754,12 +935,25 @@ namespace sophiar {
                 CO_ENSURE(setup_com_socket())
             }
 
-            ndi_api_major_version = co_await get_api_major_version();
+            // query device info
+            co_await query_api_info();
 
             // TODO increase Param.Tracking.IlluminatorRate to 60 for Polaris Spectra device
 
             CO_ENSURE(initialize_ndi())
-            CO_ENSURE(config_tools())
+            CO_ENSURE(release_free_port_handles()); // required by document
+
+            // setup tools based on device type
+            switch (ndi_device_type) {
+                case NDI_Polaris: {
+                    CO_ENSURE(config_tools_polaris());
+                    break;
+                }
+                case NDI_Aurora: {
+                    CO_ENSURE(config_tools_aurora());
+                    break;
+                }
+            }
 
             co_return true;
         }

src/utility/config_utility.hpp

@@ -84,6 +84,19 @@ inline auto TRY_LOAD_BOOL_ITEM2(const nlohmann::json &config,
 #define TRY_LOAD_BOOL_ITEM(var_name, default_val) \
     TRY_LOAD_BOOL_ITEM2(config, var_name, default_val)
 
+inline auto TRY_LOAD_UINT_ITEM2(const nlohmann::json &config,
+                                const std::string &var_name, uint64_t default_val) {
+    if (config.contains(var_name)) {
+        assert(config[var_name].is_number_integer());
+        return config[var_name].get<uint64_t>();
+    } else {
+        return default_val;
+    }
+}
+
+#define TRY_LOAD_UINT_ITEM(var_name, default_val) \
+    TRY_LOAD_UINT_ITEM2(config, var_name, default_val)
+
 inline auto TRY_LOAD_FLOAT_ITEM2(const nlohmann::json &config,
                                  const std::string &var_name, double default_val) {
     if (config.contains(var_name)) {
@@ -97,6 +110,19 @@ inline auto TRY_LOAD_FLOAT_ITEM2(const nlohmann::json &config,
 #define TRY_LOAD_FLOAT_ITEM(var_name, default_val) \
     TRY_LOAD_FLOAT_ITEM2(config, var_name, default_val)
 
+inline auto TRY_LOAD_STRING_ITEM2(const nlohmann::json &config,
+                                 const std::string &var_name, const std::string &default_val) {
+    if (config.contains(var_name)) {
+        assert(config[var_name].is_string());
+        return config[var_name].get<std::string>();
+    } else {
+        return default_val;
+    }
+}
+
+#define TRY_LOAD_STRING_ITEM(var_name, default_val) \
+    TRY_LOAD_STRING_ITEM2(config, var_name, default_val)
+
 template<typename SmallObjType>
 inline sophiar::variable_index_type
 TRY_LOAD_VARIABLE_INDEX2_IMPL(const nlohmann::json &config,

src/utility/versatile_buffer2.hpp

@@ -21,6 +21,16 @@
 
 namespace sophiar {
 
+    template<size_t N>
+    struct string_literal {
+        constexpr string_literal(const char (&str)[N]) {
+            std::copy_n(str, size, value);
+        }
+
+        static constexpr auto size = N - 1; // strip the tailing '\0'
+        char value[size];
+    };
+
     template<size_t Length>
     struct static_memory {
 

tests/data/ndi_interface_config.json

@@ -5,6 +5,10 @@
       "name": "probe_in_tracker",
       "type": "transform_obj"
     },
+    {
+      "name": "ref_in_tracker",
+      "type": "transform_obj"
+    },
     {
       "name": "probe_uncertainty",
       "type": "double_obj"
@@ -15,17 +19,31 @@
       "type": "ndi_interface",
       "name": "ndi",
       "init_config": {
-        "address_type": "serial",
-        "ip": "169.254.132.51",
+        "address_type": "ethernet",
+        "ip": "10.0.0.5",
         "tcp_port": 8765,
-        "com_port": "COM3",
+        "com_port": "/dev/ttyUSB0",
         "tool_list": [
           {
-            "rom_path": "D:\\Program\\Robot\\Tools\\roms\\Probe_Small_4Ball.rom",
+            "rom_path": "/home/tpx/data/roms/Glass_4Ball_2.rom",
             "outputs": {
               "transform": "probe_in_tracker",
               "marker_uncertainty": "probe_uncertainty"
             }
+          },
+          {
+            "serial_number": "3DD50000",
+            "physical_port": 4,
+            "outputs": {
+              "transform": "probe_in_tracker",
+              "marker_uncertainty": "probe_uncertainty"
+            }
+          },
+          {
+            "physical_port": 1,
+            "outputs": {
+              "transform": "ref_in_tracker"
+            }
           }
         ]
       },
@@ -35,11 +53,10 @@
       }
     },
     {
-      "type": "transform_obj_recorder",
-      "name": "debug_recorder",
+      "type": "transform_obj_watcher",
+      "name": "debug_watcher",
       "start_config": {
-        "variable_name": "probe_in_tracker",
-        "save_file": "probe.txt"
+        "variable_name": "probe_in_tracker"
       }
     },
     {
@@ -47,7 +64,7 @@
       "name": "all",
       "dependencies": [
         "ndi",
-        "debug_recorder"
+        "debug_watcher"
       ]
     }
   ]