t/hand: Add async wrapper around sync helper

Co-authored-by: Jakob Bornecrantz <jakob@collabora.com>

src/xrt/CMakeLists.txt

@@ -1,8 +1,10 @@
-# Copyright 2019, Collabora, Ltd.
+# Copyright 2019-2022, Collabora, Ltd.
 # SPDX-License-Identifier: BSL-1.0
 
+# Order matters.
 add_subdirectory(include)
 add_subdirectory(auxiliary)
+add_subdirectory(tracking)
 add_subdirectory(drivers)
 add_subdirectory(compositor)
 add_subdirectory(state_trackers)

src/xrt/meson.build

@@ -5,6 +5,7 @@ xrt_include = include_directories('include')
 
 subdir('include')
 subdir('auxiliary')
+subdir('tracking')
 subdir('drivers')
 subdir('compositor')
 subdir('state_trackers')

src/xrt/tracking/CMakeLists.txt

@@ -0,0 +1,6 @@
+# Copyright 2022, Collabora, Ltd.
+# SPDX-License-Identifier: BSL-1.0
+
+if(XRT_BUILD_DRIVER_HANDTRACKING)
+	add_subdirectory(hand)
+endif()

src/xrt/tracking/hand/CMakeLists.txt

@@ -0,0 +1,19 @@
+# Copyright 2022, Collabora, Ltd.
+# SPDX-License-Identifier: BSL-1.0
+
+
+###
+# Async wrapper around sync helper.
+#
+add_library(
+	hand_async STATIC
+	t_hand_tracking_async.c
+	)
+target_link_libraries(
+	hand_async
+	PRIVATE
+		xrt-interfaces
+		aux_os
+		aux_util
+		aux_math
+	)

src/xrt/tracking/hand/meson.build

@@ -0,0 +1,15 @@
+# Copyright 2022, Collabora, Ltd.
+# SPDX-License-Identifier: BSL-1.0
+
+
+###
+# Async wrapper around sync helper.
+#
+lib_t_hand_async = static_library(
+	't_hand_async',
+	files(
+		't_hand_tracking_async.c'
+	),
+	include_directories: [xrt_include],
+	dependencies: [aux]
+)

src/xrt/tracking/hand/old_rgb/meson.build

@@ -0,0 +1,19 @@
+# Copyright 2022, Collabora, Ltd.
+# SPDX-License-Identifier: BSL-1.0
+
+
+# Old RGB hand tracking library.
+lib_t_ht_old_rgb = static_library(
+	't_ht_old_rgb',
+  files(
+    'rgb_hand_math.hpp',
+    'rgb_image_math.hpp',
+    'rgb_interface.h',
+    'rgb_model.hpp',
+    'rgb_nms.hpp',
+    'rgb_sync.cpp',
+    'rgb_sync.hpp',
+  ),
+  include_directories: [xrt_include, cjson_include],
+  dependencies: [aux, onnxruntime, opencv, eigen3]
+)

src/xrt/tracking/hand/t_hand_tracking_async.c

