From 012c86352e4eeaac3087b55c2d6ed10a59b73526 Mon Sep 17 00:00:00 2001 From: Moses Turner Date: Tue, 8 Mar 2022 13:30:37 -0600 Subject: [PATCH] t/hand: Add async wrapper around sync helper Co-authored-by: Jakob Bornecrantz --- src/xrt/CMakeLists.txt | 4 +- src/xrt/meson.build | 1 + src/xrt/tracking/CMakeLists.txt | 6 + src/xrt/tracking/hand/CMakeLists.txt | 19 ++ src/xrt/tracking/hand/meson.build | 15 ++ src/xrt/tracking/hand/old_rgb/meson.build | 19 ++ src/xrt/tracking/hand/t_hand_tracking_async.c | 185 ++++++++++++++++++ src/xrt/tracking/meson.build | 6 + 8 files changed, 254 insertions(+), 1 deletion(-) create mode 100644 src/xrt/tracking/CMakeLists.txt create mode 100644 src/xrt/tracking/hand/CMakeLists.txt create mode 100644 src/xrt/tracking/hand/meson.build create mode 100644 src/xrt/tracking/hand/old_rgb/meson.build create mode 100644 src/xrt/tracking/hand/t_hand_tracking_async.c create mode 100644 src/xrt/tracking/meson.build diff --git a/src/xrt/CMakeLists.txt b/src/xrt/CMakeLists.txt index 5b9addd7c..01b8d1f29 100644 --- a/src/xrt/CMakeLists.txt +++ b/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) diff --git a/src/xrt/meson.build b/src/xrt/meson.build index 0f1817777..cc0ee06fd 100644 --- a/src/xrt/meson.build +++ b/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') diff --git a/src/xrt/tracking/CMakeLists.txt b/src/xrt/tracking/CMakeLists.txt new file mode 100644 index 000000000..6c4ee8129 --- /dev/null +++ b/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() diff --git a/src/xrt/tracking/hand/CMakeLists.txt b/src/xrt/tracking/hand/CMakeLists.txt new file mode 100644 index 000000000..d5ab92d6a --- /dev/null +++ b/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 + ) diff --git a/src/xrt/tracking/hand/meson.build b/src/xrt/tracking/hand/meson.build new file mode 100644 index 000000000..f06119542 --- /dev/null +++ b/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] +) diff --git a/src/xrt/tracking/hand/old_rgb/meson.build b/src/xrt/tracking/hand/old_rgb/meson.build new file mode 100644 index 000000000..7a67a0d8a --- /dev/null +++ b/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] +) diff --git a/src/xrt/tracking/hand/t_hand_tracking_async.c b/src/xrt/tracking/hand/t_hand_tracking_async.c new file mode 100644 index 000000000..9f8c083c7 --- /dev/null +++ b/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 + * @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; +} diff --git a/src/xrt/tracking/meson.build b/src/xrt/tracking/meson.build new file mode 100644 index 000000000..0cdc68098 --- /dev/null +++ b/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