Implemented stray points detection from NDI.

CMakeLists.txt

@@ -11,6 +11,7 @@ add_executable(${PROJECT_NAME} src/main.cpp
         src/image_process/impl/process_funcs.cpp
         src/image_process/impl/versatile_convertor.cpp
         src/impl/main_impl.cpp
+        src/impl/app_base.cpp
         src/impl/apps/app_selector/app_selector.cpp
         src/impl/apps/debug/app_debug.cpp
         src/impl/apps/depth_guide/depth_guide.cpp
@@ -74,6 +75,7 @@ add_executable(${PROJECT_NAME} src/main.cpp
         src/render/impl/render_utility.cpp)
 
 add_subdirectory(src/core_v2)
+add_subdirectory(src/device_v5)
 add_subdirectory(src/image_process_v5)
 add_subdirectory(src/render_osg)
 add_subdirectory(src/module_v5)

data/config_remote_ar_v2.yaml

@@ -1,4 +1,8 @@
 app_name: remote_ar_v2
 
 left_camera_name: "LeftEye"
-right_camera_name: "RightEye"
+right_camera_name: "RightEye"
+
+ndi_ip: 192.168.1.202
+ndi_port: 8765
+sophiar_config: ./sophiar_empty.json

data/sophiar_empty.json

@@ -0,0 +1,4 @@
+{
+  "variable_list": [],
+  "object_list": []
+}

src/device_v5/CMakeLists.txt

@@ -0,0 +1,2 @@
+target_sources(${PROJECT_NAME} PRIVATE
+        ndi_stray_point_tracker.cpp)

src/device_v5/ndi_stray_point_tracker.cpp

@@ -0,0 +1,135 @@
+#include "ndi_stray_point_tracker.h"
+
+// from sophiar
+#include "utility/coro_worker.hpp"
+#include "utility/coro_worker_helper_func.hpp"
+
+#include <boost/asio/io_context.hpp>
+#include <boost/asio/ip/tcp.hpp>
+#include <boost/asio/steady_timer.hpp>
+#include <boost/asio/write.hpp>
+#include <boost/asio/read_until.hpp>
+#include <boost/asio/streambuf.hpp>
+#include <boost/asio/use_awaitable.hpp>
+#include <boost/asio/awaitable.hpp>
+#include <boost/asio/co_spawn.hpp>
+#include <boost/asio/detached.hpp>
+
+#include <shared_mutex>
+
+using namespace sophiar;
+
+namespace ba = boost::asio;
+using namespace ba::ip;
+using namespace std::placeholders;
+
+using boost::system::error_code;
+
+namespace {
+    constexpr auto track_interval = std::chrono::milliseconds(17); // 60Hz
+    constexpr const char track_command[] = "BX 1000\r";
+    constexpr auto ndi_endian = boost::endian::order::little;
+    constexpr uint16_t binary_header_magic = 0xA5C4;
+}
+
+struct ndi_stray_point_tracker::impl {
+    create_config conf;
+
+    std::optional<tcp::socket> socket;
+    coro_worker::pointer track_worker;
+
+    points_type last_points;
+    std::shared_mutex mu;
+
+    void establish_connection() {
+        socket.emplace(*global_context);
+        const auto ndi_ep = tcp::endpoint(make_address(conf.ip_addr), conf.port);
+        socket->async_connect(ndi_ep, [this](const error_code &ec) {
+            assert(!ec);
+            track_worker = make_interval_coro_worker(
+                track_interval, [this] { return track_once(); });
+            track_worker->run();
+        });
+    }
+
+    ba::awaitable<dynamic_memory::pointer> read_reply() {
+        // read header
+        auto header_buf = static_memory<6>{};
+        co_await async_fill_memory_from(*socket, header_buf);
+        auto header_reader = versatile_reader<ndi_endian>(header_buf);
+        uint16_t header_magic, reply_length, header_crc16;
+        header_reader >> header_magic >> reply_length >> header_crc16;
+        assert(header_magic == binary_header_magic);
+
+        // TODO: check header crc
+
+        // read reply content
+        auto reply_buf = dynamic_memory::new_instance(reply_length + 2);
+        co_await async_fill_memory_from(*socket, *reply_buf);
+
+        // TODO: check content crc
+
+        reply_buf->increase_size(-2); // strip the crc
+        co_return std::move(reply_buf);
+    }
+
+    ba::awaitable<bool> track_once() {
+        // send command
+        co_await async_write(*socket, ba::buffer(track_command), ba::use_awaitable);
+        // read reply
+        const auto reply = co_await read_reply();
+
+        // parse reply
+        auto reader = versatile_reader<ndi_endian>(*reply);
+        const auto num_handle = reader.read_value<uint8_t>();
+        reader.manual_offset(2 * sizeof(uint8_t) * num_handle); // skip the tool data
+
+        const auto num_points = reader.read_value<uint8_t>();
+        const auto ov_size = (num_points + 8) / 8;
+        reader.manual_offset(ov_size * sizeof(uint8_t)); // TODO: check point status, P56
+        if (true) {
+            auto lock = std::unique_lock(mu);
+            last_points.resize(num_points);
+            for (auto &point: last_points) {
+                point.out_of_volume = false;
+                reader >> point.pos.x >> point.pos.y >> point.pos.z;
+            }
+        }
+        co_return true;
+    }
+
+    explicit impl(create_config _conf)
+        : conf(std::move(_conf)) {
+        establish_connection();
+    }
+};
+
+ndi_stray_point_tracker::ndi_stray_point_tracker(const create_config &conf)
+    : pimpl(std::make_shared<impl>(conf)) {
+}
+
+ndi_stray_point_tracker::~ndi_stray_point_tracker() {
+    co_spawn(*global_context, [pimpl = pimpl] -> ba::awaitable<void> {
+        SAFE_RESET_WORKER(pimpl->track_worker);
+        co_return;
+    }, ba::detached);
+}
+
+ndi_stray_point_tracker::points_type
+ndi_stray_point_tracker::get_stray_points() const {
+    auto lock = std::shared_lock(pimpl->mu);
+    return pimpl->last_points;
+}
+
+namespace {
+    std::optional<ndi_stray_point_tracker> tracker;
+}
+
+void create_ndi_stray_points_tracker(
+    const ndi_stray_point_tracker::create_config &conf) {
+    tracker.emplace(conf);
+}
+
+ndi_stray_point_tracker::points_type get_ndi_stray_points() {
+    return tracker->get_stray_points();
+}

src/device_v5/ndi_stray_point_tracker.h

@@ -0,0 +1,37 @@
+#ifndef TRACKER_NDI_H
+#define TRACKER_NDI_H
+
+#include <glm/vec3.hpp>
+
+#include <memory>
+#include <vector>
+
+class ndi_stray_point_tracker {
+public:
+    struct create_config {
+        std::string ip_addr;
+        uint16_t port;
+    };
+
+    explicit ndi_stray_point_tracker(const create_config &conf);
+
+    ~ndi_stray_point_tracker();
+
+    struct point_info {
+        glm::vec3 pos;
+        bool out_of_volume;
+    };
+
+    using points_type = std::vector<point_info>;
+
+    [[nodiscard]] points_type get_stray_points() const;
+
+private:
+    struct impl;
+    std::shared_ptr<impl> pimpl;
+};
+
+void create_ndi_stray_points_tracker(const ndi_stray_point_tracker::create_config& conf);
+auto get_ndi_stray_points() -> ndi_stray_point_tracker::points_type;
+
+#endif //TRACKER_NDI_H

src/impl/app_base.cpp

@@ -0,0 +1,22 @@
+#include "app_base.h"
+
+// from sophiar
+#include "core/local_connection.h"
+#include "core/global_defs.h"
+
+using namespace sophiar;
+
+extern local_connection *g_sophiar_conn;
+
+void app_base::start_sophiar(const std::string &conf_path) {
+    sophiar_thread.emplace([=] {
+        run_sophiar(conf_path);
+    });
+    g_sophiar_conn = new local_connection();
+}
+
+void app_base::wait_sophiar() {
+    while (global_context == nullptr) {
+        std::this_thread::yield();
+    }
+}

src/impl/app_base.h

@@ -9,7 +9,6 @@
 
 class app_base {
 public:
-
     using io_context = boost::asio::io_context;
 
     struct create_config {
@@ -26,6 +25,12 @@ public:
 
     virtual void render_background() = 0;
 
+protected:
+    std::optional<std::jthread> sophiar_thread;
+
+    void start_sophiar(const std::string &conf_path);
+
+    static void wait_sophiar(); // block until sophiar is started
 };
 
 #endif //DEPTHGUIDE_APP_BASE_H

src/impl/apps/app_selector/app_selector.cpp

@@ -23,7 +23,7 @@ app_selector::app_selector(const create_config &_conf) {
     dialog_conf.flags |= ImGuiFileDialogFlags_HideColumnType;
     dialog_conf.flags |= ImGuiFileDialogFlags_ReadOnlyFileNameField;
     dialog_conf.flags |= ImGuiFileDialogFlags_CaseInsensitiveExtention;
-    dialog_conf.path = "/home/tpx/ext/project/DepthGuide/data"; // TODO: remember last value
+    dialog_conf.path = "."; // TODO: remember last value
     dialog->OpenDialog(dialog_name, "Choose YAML file",
                        "YAML files{.yaml,.yml}", dialog_conf);
 }

src/impl/apps/remote_ar/remote_ar_v2.cpp

@@ -4,6 +4,10 @@
 #include "image_process_v5/image_process.h"
 #include "core/yaml_utility.hpp"
 #include "core/imgui_utility.hpp"
+#include "device_v5/ndi_stray_point_tracker.h"
+
+// from sophiar
+#include "core/local_connection.h"
 
 app_remote_ar_v2::app_remote_ar_v2(create_config _conf)
     : main_conf(std::move(_conf)) {
@@ -62,6 +66,16 @@ app_remote_ar_v2::app_remote_ar_v2(create_config _conf)
         sub_conf.items.emplace_back(uvc_img_id, "Endoscope", true);
         saver.emplace(sub_conf);
     }
+
+    start_sophiar(LOAD_STR("sophiar_config"));
+    wait_sophiar();
+
+    if (true) {
+        auto sub_conf = ndi_stray_point_tracker::create_config();
+        sub_conf.ip_addr = LOAD_STR("ndi_ip");
+        sub_conf.port = LOAD_NUMBER(uint16_t, "ndi_port");
+        create_ndi_stray_points_tracker(sub_conf);
+    }
 }
 
 app_remote_ar_v2::~app_remote_ar_v2() = default;

src/impl/main_impl.cpp

@@ -4,6 +4,9 @@
 #include "core_v2/memory_manager.h"
 #include "core_v2/utility.hpp"
 
+// from sophiar
+#include "core/local_connection.h"
+
 #include <boost/asio/io_context.hpp>
 #include <boost/asio/post.hpp>
 #include <boost/asio/steady_timer.hpp>
@@ -27,6 +30,7 @@ GLFWwindow *window = nullptr;
 smart_cuda_stream *default_cuda_stream = nullptr;
 io_context *main_ctx;
 BS::thread_pool *g_thread_pool;
+sophiar::local_connection *g_sophiar_conn = nullptr;
 
 using cleanup_list_type =
 std::vector<cleanup_func_type>;
@@ -321,4 +325,5 @@ void cleanup() {
     delete main_ob;
     delete main_ctx;
     delete g_memory_manager;
+    delete g_sophiar_conn;
 }