@@ -0,0 +1,185 @@
+// Copyright 2022, Collabora, Ltd.
+// SPDX-License-Identifier: BSL-1.0
+/*!
+ * @file
+ * @brief  Camera based hand tracking driver code.
+ * @author Moses Turner <moses@collabora.com>
+ * @ingroup drv_ht
+ */
+
+#include "tracking/t_hand_tracking.h"
+#include "util/u_misc.h"
+#include "util/u_trace_marker.h"
+#include "util/u_logging.h"
+#include "os/os_threading.h"
+
+
+//!@todo Definitely needs a destroy function, will leak a ton.
+
+struct ht_async_impl
+{
+	struct t_hand_tracking_async base;
+
+	struct t_hand_tracking_sync *provider;
+
+	struct xrt_frame *frames[2];
+
+	struct
+	{
+		struct xrt_hand_joint_set hands[2];
+		uint64_t timestamp;
+	} working;
+
+	struct
+	{
+		struct os_mutex mutex;
+		struct xrt_hand_joint_set hands[2];
+		uint64_t timestamp;
+	} present;
+
+	// in here:
+	// mutex is so that the mainloop and two push_frames don't fight over referencing frames;
+	// cond is so that we can wake up the mainloop at certain times;
+	// running is so we can stop the thread when Monado exits
+	struct os_thread_helper mainloop;
+
+	volatile bool hand_tracking_work_active;
+};
+
+static inline struct ht_async_impl *
+ht_async_impl(struct t_hand_tracking_async *base)
+{
+	return (struct ht_async_impl *)base;
+}
+
+static void *
+ht_async_mainloop(void *ptr)
+{
+	XRT_TRACE_MARKER();
+
+	struct ht_async_impl *hta = (struct ht_async_impl *)ptr;
+
+	os_thread_helper_lock(&hta->mainloop);
+
+	while (hta->mainloop.running) {
+
+		// No new frame, wait.
+		if (hta->frames[0] == NULL && hta->frames[1] == NULL) {
+			os_thread_helper_wait_locked(&hta->mainloop);
+		}
+
+		// In this case, we were woken up because Monado is exiting and we need to exit too
+		if (!hta->mainloop.running) {
+			break;
+		}
+
+		os_thread_helper_unlock(&hta->mainloop);
+
+		t_ht_sync_process(hta->provider, hta->frames[0], hta->frames[1], &hta->working.hands[0],
+		                  &hta->working.hands[1], &hta->working.timestamp);
+
+		xrt_frame_reference(&hta->frames[0], NULL);
+		xrt_frame_reference(&hta->frames[1], NULL);
+		os_mutex_lock(&hta->present.mutex);
+		hta->present.timestamp = hta->working.timestamp;
+		hta->present.hands[0] = hta->working.hands[0];
+		hta->present.hands[1] = hta->working.hands[1];
+		os_mutex_unlock(&hta->present.mutex);
+
+		hta->hand_tracking_work_active = false;
+
+		// Have to lock it again.
+		os_thread_helper_lock(&hta->mainloop);
+	}
+
+	os_thread_helper_unlock(&hta->mainloop);
+
+	return NULL;
+}
+
+static void
+ht_async_receive_left(struct xrt_frame_sink *sink, struct xrt_frame *frame)
+{
+	struct ht_async_impl *hta = ht_async_impl(container_of(sink, struct t_hand_tracking_async, left));
+	if (hta->hand_tracking_work_active) {
+		// Throw away this frame
+		return;
+	}
+	assert(hta->frames[0] == NULL);
+	xrt_frame_reference(&hta->frames[0], frame);
+}
+
+static void
+ht_async_receive_right(struct xrt_frame_sink *sink, struct xrt_frame *frame)
+{
+	struct ht_async_impl *hta = ht_async_impl(container_of(sink, struct t_hand_tracking_async, right));
+	if (hta->hand_tracking_work_active || hta->frames[0] == NULL) {
+		// Throw away this frame - either the hand tracking work is running now,
+		// or it was a very short time ago, and ht_async_receive_left threw away its frame
+		// or there's some other bug where left isn't pushed before right.
+		return;
+	}
+	assert(hta->frames[0] != NULL);
+	assert(hta->frames[1] == NULL);
+	xrt_frame_reference(&hta->frames[1], frame);
+	hta->hand_tracking_work_active = true;
+	// hta->working.timestamp = frame->timestamp;
+	os_thread_helper_lock(&hta->mainloop);
+	os_thread_helper_signal_locked(&hta->mainloop);
+	os_thread_helper_unlock(&hta->mainloop);
+}
+
+void
+ht_async_get_hand(struct t_hand_tracking_async *ht_async,
+                  enum xrt_input_name name,
+                  uint64_t desired_timestamp_ns,
+                  struct xrt_hand_joint_set *out_value,
+                  uint64_t *out_timestamp_ns)
+{
+	struct ht_async_impl *hta = ht_async_impl(ht_async);
+	assert(name == XRT_INPUT_GENERIC_HAND_TRACKING_LEFT || name == XRT_INPUT_GENERIC_HAND_TRACKING_RIGHT);
+
+	int idx = 0;
+	if (name == XRT_INPUT_GENERIC_HAND_TRACKING_RIGHT) {
+		idx = 1;
+	}
+	*out_value = hta->present.hands[idx];
+	*out_timestamp_ns = hta->present.timestamp;
+}
+
+void
+ht_async_break_apart(struct xrt_frame_node *node)
+{
+	struct ht_async_impl *hta = ht_async_impl(container_of(node, struct t_hand_tracking_async, node));
+	os_thread_helper_stop(&hta->mainloop);
+}
+
+void
+ht_async_destroy(struct xrt_frame_node *node)
+{
+	struct ht_async_impl *hta = ht_async_impl(container_of(node, struct t_hand_tracking_async, node));
+	os_thread_helper_destroy(&hta->mainloop);
+
+	t_ht_sync_destroy(&hta->provider);
+
+	free(hta);
+}
+
+struct t_hand_tracking_async *
+t_hand_tracking_async_default_create(struct xrt_frame_context *xfctx, struct t_hand_tracking_sync *sync)
+{
+	struct ht_async_impl *hta = U_TYPED_CALLOC(struct ht_async_impl);
+	hta->base.left.push_frame = ht_async_receive_left;
+	hta->base.right.push_frame = ht_async_receive_right;
+	hta->base.node.break_apart = ht_async_break_apart;
+	hta->base.node.destroy = ht_async_destroy;
+	hta->base.get_hand = ht_async_get_hand;
+
+	hta->provider = sync;
+
+	os_thread_helper_init(&hta->mainloop);
+	os_thread_helper_start(&hta->mainloop, ht_async_mainloop, hta);
+	xrt_frame_context_add(xfctx, &hta->base.node);
+
+	return &hta->base;
+}

src/xrt/tracking/meson.build

@@ -0,0 +1,6 @@
+# Copyright 2022, Collabora, Ltd.
+# SPDX-License-Identifier: BSL-1.0
+
+if 'handtracking' in drivers
+	subdir('hand')
+endif